Merge branch 'Isak_part1' of ssh://gitea.cs.ru.is:2222/helgie22/ESP_Project4_H26 into Isak_part1

.gitignore

@@ -3,3 +3,6 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+scripts
+__pycache__/
+*.typ

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
+A repository for project 4, done in the Embedded System Programming class in Reykjavík University in the Fall semester of 2026

include/digital_in.h

@@ -4,19 +4,28 @@
 #include <stdint.h>
 #include <zephyr/drivers/gpio.h>
 
-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

include/digital_out.h

@@ -4,10 +4,7 @@
 #include<stdint.h>
 #include <zephyr/drivers/gpio.h>
 
-
 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

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;

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;

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
     {

include/rotary_encoder_driver.h

@@ -3,8 +3,9 @@
 
 #include <stdint.h>
 #include <zephyr/drivers/gpio.h>
+#include <zephyr/kernel.h>
 
-//#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

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

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

include/timer_usec.h

@@ -1,16 +1,9 @@
 #ifndef TIMER_USEC_H
 #define TIMER_USEC_H
 
-//#include <avr/io.h>
-
 #include <stdint.h>
 #include <zephyr/kernel.h>
 
-// struct Timer_usec {
-//     Timer_usec();
-//     void init(uint16_t period_us);
-//};
-
 class Timer_usec {
 public:
     using callback_t = void (*)(void *context);

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

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 <Arduino.h>
-//#include <avr/interrupt.h>
 
 #include <zephyr/device.h>
 #include <zephyr/devicetree.h>
@@ -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.
+        // Handle every character the ISR has queued since the last pass.
-        if (uart_poll_in(console_dev, &received_char) == 0) {
+        while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) {
-            motor_controller.command(
+            motor_controller.command(received_char);
-                static_cast<char>(received_char));
         }
 
         motor_controller.update();
 
-        // Give Zephyr's scheduler time to run other threads.
+        // Sleep until the encoder has a new speed sample (every 4 ms), so
-        k_msleep(1);
+        // 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));
     }
 }

src/digital_in.cpp

@@ -1,17 +1,8 @@
-//#include <avr/io.h>
 #include "digital_in.h"
 
+#include <errno.h>
 #include <zephyr/devicetree.h>
-
+#include <zephyr/sys/util.h>
-// Digital_in::Digital_in(uint8_t pin) : pin_mask(1<<pin) {}
-
-// void Digital_in::init() {
-//     DDRD  &= ~pin_mask;
-//     PORTD |=  pin_mask;
-// }
-
-// bool Digital_in::is_hi() { return PIND & pin_mask; }
-// bool Digital_in::is_lo() { return !is_hi(); }
 
 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);
+}

src/digital_out.cpp

@@ -1,16 +1,7 @@
 #include "digital_out.h"
 
-//#include <avr/io.h>
-
 #include <zephyr/devicetree.h>
 
-// Digital_out::Digital_out(uint8_t pin) : pin_mask(1<<pin) {}
-
-// void Digital_out::init()   { DDRB  |=  pin_mask; }
-// void Digital_out::toggle() { PORTB ^=  pin_mask; }
-// void Digital_out::set_hi() { PORTB |=  pin_mask; }
-// void Digital_out::set_lo() { PORTB &= ~pin_mask; }
-
 static const struct gpio_dt_spec status_led_gpio =
     GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
 

src/initialization_state.cpp

@@ -1,25 +1,11 @@
 #include "initialization_state.h"
 #include "motor_controller.h"
 
-//#include <Arduino.h>
-
 #include <zephyr/kernel.h>
 #include <zephyr/sys/printk.h>
 
-
 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();
@@ -32,19 +18,10 @@ void InitializationState::enter()
 
 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");
 }

src/main.c

@@ -1,12 +1,9 @@
 #include <zephyr/kernel.h>
 
-#ifndef PIO_UNIT_TESTING
-
 void motor_application_main(void);
 
-void main(void)
+int main(void)
 {
     motor_application_main();
+    return 0;
 }
-
-#endif

src/motor_controller.cpp

@@ -1,9 +1,6 @@
 #include "motor_controller.h"
 #include "state.h"
 
-//#include <Arduino.h>
-
-
 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();

src/motor_driver.cpp

@@ -1,46 +1,14 @@
 #include "motor_driver.h"
 
-//#include <avr/io.h>
-
 #include <zephyr/devicetree.h>
 #include <zephyr/drivers/gpio.h>
 #include <zephyr/drivers/pwm.h>
 
-// Í 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 =
+static const struct pwm_dt_spec bin2_pwm =
-    GPIO_DT_SPEC_GET(DT_ALIAS(motor_bin2), gpios);
+    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);
@@ -52,7 +20,6 @@ static const struct gpio_dt_spec motor_slp =
     GPIO_DT_SPEC_GET(DT_ALIAS(motor_slp), gpios);
 
 
