diff --git a/.gitignore b/.gitignore index 89cc49c..2ab078b 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch +scripts +__pycache__/ +*.typ \ No newline at end of file diff --git a/README.md b/README.md index e560b96..4096010 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,3 @@ # ESP_Project4_H26 -A repository for project 3, done in the Embedded System Programming class in Reykjavík University in the Fall semester of 2026 \ No newline at end of file +A repository for project 4, done in the Embedded System Programming class in Reykjavík University in the Fall semester of 2026 \ No newline at end of file diff --git a/include/digital_in.h b/include/digital_in.h index 9ac64f5..5c3715d 100644 --- a/include/digital_in.h +++ b/include/digital_in.h @@ -4,19 +4,28 @@ #include #include -struct Digital_in { +class Digital_in { +public: enum : uint8_t { ENCODER_C1 = 0, ENCODER_C2 = 1 }; - const struct gpio_dt_spec *gpio; - explicit Digital_in(uint8_t pin_id); void init(); bool is_hi(); bool is_lo(); + + // Calls handler on every rising edge of the pin. The caller owns the + // gpio_callback struct so it can find its own object with CONTAINER_OF. + // Returns 0 on success, or a negative Zephyr error code. + int enable_rising_edge_interrupt( + struct gpio_callback *callback, + gpio_callback_handler_t handler); + +private: + const struct gpio_dt_spec *gpio; }; -#endif \ No newline at end of file +#endif diff --git a/include/digital_out.h b/include/digital_out.h index 4b08482..7a0a5da 100644 --- a/include/digital_out.h +++ b/include/digital_out.h @@ -4,10 +4,7 @@ #include #include - struct Digital_out { - // uint8_t pin_mask; - enum : uint8_t { STATUS_LED = 0 }; @@ -21,7 +18,6 @@ struct Digital_out { private: const struct gpio_dt_spec *gpio; - }; #endif diff --git a/include/initialization_state.h b/include/initialization_state.h index 7e2352a..03cf56d 100644 --- a/include/initialization_state.h +++ b/include/initialization_state.h @@ -5,13 +5,9 @@ #include "state.h" - class InitializationState : public State { private: - // static const unsigned long WAIT_MS = 2000; - - // unsigned long enter_time_ms = 0; static constexpr int64_t WAIT_MS = 2000; @@ -28,4 +24,4 @@ public: void exit() override; }; -#endif \ No newline at end of file +#endif diff --git a/include/motor_controller.h b/include/motor_controller.h index 4923ec9..63cbb24 100644 --- a/include/motor_controller.h +++ b/include/motor_controller.h @@ -14,9 +14,8 @@ class State; class MotorController { private: - static constexpr double DEFAULT_REFERENCE_RPM = 40.0; + static constexpr double DEFAULT_REFERENCE_RPM = 25.0; static constexpr double PWM_PER_PERCENT = 255.0 / 100.0; - //static const uint8_t LED_PIN = 5; // PB5 = D13 = LED_BUILTIN static constexpr uint8_t LED_PIN = Digital_out::STATUS_LED; State* current_state; diff --git a/include/motor_driver.h b/include/motor_driver.h index 15711b2..a492326 100644 --- a/include/motor_driver.h +++ b/include/motor_driver.h @@ -22,4 +22,4 @@ private: bool initialized; // Flag that ignores motor commands if setup fails }; -#endif \ No newline at end of file +#endif diff --git a/include/operational_state.h b/include/operational_state.h index f213ce9..e0867ef 100644 --- a/include/operational_state.h +++ b/include/operational_state.h @@ -26,4 +26,4 @@ public: void exit() override; }; -#endif \ No newline at end of file +#endif diff --git a/include/pre_operational_state.h b/include/pre_operational_state.h index 3de0737..12b014b 100644 --- a/include/pre_operational_state.h +++ b/include/pre_operational_state.h @@ -13,12 +13,9 @@ private: P_controller &pi_controller; bool pi_selected; - //unsigned long previous_time; int64_t previous_time; bool led_state; - //bool waiting_for_kp; - //bool waiting_for_ti; enum InputMode { @@ -41,4 +38,4 @@ public: void exit() override; }; -#endif \ No newline at end of file +#endif diff --git a/include/rotary_encoder_driver.h b/include/rotary_encoder_driver.h index f6006bd..34c7128 100644 --- a/include/rotary_encoder_driver.h +++ b/include/rotary_encoder_driver.h @@ -3,8 +3,9 @@ #include #include +#include -//#include "digital_in.h" +#include "digital_in.h" #include "timer_usec.h" class RotaryEncoderDriver { @@ -25,6 +26,10 @@ public: bool new_speed_available(); void reset_speed_measurement(); + // Blocks until the next speed sample, or until the timeout expires. + // Lets the main loop run in step with the 4 ms sampling. + void wait_for_sample(k_timeout_t timeout); + private: static const uint8_t HIST_LEN = 4; // 4 x Ts = 16 ms gluggi fyrir rpm-utreikning @@ -35,12 +40,11 @@ private: volatile int32_t history[HIST_LEN]; volatile uint8_t hist_idx; - - - //Digital_in eA; - //Digital_in eB; + Digital_in eA; + Digital_in eB; struct gpio_callback encoder_c1_callback; + struct k_sem sample_sem; Timer_usec timer; static void gpio_edge_handler( @@ -49,28 +53,6 @@ private: uint32_t pins); static void timer_expired(void *context); - - -// public: -// RotaryEncoderDriver(); - -// void begin(); - -// void update(); - -// int32_t position(); - -// void update_delta(); - -// float count_per_sec(); - -// float rpm(); - -// bool new_speed_available(); - -// void reset_speed_measurement(); - - - }; + #endif diff --git a/include/state.h b/include/state.h index 2707215..f6a010f 100644 --- a/include/state.h +++ b/include/state.h @@ -11,7 +11,6 @@ protected: public: virtual ~State() { - } void set_context(MotorController* context) @@ -26,7 +25,6 @@ public: virtual void command(char c) = 0; // Listen for reset virtual void exit() = 0; // Stop motor - }; -#endif \ No newline at end of file +#endif diff --git a/include/stopped_state.h b/include/stopped_state.h index af081a0..ad06f9d 100644 --- a/include/stopped_state.h +++ b/include/stopped_state.h @@ -8,9 +8,8 @@ class StoppedState : public State { private: - //unsigned long previous_time; // Used for blinking LED int64_t previous_time; - bool led_state; // Ditto + bool led_state; public: StoppedState(); @@ -22,7 +21,6 @@ public: void command(char c) override; void exit() override; - }; #endif diff --git a/include/timer_usec.h b/include/timer_usec.h index 3872cf9..18e9228 100644 --- a/include/timer_usec.h +++ b/include/timer_usec.h @@ -1,16 +1,9 @@ #ifndef TIMER_USEC_H #define TIMER_USEC_H -//#include - #include #include -// struct Timer_usec { -// Timer_usec(); -// void init(uint16_t period_us); -//}; - class Timer_usec { public: using callback_t = void (*)(void *context); diff --git a/platformio.ini b/platformio.ini index ced70ed..a360d14 100644 --- a/platformio.ini +++ b/platformio.ini @@ -18,3 +18,6 @@ monitor_speed = 115200 upload_flags = -c reset_config srst_only srst_nogate connect_assert_srst + +; CSV-útprentun í Operational fyrir step/load response (sjá capture_serial.py) +; build_flags = -D LOG_CSV=1 diff --git a/src/gamla_m_ain.cpp b/src/app.cpp similarity index 59% rename from src/gamla_m_ain.cpp rename to src/app.cpp index e565271..6be897e 100644 --- a/src/gamla_m_ain.cpp +++ b/src/app.cpp @@ -1,8 +1,3 @@ -// main.cpp er sleppt i test-build (pio test) svo setup()/loop() rekist ekki a prof -//#ifndef PIO_UNIT_TESTING - -//#include -//#include #include #include @@ -23,7 +18,6 @@ #include "operational_state.h" #include "stopped_state.h" - static RotaryEncoderDriver encoder; static MotorDriver motor; @@ -56,42 +50,34 @@ static MotorController motor_controller( ); -// int main() -// { -// init(); -// Serial.begin(115200); -// cli(); - -// encoder.begin(); -// motor.begin(); - -// sei(); - -// motor_controller.begin(); - -// while (true) -// { -// if (Serial.available() > 0) -// { -// char c = Serial.read(); - -// motor_controller.command(c); - -// } - - -// motor_controller.update(); -// } - - -// return 0; -// } - -// #endif - 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)) { @@ -99,6 +85,9 @@ extern "C" void motor_application_main(void) 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); @@ -109,17 +98,18 @@ extern "C" void motor_application_main(void) motor_controller.begin(); while (true) { - unsigned char received_char; + char received_char; - // Read a command if a character is waiting. - if (uart_poll_in(console_dev, &received_char) == 0) { - motor_controller.command( - static_cast(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(); - // Give Zephyr's scheduler time to run other threads. - k_msleep(1); + // 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)); } } diff --git a/src/digital_in.cpp b/src/digital_in.cpp index e8e1b08..32c3b40 100644 --- a/src/digital_in.cpp +++ b/src/digital_in.cpp @@ -1,17 +1,8 @@ -//#include #include "digital_in.h" +#include #include - -// Digital_in::Digital_in(uint8_t pin) : pin_mask(1< static const struct gpio_dt_spec encoder_c1_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c1), gpios); @@ -62,3 +53,22 @@ bool Digital_in::is_lo() return !is_hi(); } +int Digital_in::enable_rising_edge_interrupt( + struct gpio_callback *callback, + gpio_callback_handler_t handler) +{ + if (gpio == nullptr || !gpio_is_ready_dt(gpio)) { + return -ENODEV; + } + + gpio_init_callback(callback, handler, BIT(gpio->pin)); + + const int ret = gpio_add_callback(gpio->port, callback); + if (ret < 0) { + return ret; + } + + // Physical rising edge, independent of the active level in the overlay. + return gpio_pin_interrupt_configure( + gpio->port, gpio->pin, GPIO_INT_EDGE_RISING); +} diff --git a/src/digital_out.cpp b/src/digital_out.cpp index c5989d6..64aaafc 100644 --- a/src/digital_out.cpp +++ b/src/digital_out.cpp @@ -1,16 +1,7 @@ #include "digital_out.h" -//#include - #include -// Digital_out::Digital_out(uint8_t pin) : pin_mask(1< - #include #include - void InitializationState::enter() { - // Serial.println(); - // Serial.println("State: Initialization"); - - // context->led_off(); - - // context->stop_motor(); - - // context->reset_speed_measurement(); - - // enter_time_ms = millis(); - printk("\nState: Initialization\n"); context->led_off(); context->stop_motor(); context->reset_speed_measurement(); - enter_time_ms = k_uptime_get(); + enter_time_ms = k_uptime_get(); } void InitializationState::update() { - // // Non-blocking bið svo aðallykkjan haldi áfram að keyra - // if (millis() - enter_time_ms >= WAIT_MS) - // { - // context->to_pre_operational(); - // // context->to_operational(); - // } - - // Check the time without blocking the application. if ((k_uptime_get() - enter_time_ms) >= WAIT_MS) { context->to_pre_operational(); - } - + } } @@ -56,6 +33,5 @@ void InitializationState::command(char c) void InitializationState::exit() { - //Serial.println("Boot-up"); printk("Boot-up\n"); } diff --git a/src/main.c b/src/main.c index 08da793..cc8f040 100644 --- a/src/main.c +++ b/src/main.c @@ -1,12 +1,9 @@ #include -#ifndef PIO_UNIT_TESTING - void motor_application_main(void); -void main(void) +int main(void) { motor_application_main(); + return 0; } - -#endif \ No newline at end of file diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index 7dc6452..3482e54 100644 --- a/src/motor_controller.cpp +++ b/src/motor_controller.cpp @@ -1,9 +1,6 @@ #include "motor_controller.h" #include "state.h" -//#include - - MotorController::MotorController( RotaryEncoderDriver &encoder, MotorDriver &motor, @@ -33,18 +30,6 @@ MotorController::MotorController( } -// void MotorController::set_states( -// State* initialization, -// State* operational, -// State* stopped -// ) - - // initialization_state = initialization; - // operational_state = operational; - // stopped_state = stopped; - - - void MotorController::begin() { led.init(); diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index 5d3fd50..d0e7289 100644 --- a/src/motor_driver.cpp +++ b/src/motor_driver.cpp @@ -1,55 +1,25 @@ #include "motor_driver.h" -//#include - #include #include #include -// Í Fast PWM gefur OCR = 0 samt einn stuttan púls í hverri lotu. -// Til að pinni sé alveg LOW er hann aftengdur frá timernum (COM-bitar) -// og keyrður LOW gegnum PORT-registerið. - -// static void ain1_off() // D3 = PD3 = OC2B -// { -// TCCR2A &= ~(1 << COM2B1); -// PORTD &= ~(1 << PORTD3); -// OCR2B = 0; -// } - -// static void ain2_off() // D11 = PB3 = OC2A -// { -// TCCR2A &= ~(1 << COM2A1); -// PORTB &= ~(1 << PORTB3); -// OCR2A = 0; -// } - -// static void ain1_pwm(uint8_t duty) -// { -// OCR2B = duty; -// TCCR2A |= (1 << COM2B1); -// } - -// static void ain2_pwm(uint8_t duty) -// { -// OCR2A = duty; -// TCCR2A |= (1 << COM2A1); -// } - static const struct pwm_dt_spec bin1_pwm = PWM_DT_SPEC_GET(DT_ALIAS(motor_bin1_pwm)); -static const struct gpio_dt_spec bin2 = - GPIO_DT_SPEC_GET(DT_ALIAS(motor_bin2), gpios); +static const struct pwm_dt_spec bin2_pwm = + PWM_DT_SPEC_GET(DT_ALIAS(motor_bin2_pwm)); static const struct gpio_dt_spec motor_flt = GPIO_DT_SPEC_GET(DT_ALIAS(motor_flt), gpios); +static const struct gpio_dt_spec fault_test_button = + GPIO_DT_SPEC_GET(DT_ALIAS(fault_test_button), gpios); + static const struct gpio_dt_spec motor_slp = GPIO_DT_SPEC_GET(DT_ALIAS(motor_slp), gpios); - MotorDriver::MotorDriver() : initialized(false) { @@ -58,41 +28,18 @@ MotorDriver::MotorDriver() void MotorDriver::begin() { - // Arduino D3 = PD3 = AIN1 - // Arduino D11 = PB3 = AIN2 - // Arduino D4 = PD4 = FLT - - // DDRD |= (1 << DDD3); - // DDRB |= (1 << DDB3); - - // DDRD &= ~(1 << DDD4); - - // PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2 - - - // // Timer2 Fast PWM - // TCCR2A = 0; - // TCCR2B = 0; - - // // Fast PWM mode - // TCCR2A |= (1 << WGM20); - // TCCR2A |= (1 << WGM21); - - // // Timer2 prescaler = 32 - // TCCR2B |= (1 << CS21); - // TCCR2B |= (1 << CS20); - initialized = false; if (!pwm_is_ready_dt(&bin1_pwm) || - !gpio_is_ready_dt(&bin2) || + !pwm_is_ready_dt(&bin2_pwm) || !gpio_is_ready_dt(&motor_flt) || + !gpio_is_ready_dt(&fault_test_button) || !gpio_is_ready_dt(&motor_slp)) { return; } - if (gpio_pin_configure_dt(&bin2, GPIO_OUTPUT_INACTIVE) < 0 || - gpio_pin_configure_dt(&motor_flt, GPIO_INPUT) < 0 || + if (gpio_pin_configure_dt(&motor_flt, GPIO_INPUT) < 0 || + gpio_pin_configure_dt(&fault_test_button, GPIO_INPUT) < 0 || gpio_pin_configure_dt(&motor_slp, GPIO_OUTPUT_INACTIVE) < 0) { return; } @@ -102,10 +49,10 @@ void MotorDriver::begin() return; } - if (pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0) < 0) { + if (pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0) < 0 || + pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0) < 0) { return; } - // PWM-úttök eru tengd í set() eftir þörfum initialized = true; stop(); } @@ -125,22 +72,17 @@ void MotorDriver::reverse() void MotorDriver::stop() { - // Both LOW - // ain1_off(); - // ain2_off(); - if (!initialized) { return; } // BIN1 low and BIN2 low puts the DRV8833 in coast mode. pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0); - gpio_pin_set_dt(&bin2, 0); + pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0); } void MotorDriver::set(double pwm) { - if (!initialized) { return; } @@ -159,63 +101,42 @@ void MotorDriver::set(double pwm) } - // uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm); + const double magnitude = pwm > 0.0 ? pwm : -pwm; - // if (duty == 0) - // { - // stop(); - // } - // else if (pwm > 0) - // { - // // Forward - // ain2_off(); - // ain1_pwm(duty); - // } - // else - // { - // // Reverse - // ain1_off(); - // ain2_pwm(duty); - // } - - const uint32_t magnitude = - static_cast(pwm > 0.0 ? pwm : -pwm); - - // PWM pulse width in nanoseconds. The overlay sets a 50 us period. - const uint32_t requested_pulse = - (bin1_pwm.period * magnitude) / 255U; - - // First put BIN1 low so changing BIN2 does not switch direction - // while the previous PWM signal is still active. - pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0); - - uint32_t bin1_pulse; + // Pulse width in nanoseconds, kept as a double until the end so the + // 4000-count timer resolution is not lost to integer truncation. + const uint32_t pulse = static_cast( + static_cast(bin1_pwm.period) * magnitude / 255.0); + // Same PWM scheme as Project 3: one input PWM, the other input low, + // so both directions use fast decay (coast during the off time). + // The idle input is set low first; both channels share TIM1 with + // preloaded compare registers, so the change takes effect together + // at the next timer update. if (pwm > 0.0) { - // Forward: BIN2 low; BIN1 PWM duty equals the command magnitude. - gpio_pin_set_dt(&bin2, 0); - bin1_pulse = requested_pulse; + // Forward: BIN1 PWM, BIN2 low. + pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0); + pwm_set_dt(&bin1_pwm, bin1_pwm.period, pulse); } else { - // Reverse: BIN2 high; BIN1 pulse is inverted for reverse PWM. - gpio_pin_set_dt(&bin2, 1); - bin1_pulse = bin1_pwm.period - requested_pulse; + // Reverse: BIN1 low, BIN2 PWM. + pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0); + pwm_set_dt(&bin2_pwm, bin2_pwm.period, pulse); } - - pwm_set_dt(&bin1_pwm, bin1_pwm.period, bin1_pulse); - } bool MotorDriver::fault_active() { - //return !(PIND & (1 << PIND4)); - if (!initialized) { return true; // Fail safe if the driver did not initialize. } // The overlay marks FLT as active low. gpio_pin_get_dt() returns // the logical active state, so 1 means a fault is active. - const int fault = gpio_pin_get_dt(&motor_flt); - return fault < 0 || fault != 0; + const int driver_fault = gpio_pin_get_dt(&motor_flt); + const int button_fault = gpio_pin_get_dt(&fault_test_button); + return driver_fault < 0 || + button_fault < 0 || + driver_fault != 0 || + button_fault != 0; } diff --git a/src/operational_state.cpp b/src/operational_state.cpp index 2190551..c3e8923 100644 --- a/src/operational_state.cpp +++ b/src/operational_state.cpp @@ -1,14 +1,10 @@ #include "operational_state.h" #include "motor_controller.h" -//#include - #include - void OperationalState::enter() { - //Serial.println("State: Operational"); printk("State: Operational\n"); context->reset_controller(); @@ -18,25 +14,15 @@ void OperationalState::enter() sample_count = 0; #if LOG_CSV - //Serial.println("time_ms,ref_rpm,actual_rpm,pwm"); printk("time_ms,ref_rpm,actual_rpm,pwm\n"); #endif } -// Part 1 - -//void OperationalState::update() -//{ -// context->run_speed_controller(); -//} - -// Part 2 void OperationalState::update() { if (context->fault_active()) { - //Serial.println("Fault detected"); printk("Fault detected\n"); context->to_stopped(); @@ -50,13 +36,6 @@ void OperationalState::update() #if LOG_CSV // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational - // Serial.print(sample_count * 4UL); - // Serial.print(','); - // Serial.print(context->get_reference_rpm(), 1); - // Serial.print(','); - // Serial.print(context->get_actual_rpm(), 2); - // Serial.print(','); - // Serial.println(context->get_pwm(), 1); printk("%u,%.1f,%.2f,%.1f\n", static_cast(sample_count * 4U), context->get_reference_rpm(), @@ -71,7 +50,6 @@ void OperationalState::command(char c) { if (c == 'r' || c == 'R') { - //Serial.println("Command: reset"); printk("Command: reset\n"); context->to_initialization(); @@ -79,7 +57,6 @@ void OperationalState::command(char c) else if (c == 'p' || c == 'P') { - //Serial.println("Command: set pre-operational"); printk("Command: set pre-operational\n"); context->to_pre_operational(); @@ -92,4 +69,4 @@ void OperationalState::exit() context->stop_motor(); context->led_off(); -} \ No newline at end of file +} diff --git a/src/p_controller.cpp b/src/p_controller.cpp index 616207b..fc82027 100644 --- a/src/p_controller.cpp +++ b/src/p_controller.cpp @@ -10,4 +10,4 @@ double P_controller::update(double ref, double actual) double error = ref - actual; return Kp * error; -} \ No newline at end of file +} diff --git a/src/pi_controller.cpp b/src/pi_controller.cpp index c2e980f..eaf1f6b 100644 --- a/src/pi_controller.cpp +++ b/src/pi_controller.cpp @@ -23,7 +23,6 @@ double PI_controller::update(double ref, double actual) integral = new_integral; } return u; - // return 0.0; } void PI_controller::reset() diff --git a/src/pre_operational_state.cpp b/src/pre_operational_state.cpp index a1ed94d..c1c7cd2 100644 --- a/src/pre_operational_state.cpp +++ b/src/pre_operational_state.cpp @@ -3,22 +3,16 @@ #include "p_controller.h" -//#include - #include #include #include - - PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller) : p_controller(p_controller), pi_controller(pi_controller), pi_selected(true), previous_time(0), led_state(false), - //waiting_for_kp(false), - //waiting_for_ti(false), input_mode(NONE), input_index(0) { @@ -26,22 +20,22 @@ PreOperationalState::PreOperationalState(P_controller &p_controller, P_controlle void PreOperationalState::enter() { - //Serial.println("State: Pre-operational"); printk("State: Pre-operational\n"); + // The selection survives a reset, so report it (used by capture_serial.py). + printk("Controller: %s\n", pi_selected ? "PI" : "P"); + context->stop_motor(); context->led_off(); led_state = false; - //previous_time = millis(); previous_time = k_uptime_get(); } void PreOperationalState::update() { - //unsigned long current_time = millis(); const int64_t current_time = k_uptime_get(); if (current_time - previous_time >= 500) @@ -81,17 +75,13 @@ void PreOperationalState::command(char c) { context->set_Kp(value); - // Serial.print("Kp set to: "); - // Serial.println(value); - printk("Kp updated\n"); + printk("Kp set to: %.4f\n", value); } else if (input_mode == ENTER_TI) { context->set_Ti(value); - // Serial.print("Ti set to: "); - // Serial.println(value); - printk("Ti updated\n"); + printk("Ti set to: %.4f\n", value); } } @@ -110,7 +100,6 @@ void PreOperationalState::command(char c) if (input_index < sizeof(input_buffer) - 1) { input_buffer[input_index++] = c; - //input_index++; } } @@ -121,7 +110,6 @@ void PreOperationalState::command(char c) // Normal state commands if (c == 'o' || c == 'O') { - //Serial.println("Command: set operational"); printk("Command: set operational\n"); context->to_operational(); @@ -129,7 +117,6 @@ void PreOperationalState::command(char c) else if (c == 'r' || c == 'R') { - //Serial.println("Command: reset"); printk("Command: reset\n"); context->to_initialization(); @@ -140,7 +127,6 @@ void PreOperationalState::command(char c) input_mode = ENTER_KP; input_index = 0; - //Serial.println("Enter Kp and press Enter:"); printk("Enter Kp and press Enter:\n"); } @@ -149,7 +135,6 @@ void PreOperationalState::command(char c) input_mode = ENTER_TI; input_index = 0; - //Serial.println("Enter Ti and press Enter:"); printk("Enter Ti and press Enter:\n"); } @@ -161,13 +146,11 @@ void PreOperationalState::command(char c) if (pi_selected) { context->set_controller(pi_controller); - //Serial.println("Controller: PI"); printk("Controller: PI\n"); } else { context->set_controller(p_controller); - //Serial.println("Controller: P"); printk("Controller: P\n"); } } @@ -176,4 +159,4 @@ void PreOperationalState::command(char c) void PreOperationalState::exit() { context->led_off(); -} \ No newline at end of file +} diff --git a/src/rotary_encoder_driver.cpp b/src/rotary_encoder_driver.cpp index b7b75d0..ac10556 100644 --- a/src/rotary_encoder_driver.cpp +++ b/src/rotary_encoder_driver.cpp @@ -4,17 +4,13 @@ #include #include -static const struct gpio_dt_spec encoder_c1 = - GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c1), gpios); - -static const struct gpio_dt_spec encoder_c2 = - GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c2), gpios); - RotaryEncoderDriver::RotaryEncoderDriver() : count(0), delta(0), speed_ready(false), - hist_idx(0) + hist_idx(0), + eA(Digital_in::ENCODER_C1), + eB(Digital_in::ENCODER_C2) { for (uint8_t i = 0; i < HIST_LEN; ++i) { history[i] = 0; @@ -23,35 +19,16 @@ RotaryEncoderDriver::RotaryEncoderDriver() int RotaryEncoderDriver::begin() { - if (!gpio_is_ready_dt(&encoder_c1) || - !gpio_is_ready_dt(&encoder_c2)) { - return -ENODEV; - } + eA.init(); + eB.init(); - int ret = gpio_pin_configure_dt(&encoder_c1, GPIO_INPUT); - if (ret < 0) { - return ret; - } + // Max count 1: the main loop only needs to know that a new sample + // exists, not how many it missed. + k_sem_init(&sample_sem, 0, 1); - ret = gpio_pin_configure_dt(&encoder_c2, GPIO_INPUT); - if (ret < 0) { - return ret; - } - - gpio_init_callback( + const int ret = eA.enable_rising_edge_interrupt( &encoder_c1_callback, - gpio_edge_handler, - BIT(encoder_c1.pin)); - - ret = gpio_add_callback(encoder_c1.port, &encoder_c1_callback); - if (ret < 0) { - return ret; - } - - // C1 is active high, so “to active” means a rising edge. - ret = gpio_pin_interrupt_configure_dt( - &encoder_c1, - GPIO_INT_EDGE_TO_ACTIVE); + gpio_edge_handler); if (ret < 0) { return ret; } @@ -68,16 +45,16 @@ void RotaryEncoderDriver::gpio_edge_handler( struct gpio_callback *callback, uint32_t pins) { - (void)dev; // This handler does not need the device pointer. + // Zephyr only calls this handler for C1, the one pin in the callback. + (void)dev; + (void)pins; auto *encoder = CONTAINER_OF( callback, RotaryEncoderDriver, encoder_c1_callback); - if ((pins & BIT(encoder_c1.pin)) != 0U) { - encoder->update(); - } + encoder->update(); } void RotaryEncoderDriver::timer_expired(void *context) @@ -89,13 +66,10 @@ void RotaryEncoderDriver::timer_expired(void *context) void RotaryEncoderDriver::update() { // C2's level at the C1 edge tells us the direction. - const int direction = gpio_pin_get_dt(&encoder_c2); - if (direction < 0) { - return; - } + const bool forward = eB.is_hi(); const unsigned int key = irq_lock(); - count += direction ? 1 : -1; + count += forward ? 1 : -1; irq_unlock(key); } @@ -122,6 +96,14 @@ void RotaryEncoderDriver::update_delta() speed_ready = true; irq_unlock(key); + + // Wake the main loop so the controller runs right after the sample. + k_sem_give(&sample_sem); +} + +void RotaryEncoderDriver::wait_for_sample(k_timeout_t timeout) +{ + (void)k_sem_take(&sample_sem, timeout); } float RotaryEncoderDriver::count_per_sec() @@ -168,4 +150,4 @@ void RotaryEncoderDriver::reset_speed_measurement() hist_idx = 0; irq_unlock(key); -} \ No newline at end of file +} diff --git a/src/rotary_encoder_driver_gamli.cpp b/src/rotary_encoder_driver_gamli.cpp deleted file mode 100644 index dc17219..0000000 --- a/src/rotary_encoder_driver_gamli.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// #include "rotary_encoder_driver.h" -// #include - -// static RotaryEncoderDriver* driver = nullptr; - -// RotaryEncoderDriver::RotaryEncoderDriver() -// : count(0) -// , delta(0) -// , speed_ready(false) -// , hist_idx(0) -// , eA(2) -// , eB(5) -// { -// for (uint8_t i = 0; i < HIST_LEN; i++) -// { -// history[i] = 0; -// } -// } - - -// void RotaryEncoderDriver::begin() -// { -// driver = this; - -// eA.init(); -// eB.init(); - -// EICRA |= (1<update(); -// } -// ISR(TIMER1_COMPA_vect) -// { -// driver->update_delta(); -// } diff --git a/src/stopped_state.cpp b/src/stopped_state.cpp index 56a30e4..856231e 100644 --- a/src/stopped_state.cpp +++ b/src/stopped_state.cpp @@ -1,12 +1,9 @@ #include "stopped_state.h" #include "motor_controller.h" -//#include - #include #include - StoppedState::StoppedState() : previous_time(0), led_state(false) @@ -16,8 +13,6 @@ StoppedState::StoppedState() void StoppedState::enter() { - //Serial.println("State: Stopped"); - printk("State: Stopped\n"); context->stop_motor(); @@ -26,7 +21,6 @@ void StoppedState::enter() context->led_off(); - //previous_time = millis(); previous_time = k_uptime_get(); } @@ -34,7 +28,6 @@ void StoppedState::enter() void StoppedState::update() { - //unsigned long current_time = millis(); const int64_t current_time = k_uptime_get(); if (current_time - previous_time >= 250) @@ -61,7 +54,6 @@ void StoppedState::command(char c) { if (c == 'r' || c == 'R') { - //Serial.println("Command: reset"); printk("Command: reset\n"); context->to_initialization(); @@ -69,7 +61,6 @@ void StoppedState::command(char c) else if (c == 'o' || c == 'O') { - //Serial.println("Command: set operational"); printk("Command: set operational\n"); context->to_operational(); @@ -77,7 +68,6 @@ void StoppedState::command(char c) else if (c == 'p' || c == 'P') { - //Serial.println("Command: set pre-operational"); printk("Command: set pre-operational\n"); context->to_pre_operational(); @@ -88,4 +78,4 @@ void StoppedState::command(char c) void StoppedState::exit() { context->led_off(); -} \ No newline at end of file +} diff --git a/src/timer_usec.cpp b/src/timer_usec.cpp index 9d8f53b..e534814 100644 --- a/src/timer_usec.cpp +++ b/src/timer_usec.cpp @@ -1,11 +1,9 @@ #include "timer_usec.h" -Timer_usec::Timer_usec() //= default; +Timer_usec::Timer_usec() : callback(nullptr), callback_context(nullptr) - { - } void Timer_usec::set_callback( @@ -17,17 +15,6 @@ void Timer_usec::set_callback( } -// void Timer_usec::init(uint16_t period_us) { // for custom duty cycle duration in ms in part 1 -// TCCR1A = 0; -// TCCR1B = 0; -// TCNT1 = 0; - -// OCR1A = period_us * 2 - 1; -// TCCR1B |= (1<callback != nullptr) { self->callback(self->callback_context); } -} \ No newline at end of file +} diff --git a/test/test_pi_controller/test_main.cpp b/test/test_pi_controller/test_main.cpp deleted file mode 100644 index f14b87b..0000000 --- a/test/test_pi_controller/test_main.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include - -// #ifdef ARDUINO -#include -// #endif - -#include "p_controller.h" -#include "pi_controller.h" - -static const double KP = 2.0; -static const double TI = 1.0; -static const double T = 0.1; -static const double TOL = 1e-4; // double er 32-bita float a AVR - -// void setUp(void) {} -// void tearDown(void) {} - -// Normal flow: engin villa -> ekkert úttak -void test_zero_error_gives_zero_output(void) -{ - PI_controller c(KP, TI, T); - TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(10.0, 10.0)); -} - -// Normal flow: fyrsta skref u = Kp * (e + e*T/Ti) -void test_first_step(void) -{ - PI_controller c(KP, TI, T); - double e = 5.0; - TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0)); -} - -// Normal flow: stöðug villa safnast upp, u_n = Kp * (e + n*e*T/Ti) -void test_integral_accumulates(void) -{ - PI_controller c(KP, TI, T); - double e = 5.0; - double u = 0.0; - for (int n = 1; n <= 5; n++) - { - u = c.update(e, 0.0); - TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + n * e * T / TI), u); - } -} - -// Normal flow: þegar villan er orðin 0 heldur integralið úttakinu uppi -void test_output_held_when_error_zero(void) -{ - PI_controller c(KP, TI, T); - double e = 5.0; - for (int n = 0; n < 5; n++) - { - c.update(e, 0.0); - } - double E = 5 * e * T; - TEST_ASSERT_FLOAT_WITHIN(TOL, KP * E / TI, c.update(0.0, 0.0)); -} - -// Alt flow: villa langt utan getu -> mettun í +100 % -void test_saturates_high(void) -{ - PI_controller c(KP, TI, T); - TEST_ASSERT_FLOAT_WITHIN(TOL, 100.0, c.update(10000.0, 0.0)); -} - -// Alt flow: mettun í -100 % -void test_saturates_low(void) -{ - PI_controller c(KP, TI, T); - TEST_ASSERT_FLOAT_WITHIN(TOL, -100.0, c.update(-10000.0, 0.0)); -} - -// Alt flow: anti-windup. Eftir langa mettun á lítil neikvæð villa -// strax að gefa úttak undir mettun (integralið óx ekki) -void test_no_integral_windup(void) -{ - PI_controller c(KP, TI, T); - for (int n = 0; n < 1000; n++) - { - c.update(10000.0, 0.0); - } - double u = c.update(0.0, 1.0); // e = -1 - TEST_ASSERT_TRUE(u < 0.0); -} - -// Alt flow: reset (stopped -> operational) hreinsar integralið -void test_reset_clears_integral(void) -{ - PI_controller c(KP, TI, T); - for (int n = 0; n < 5; n++) - { - c.update(5.0, 0.0); - } - c.reset(); - TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(0.0, 0.0)); -} - -// Polymorphism: kall í gegnum P_controller& gefur PI hegðun -void test_polymorphic_update(void) -{ - PI_controller pi(KP, TI, T); - P_controller& c = pi; - double e = 5.0; - TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0)); -} - -// P_controller hegðar sér óbreytt -void test_p_controller_unchanged(void) -{ - P_controller c(KP); - TEST_ASSERT_FLOAT_WITHIN(TOL, KP * 5.0, c.update(5.0, 0.0)); -} - -void setup() -{ - // init(); - delay(2000); - UNITY_BEGIN(); - RUN_TEST(test_zero_error_gives_zero_output); - RUN_TEST(test_first_step); - RUN_TEST(test_integral_accumulates); - RUN_TEST(test_output_held_when_error_zero); - RUN_TEST(test_saturates_high); - RUN_TEST(test_saturates_low); - RUN_TEST(test_no_integral_windup); - RUN_TEST(test_reset_clears_integral); - RUN_TEST(test_polymorphic_update); - RUN_TEST(test_p_controller_unchanged); - // return UNITY_END(); - UNITY_END(); -} - -void loop() {} - -// #ifdef ARDUINO -// void setup() -// { -// delay(2000); // bida eftir serial tengingu eftir reset -// run_tests(); -// } - -// void loop() -// { -// } -// #else -// int main(int argc, char** argv) -// { -// return run_tests(); -// } -// #endif diff --git a/zephyr/boards/nucleo_l432kc.overlay b/zephyr/boards/nucleo_l432kc.overlay index 5d27c9f..c9b8f66 100644 --- a/zephyr/boards/nucleo_l432kc.overlay +++ b/zephyr/boards/nucleo_l432kc.overlay @@ -1,5 +1,6 @@ #include #include +#include / { aliases { @@ -7,9 +8,10 @@ encoder-c2 = &encoder_c2; motor-bin1-pwm = &motor_bin1_pwm; - motor-bin2 = &motor_bin2; + motor-bin2-pwm = &motor_bin2_pwm; motor-flt = &motor_flt; motor-slp = &motor_slp; + fault-test-button = &fault_test_button; }; buttons { @@ -29,16 +31,16 @@ gpios = <&gpioa 0 (GPIO_ACTIVE_LOW | GPIO_PULL_UP)>; /* A0, PA0 */ label = "Motor fault"; }; + + fault_test_button: fault_test_button { + gpios = <&gpiob 0 (GPIO_ACTIVE_LOW | GPIO_PULL_UP)>; /* D3, PB0 */ + label = "Fault test button"; + }; }; motor_gpio_outputs { compatible = "gpio-leds"; - motor_bin2: motor_bin2 { - gpios = <&gpiob 1 GPIO_ACTIVE_HIGH>; /* D6, PB1 */ - label = "Motor BIN2"; - }; - motor_slp: motor_slp { gpios = <&gpioa 1 GPIO_ACTIVE_HIGH>; /* A1, PA1 */ label = "Motor sleep"; @@ -52,16 +54,25 @@ pwms = <&pwm1 1 PWM_USEC(50) 0>; /* D9, PA8; 20 kHz */ label = "Motor BIN1 PWM"; }; + + /* PB1 has no regular TIM1 channel, only the complementary output + * TIM1_CH3N. With CH3 itself disabled, CH3N outputs the + * non-inverted CH3 reference, so it behaves like a normal channel. */ + motor_bin2_pwm: motor_bin2_pwm { + pwms = <&pwm1 3 PWM_USEC(50) (PWM_POLARITY_NORMAL | STM32_PWM_COMPLEMENTARY)>; /* D6, PB1; 20 kHz */ + label = "Motor BIN2 PWM"; + }; }; }; &timers1 { status = "okay"; - st,prescaler = <79>; + /* No prescaler: 80 MHz timer clock, so a 50 us period is 4000 counts. */ + st,prescaler = <0>; pwm1: pwm { status = "okay"; - pinctrl-0 = <&tim1_ch1_pa8>; + pinctrl-0 = <&tim1_ch1_pa8 &tim1_ch3n_pb1>; pinctrl-names = "default"; }; }; \ No newline at end of file diff --git a/zephyr/prj.conf b/zephyr/prj.conf index d18c3e9..14b9d3f 100644 --- a/zephyr/prj.conf +++ b/zephyr/prj.conf @@ -6,5 +6,6 @@ CONFIG_PWM=y CONFIG_SERIAL=y CONFIG_CONSOLE=y CONFIG_UART_CONSOLE=y +CONFIG_UART_INTERRUPT_DRIVEN=y CONFIG_PRINTK=y CONFIG_CBPRINTF_FP_SUPPORT=y \ No newline at end of file