116 lines
3.0 KiB
C++
116 lines
3.0 KiB
C++
|
|
#include <zephyr/device.h>
|
|
#include <zephyr/devicetree.h>
|
|
#include <zephyr/drivers/uart.h>
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/sys/printk.h>
|
|
|
|
#include "rotary_encoder_driver.h"
|
|
#include "motor_driver.h"
|
|
#include "pi_controller.h"
|
|
#include "p_controller.h"
|
|
#include "motor_controller.h"
|
|
|
|
#include "initialization_state.h"
|
|
|
|
#include "pre_operational_state.h"
|
|
|
|
#include "operational_state.h"
|
|
#include "stopped_state.h"
|
|
|
|
static RotaryEncoderDriver encoder;
|
|
|
|
static MotorDriver motor;
|
|
|
|
const double Kp_P = 10.0; // P-controller from Project 2
|
|
|
|
const double Kp = 10.125; // Ziegler-Nichols PI
|
|
const double Ti = 0.0334;
|
|
const double T = 0.004;
|
|
|
|
static P_controller p_controller(Kp_P);
|
|
static PI_controller pi_controller(Kp, Ti, T);
|
|
|
|
|
|
static InitializationState initialization_state;
|
|
// Pre-operational velur á milli stýringanna með 'c'
|
|
static PreOperationalState pre_operational_state(p_controller, pi_controller);
|
|
static OperationalState operational_state;
|
|
static StoppedState stopped_state;
|
|
|
|
// Stöðurnar verða að vera skilgreindar á undan motor_controller
|
|
static MotorController motor_controller(
|
|
encoder,
|
|
motor,
|
|
pi_controller, // Sjálfgefin stýring
|
|
initialization_state,
|
|
pre_operational_state,
|
|
operational_state,
|
|
stopped_state
|
|
);
|
|
|
|
|
|
static const struct device *const console_dev =
|
|
DEVICE_DT_GET(DT_CHOSEN(zephyr_console));
|
|
|
|
// Received characters are queued by the UART ISR, like the RX buffer that
|
|
// Arduino's Serial had. The USART has a single receive register, so
|
|
// polling it from the main loop loses characters that arrive in a burst.
|
|
K_MSGQ_DEFINE(rx_queue, sizeof(char), 64, 1);
|
|
|
|
static void uart_rx_isr(const struct device *dev, void *user_data)
|
|
{
|
|
(void)user_data;
|
|
|
|
if (!uart_irq_update(dev)) {
|
|
return;
|
|
}
|
|
|
|
while (uart_irq_rx_ready(dev)) {
|
|
unsigned char c;
|
|
|
|
if (uart_fifo_read(dev, &c, 1) != 1) {
|
|
break;
|
|
}
|
|
|
|
// Drop the character if the queue is full.
|
|
(void)k_msgq_put(&rx_queue, &c, K_NO_WAIT);
|
|
}
|
|
}
|
|
|
|
extern "C" void motor_application_main(void)
|
|
{
|
|
if (!device_is_ready(console_dev)) {
|
|
printk("Console UART is not ready\n");
|
|
return;
|
|
}
|
|
|
|
uart_irq_callback_user_data_set(console_dev, uart_rx_isr, nullptr);
|
|
uart_irq_rx_enable(console_dev);
|
|
|
|
const int encoder_result = encoder.begin();
|
|
if (encoder_result < 0) {
|
|
printk("Encoder setup failed: %d\n", encoder_result);
|
|
return;
|
|
}
|
|
|
|
motor.begin();
|
|
motor_controller.begin();
|
|
|
|
while (true) {
|
|
char received_char;
|
|
|
|
// Handle every character the ISR has queued since the last pass.
|
|
while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) {
|
|
motor_controller.command(received_char);
|
|
}
|
|
|
|
motor_controller.update();
|
|
|
|
// Sleep until the encoder has a new speed sample (every 4 ms), so
|
|
// the controller runs right after sampling. The timeout only
|
|
// matters if the timer stops; the thread is idle while it waits.
|
|
encoder.wait_for_sample(K_MSEC(10));
|
|
}
|
|
}
|