-
 MotorDriver::MotorDriver()
     : initialized(false)
 {
@@ -61,42 +28,17 @@ 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 ||
+    if (gpio_pin_configure_dt(&motor_flt, GPIO_INPUT) < 0 ||
-        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;
@@ -107,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();
 }
@@ -130,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;
     }
@@ -164,50 +101,27 @@ 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)
+    // Pulse width in nanoseconds, kept as a double until the end so the
-    // {
+    // 4000-count timer resolution is not lost to integer truncation.
-    //     stop();
+    const uint32_t pulse = static_cast<uint32_t>(
-    // }
+        static_cast<double>(bin1_pwm.period) * magnitude / 255.0);
-    // else if (pwm > 0)
-    // {
-    //     // Forward
-    //     ain2_off();
-    //     ain1_pwm(duty);
-    // }
-    // else
-    // {
-    //     // Reverse
-    //     ain1_off();
-    //     ain2_pwm(duty);
-    // }
-
-    const uint32_t magnitude =
-        static_cast<uint32_t>(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;
 
+    // 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.
+        // Forward: BIN1 PWM, BIN2 low.
-        gpio_pin_set_dt(&bin2, 0);
+        pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0);
-        bin1_pulse = requested_pulse;
+        pwm_set_dt(&bin1_pwm, bin1_pwm.period, pulse);
     } else {
-        // Reverse: BIN2 high; BIN1 pulse is inverted for reverse PWM.
+        // Reverse: BIN1 low, BIN2 PWM.
-        gpio_pin_set_dt(&bin2, 1);
+        pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0);
-        bin1_pulse = bin1_pwm.period - requested_pulse;
+        pwm_set_dt(&bin2_pwm, bin2_pwm.period, pulse);
     }
-
-    pwm_set_dt(&bin1_pwm, bin1_pwm.period, bin1_pulse);
-
 }
 
 
@@ -231,11 +145,10 @@ bool MotorDriver::fault_active()
         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 driver_fault = gpio_pin_get_dt(&motor_flt);
     const int button_fault = gpio_pin_get_dt(&fault_test_button);
-
-    // Negative values indicate a GPIO read error, so treat them as a fault.
-    // With GPIO_ACTIVE_LOW, a pressed button reads as logical 1.
     return driver_fault < 0 ||
            button_fault < 0 ||
            driver_fault != 0 ||

src/operational_state.cpp

@@ -1,14 +1,10 @@
 #include "operational_state.h"
 #include "motor_controller.h"
 
-//#include <Arduino.h>
-
 #include <zephyr/sys/printk.h>
 
-
 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<unsigned int>(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();

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()

src/pre_operational_state.cpp

@@ -3,22 +3,16 @@
 
 #include "p_controller.h"
 
-//#include <Arduino.h>
-
 #include <stdlib.h>
 #include <zephyr/kernel.h>
 #include <zephyr/sys/printk.h>
 
-
-
 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: ");
+                    printk("Kp set to: %.4f\n", value);
-                    // Serial.println(value);
-                    printk("Kp updated\n");
                 }
                 else if (input_mode == ENTER_TI)
                 {
                     context->set_Ti(value);
 
-                    // Serial.print("Ti set to: ");
+                    printk("Ti set to: %.4f\n", value);
-                    // Serial.println(value);
-                    printk("Ti updated\n");
                 }
             }
 
@@ -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");
         }
     }

src/rotary_encoder_driver.cpp

@@ -11,17 +11,13 @@
 static const struct gpio_dt_spec isr_probe =
     GPIO_DT_SPEC_GET(DT_ALIAS(encoder_isr_probe), gpios);
 
-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;
@@ -30,40 +26,16 @@ RotaryEncoderDriver::RotaryEncoderDriver()
 
 int RotaryEncoderDriver::begin()
 {
-    if (!gpio_is_ready_dt(&encoder_c1) ||
+    eA.init();
-        !gpio_is_ready_dt(&encoder_c2) ||
+    eB.init();
-        !gpio_is_ready_dt(&isr_probe)) {
-        return -ENODEV;
-    }
 
-    int ret = gpio_pin_configure_dt(&isr_probe, GPIO_OUTPUT_INACTIVE);
+    // Max count 1: the main loop only needs to know that a new sample
-    if (ret < 0) {
+    // exists, not how many it missed.
-        return ret;
+    k_sem_init(&sample_sem, 0, 1);
-    }
 
-    ret = gpio_pin_configure_dt(&encoder_c1, GPIO_INPUT);
+    const int ret = eA.enable_rising_edge_interrupt(
-    if (ret < 0) {
-        return ret;
-    }
-
-    ret = gpio_pin_configure_dt(&encoder_c2, GPIO_INPUT);
-    if (ret < 0) {
-        return ret;
-    }
-
-    gpio_init_callback(
         &encoder_c1_callback,
-        gpio_edge_handler,
+        gpio_edge_handler);
-        BIT(encoder_c1.pin));
-
-    ret = gpio_add_callback(encoder_c1.port, &encoder_c1_callback);
-    if (ret < 0) {
-        return ret;
-    }
-
-    ret = gpio_pin_interrupt_configure_dt(
-        &encoder_c1,
-        GPIO_INT_EDGE_TO_ACTIVE);
     if (ret < 0) {
         return ret;
     }
@@ -80,18 +52,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();                 // Reads C2 and updates the count
-        gpio_pin_set_dt(&isr_probe, 1);   // Marker rising edge
-        gpio_pin_set_dt(&isr_probe, 0);   // Reset for the next interrupt
-    }
 }
 
 void RotaryEncoderDriver::timer_expired(void *context)
@@ -103,13 +73,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);
+    const bool forward = eB.is_hi();
-    if (direction < 0) {
-        return;
-    }
 
     const unsigned int key = irq_lock();
-    count += direction ? 1 : -1;
+    count += forward ? 1 : -1;
     irq_unlock(key);
 }
 
@@ -136,6 +103,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()

src/rotary_encoder_driver_gamli.cpp

@@ -1,132 +0,0 @@
-// #include "rotary_encoder_driver.h"
-// #include <avr/interrupt.h>
-
-// 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<<ISC01) | (1<<ISC00);
-//     EIMSK |= (1<<INT0);
-
-//     timer.init(SAMPLE_PERIOD_US);
-// }
-
-// void RotaryEncoderDriver::update()
-// {
-//     count += eB.is_hi() ? 1 : -1;
-// }
-
-// int32_t RotaryEncoderDriver::position()
-// {
-
-//     uint8_t oldSREG = SREG;
-
-//     cli();
-
-//     int32_t local_count = count;
-
-//     SREG = oldSREG;
-
-//     return local_count;
-// }
-
-// void RotaryEncoderDriver::update_delta()
-// {
-
-//     int32_t oldest = history[hist_idx];
-
-//     history[hist_idx] = count;
-
-//     hist_idx = (hist_idx + 1) % HIST_LEN;
-
-//     delta = (int16_t)(count - oldest);   // breyting yfir HIST_LEN * Ts (16 ms) glugga
-
-//     speed_ready = true;
-// }
-
-// float RotaryEncoderDriver::count_per_sec()
-// {
-
-//     uint8_t oldSREG = SREG;
-
-//     cli();
-
-//     int16_t local_delta = delta;
-
-//     SREG = oldSREG;
-
-//     return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
-// }
-
-
-// float RotaryEncoderDriver::rpm()
-// {
-//     return count_per_sec() * 60.0f / COUNTS_PER_REV;
-// }
-
-// bool RotaryEncoderDriver::new_speed_available()
-// {
-//     uint8_t oldSREG = SREG;
-
-//     cli();
-
-//     bool ready = speed_ready;
-
-//     if (ready)
-//     {
-//         speed_ready = false;
-//     }
-
-//     SREG = oldSREG;
-
-//     return ready;
-// }
-
-
-// void RotaryEncoderDriver::reset_speed_measurement()
-// {
-//     uint8_t oldSREG = SREG;
-
-//     cli();
-
-//     delta = 0;
-//     speed_ready = false;
-
-//     for (uint8_t i = 0; i < HIST_LEN; i++)
-//     {
-//         history[i] = count;
-//     }
-
-//     hist_idx = 0;
-
-//     SREG = oldSREG;
-// }
-
-// ISR(INT0_vect)
-// {
-//     driver->update();
-// }
-// ISR(TIMER1_COMPA_vect)
-// {
-//     driver->update_delta();
-// }

src/stopped_state.cpp

@@ -1,12 +1,9 @@
 #include "stopped_state.h"
 #include "motor_controller.h"
 
-//#include <Arduino.h>
-
 #include <zephyr/kernel.h>
 #include <zephyr/sys/printk.h>
 
-
 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();

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<<WGM12);
-//     TIMSK1 |= (1<<OCIE1A);
-//     TCCR1B |= (1<<CS11);
-// }
-
 void Timer_usec::init(uint16_t period_us)
 {
     if (period_us == 0) {

test/test_pi_controller/test_main.cpp

@@ -1,150 +0,0 @@
-#include <unity.h>
-
-// #ifdef ARDUINO
-#include <Arduino.h>
-// #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

zephyr/boards/nucleo_l432kc.overlay

@@ -1,5 +1,6 @@
 #include <zephyr/dt-bindings/gpio/gpio.h>
 #include <zephyr/dt-bindings/pwm/pwm.h>
+#include <zephyr/dt-bindings/pwm/stm32_pwm.h>
 
 / {
     aliases {
@@ -7,11 +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;
-        encoder-isr-probe = &encoder_isr_probe;
     };
 
     buttons {
@@ -41,11 +41,6 @@
     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";
@@ -64,6 +59,14 @@
             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";
+        };
     };
         zephyr,user {
         encoder-isr-probe-gpios = <&gpioa 12 GPIO_ACTIVE_HIGH>; /* D2, PA12 */
@@ -72,11 +75,12 @@
 
 &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";
     };
 };

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