Nýjir kóðar og uppfærðir gamlir kóðar fyrir arduino yfir í zephyr

src/digital_in.cpp

@@ -1,14 +1,64 @@
-#include <avr/io.h>
+//#include <avr/io.h>
 #include "digital_in.h"
 
-Digital_in::Digital_in(uint8_t pin) : pin_mask(1<<pin) {}
+#include <zephyr/devicetree.h>
 
-void Digital_in::init() {
-    DDRD  &= ~pin_mask;
-    PORTD |=  pin_mask;
+// 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);
+
+static const struct gpio_dt_spec encoder_c2_gpio =
+    GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c2), gpios);
+
+Digital_in::Digital_in(uint8_t pin_id)
+    : gpio(nullptr)
+{
+    switch (pin_id) {
+    case ENCODER_C1:
+        gpio = &encoder_c1_gpio;
+        break;
+
+    case ENCODER_C2:
+        gpio = &encoder_c2_gpio;
+        break;
+
+    default:
+        gpio = nullptr;
+        break;
+    }
 }
 
-bool Digital_in::is_hi() { return PIND & pin_mask; }
-bool Digital_in::is_lo() { return !is_hi(); }
+void Digital_in::init()
+{
+    if (gpio == nullptr || !gpio_is_ready_dt(gpio)) {
+        return;
+    }
 
+    // The overlay supplies the pin's GPIO flags.
+    gpio_pin_configure_dt(gpio, GPIO_INPUT);
+}
+
+bool Digital_in::is_hi()
+{
+    if (gpio == nullptr) {
+        return false;
+    }
+
+    // Read the physical pin level: high is true, low is false.
+    return gpio_pin_get_raw(gpio->port, gpio->pin) > 0;
+}
+
+bool Digital_in::is_lo()
+{
+    return !is_hi();
+}
 

src/digital_out.cpp

@@ -1,9 +1,50 @@
 #include "digital_out.h"
-#include <avr/io.h>
 
-Digital_out::Digital_out(uint8_t pin) : pin_mask(1<<pin) {}
+//#include <avr/io.h>
 
-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; }
+#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);
+
+Digital_out::Digital_out(uint8_t pin_id)
+    : gpio(pin_id == STATUS_LED ? &status_led_gpio : nullptr)
+{
+}
+
+void Digital_out::init()
+{
+    if (gpio == nullptr || !gpio_is_ready_dt(gpio)) {
+        return;
+    }
+
+    gpio_pin_configure_dt(gpio, GPIO_OUTPUT_INACTIVE);
+}
+
+void Digital_out::toggle()
+{
+    if (gpio != nullptr) {
+        gpio_pin_toggle_dt(gpio);
+    }
+}
+
+void Digital_out::set_hi()
+{
+    if (gpio != nullptr) {
+        gpio_pin_set_dt(gpio, 1);
+    }
+}
+
+void Digital_out::set_lo()
+{
+    if (gpio != nullptr) {
+        gpio_pin_set_dt(gpio, 0);
+    }
+}

src/gamla_m_ain.cpp

@@ -0,0 +1,125 @@
+// 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>
+#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
+);
+
+
+// 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));
+
+extern "C" void motor_application_main(void)
+{
+    if (!device_is_ready(console_dev)) {
+        printk("Console UART is not ready\n");
+        return;
+    }
+
+    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) {
+        unsigned 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<char>(received_char));
+        }
+
+        motor_controller.update();
+
+        // Give Zephyr's scheduler time to run other threads.
+        k_msleep(1);
+    }
+}

src/initialization_state.cpp

@@ -1,41 +1,61 @@
 #include "initialization_state.h"
 #include "motor_controller.h"
 
-#include <Arduino.h>
+//#include <Arduino.h>
+
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
 
 
 void InitializationState::enter()
 {
-    Serial.println();
-    Serial.println("State: Initialization");
+    // 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 = millis();
+    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)
-    {
+    // // 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();
-        // context->to_operational();
-    }
+    }    
+
 }
 
 
 void InitializationState::command(char c)
 {
+    (void)c;
 }
 
 
 void InitializationState::exit()
 {
-    Serial.println("Boot-up");
+    //Serial.println("Boot-up");
+    printk("Boot-up\n");
 }

src/main.c

@@ -1,5 +1,12 @@
 #include <zephyr/kernel.h>
 
+#ifndef PIO_UNIT_TESTING
+
+void gamla_m_ain(void);
+
 void main(void)
 {
+    gamla_m_ain();
 }
+
+#endif

src/main.cpp

@@ -1,84 +0,0 @@
-// 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 "rotary_encoder_driver.h"
-#include "motor_driver.h"
-#include "pi_controller.h"
-
-#include "motor_controller.h"
-
-#include "initialization_state.h"
-
-#include "pre_operational_state.h"
-
-#include "operational_state.h"
-#include "stopped_state.h"
-
-
-RotaryEncoderDriver encoder;
-
-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;
-
-P_controller p_controller(Kp_P);
-PI_controller pi_controller(Kp, Ti, T);
-
-
-InitializationState initialization_state;
-// Pre-operational velur á milli stýringanna með 'c'
-PreOperationalState pre_operational_state(p_controller, pi_controller);
-OperationalState operational_state;
-StoppedState stopped_state;
-
-// Stöðurnar verða að vera skilgreindar á undan motor_controller
-MotorController motor_controller(
-    encoder,
-    motor,
-    pi_controller,      // Sjálfgefin stýring
-    initialization_state,
-    pre_operational_state,
-    operational_state,
-    stopped_state
-);
-
-
-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 

src/motor_controller.cpp

@@ -1,17 +1,17 @@
 #include "motor_controller.h"
 #include "state.h"
 
-#include <Arduino.h>
+//#include <Arduino.h>
 
 
 MotorController::MotorController(
-    RotaryEncoderDriver& encoder,
-    MotorDriver& motor,
-    P_controller& controller,
-    State& initialization,
-    State& pre_operational,
-    State& operational,
-    State& stopped
+    RotaryEncoderDriver &encoder,
+    MotorDriver &motor,
+    P_controller &controller,
+    State &initialization,
+    State &pre_operational,
+    State &operational,
+    State &stopped
 )
     : current_state(nullptr),
       initialization_state(initialization),
@@ -54,15 +54,13 @@ void MotorController::begin()
 }
 
 
-void MotorController::transition_to(State& new_state)
+void MotorController::transition_to(State &new_state)
 {
-    if (current_state != nullptr)
-    {
+    if (current_state != nullptr) {
         current_state->exit();
     }
 
     current_state = &new_state;
-
     current_state->enter();
 }
 
@@ -109,9 +107,9 @@ bool MotorController::run_speed_controller()
         return false;
     }
 
-    double actual = encoder.rpm();
+    const double actual = encoder.rpm();
 
-    double pwm_percent =
+    const double pwm_percent =
         controller->update(reference_rpm, actual);
 
     double pwm =

src/motor_driver.cpp

@@ -1,40 +1,57 @@
 #include "motor_driver.h"
 
-#include <avr/io.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 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 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 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 gpio_dt_spec motor_flt =
+    GPIO_DT_SPEC_GET(DT_ALIAS(motor_flt), gpios);
+
+static const struct gpio_dt_spec motor_slp =
+    GPIO_DT_SPEC_GET(DT_ALIAS(motor_slp), gpios);
 
-static void ain2_pwm(uint8_t duty)
-{
-    OCR2A = duty;
-    TCCR2A |= (1 << COM2A1);
-}
 
 
 MotorDriver::MotorDriver()
+    : initialized(false)
 {
 }
 
@@ -45,27 +62,51 @@ void MotorDriver::begin()
     // Arduino D11 = PB3 = AIN2
     // Arduino D4  = PD4 = FLT 
 
-    DDRD |= (1 << DDD3);
-    DDRB |= (1 << DDB3);
+    // DDRD |= (1 << DDD3);
+    // DDRB |= (1 << DDB3);
 
-    DDRD &= ~(1 << DDD4); 
+    // DDRD &= ~(1 << DDD4); 
 
-    PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
+    // PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
 
 
-    // Timer2 Fast PWM
-    TCCR2A = 0;
-    TCCR2B = 0;
+    // // Timer2 Fast PWM
+    // TCCR2A = 0;
+    // TCCR2B = 0;
 
-    // Fast PWM mode
-    TCCR2A |= (1 << WGM20);
-    TCCR2A |= (1 << WGM21);
+    // // Fast PWM mode
+    // TCCR2A |= (1 << WGM20);
+    // TCCR2A |= (1 << WGM21);
 
-    // Timer2 prescaler = 32
-    TCCR2B |= (1 << CS21);
-    TCCR2B |= (1 << CS20);
+    // // Timer2 prescaler = 32
+    // TCCR2B |= (1 << CS21);
+    // TCCR2B |= (1 << CS20);
 
+    initialized = false;
+
+    if (!pwm_is_ready_dt(&bin1_pwm) ||
+        !gpio_is_ready_dt(&bin2) ||
+        !gpio_is_ready_dt(&motor_flt) ||
+        !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 ||
+        gpio_pin_configure_dt(&motor_slp, GPIO_OUTPUT_INACTIVE) < 0) {
+        return;
+    }
+
+    // SLP is active high, so drive it high to enable the DRV8833.
+    if (gpio_pin_set_dt(&motor_slp, 1) < 0) {
+        return;
+    }
+
+    if (pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0) < 0) {
+        return;
+    }
     // PWM-úttök eru tengd í set() eftir þörfum
+    initialized = true;
     stop();
 }
 
@@ -85,46 +126,96 @@ void MotorDriver::reverse()
 void MotorDriver::stop()
 {
     // Both LOW
-    ain1_off();
-    ain2_off();
+    // 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);
 }
 
 void MotorDriver::set(double pwm)
 {
+
+    if (!initialized) {
+        return;
+    }
+
     // Limit PWM command
     if (pwm > 255.0)
     {
         pwm = 255.0;
-    }
-
-    if (pwm < -255.0)
-    {
+    } else if (pwm < -255.0) {
         pwm = -255.0;
     }
 
-
-    uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm);
-
-    if (duty == 0)
-    {
+    if (pwm == 0.0) {
         stop();
+        return;
     }
-    else if (pwm > 0)
-    {
-        // Forward
-        ain2_off();
-        ain1_pwm(duty);
-    }
-    else
-    {
-        // Reverse
-        ain1_off();
-        ain2_pwm(duty);
+
+
+    // uint8_t duty = (uint8_t)(pwm > 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<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;
+
+    if (pwm > 0.0) {
+        // Forward: BIN2 low; BIN1 PWM duty equals the command magnitude.
+        gpio_pin_set_dt(&bin2, 0);
+        bin1_pulse = requested_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;
     }
+
+    pwm_set_dt(&bin1_pwm, bin1_pwm.period, bin1_pulse);
+
 }
 
 
 bool MotorDriver::fault_active()
 {
-    return !(PIND & (1 << PIND4));
+    //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;
 }

src/operational_state.cpp

@@ -1,12 +1,15 @@
 #include "operational_state.h"
 #include "motor_controller.h"
 
-#include <Arduino.h>
+//#include <Arduino.h>
+
+#include <zephyr/sys/printk.h>
 
 
 void OperationalState::enter()
 {
-    Serial.println("State: Operational");
+    //Serial.println("State: Operational");
+    printk("State: Operational\n");
 
     context->reset_controller();
 
@@ -15,7 +18,8 @@ void OperationalState::enter()
     sample_count = 0;
 
 #if LOG_CSV
-    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
+    //Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
+    printk("time_ms,ref_rpm,actual_rpm,pwm\n");
 #endif
 }
 
@@ -32,7 +36,8 @@ void OperationalState::update()
 {
     if (context->fault_active())
     {
-        Serial.println("Fault detected");
+        //Serial.println("Fault detected");
+        printk("Fault detected\n");
 
         context->to_stopped();
 
@@ -41,17 +46,22 @@ void OperationalState::update()
 
     if (context->run_speed_controller())
     {
-        sample_count++;
+        ++sample_count;
 
 #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);
+        // 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(),
+            context->get_actual_rpm(),
+            context->get_pwm());
 #endif
     }
 }
@@ -61,14 +71,16 @@ void OperationalState::command(char c)
 {
     if (c == 'r' || c == 'R')
     {
-        Serial.println("Command: reset");
+        //Serial.println("Command: reset");
+        printk("Command: reset\n");
 
         context->to_initialization();
     }
 
     else if (c == 'p' || c == 'P')
     {
-        Serial.println("Command: set pre-operational");
+        //Serial.println("Command: set pre-operational");
+         printk("Command: set pre-operational\n");
 
         context->to_pre_operational();
     }

src/pre_operational_state.cpp

@@ -1,10 +1,17 @@
 #include "pre_operational_state.h"
 #include "motor_controller.h"
 
-#include <Arduino.h>
-#include <stdlib.h>
+#include "p_controller.h"
 
-PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
+//#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),
@@ -19,7 +26,8 @@ PreOperationalState::PreOperationalState(P_controller& p_controller, P_controlle
 
 void PreOperationalState::enter()
 {
-    Serial.println("State: Pre-operational");
+    //Serial.println("State: Pre-operational");
+    printk("State: Pre-operational\n");
 
     context->stop_motor();
 
@@ -27,12 +35,14 @@ void PreOperationalState::enter()
 
     led_state = false;
 
-    previous_time = millis();
+    //previous_time = millis();
+    previous_time = k_uptime_get();
 }
 
 void PreOperationalState::update()
 {
-    unsigned long current_time = millis();
+    //unsigned long current_time = millis();
+    const int64_t current_time = k_uptime_get();
 
     if (current_time - previous_time >= 500)
     {
@@ -65,21 +75,23 @@ void PreOperationalState::command(char c)
                 input_buffer[input_index] = '\0';
 
                 // Convert text to a number
-                double value = atof(input_buffer);
+                const double value = atof(input_buffer);
 
                 if (input_mode == ENTER_KP)
                 {
                     context->set_Kp(value);
 
-                    Serial.print("Kp set to: ");
-                    Serial.println(value);
+                    // Serial.print("Kp set to: ");
+                    // Serial.println(value);
+                    printk("Kp updated\n");
                 }
                 else if (input_mode == ENTER_TI)
                 {
                     context->set_Ti(value);
 
-                    Serial.print("Ti set to: ");
-                    Serial.println(value);
+                    // Serial.print("Ti set to: ");
+                    // Serial.println(value);
+                    printk("Ti updated\n");
                 }
             }
 
@@ -97,8 +109,8 @@ void PreOperationalState::command(char c)
         {
             if (input_index < sizeof(input_buffer) - 1)
             {
-                input_buffer[input_index] = c;
-                input_index++;
+                input_buffer[input_index++] = c;
+                //input_index++;
             }
         }
 
@@ -109,14 +121,16 @@ void PreOperationalState::command(char c)
     // Normal state commands
     if (c == 'o' || c == 'O')
     {
-        Serial.println("Command: set operational");
+        //Serial.println("Command: set operational");
+        printk("Command: set operational\n");
 
         context->to_operational();
     }
 
     else if (c == 'r' || c == 'R')
     {
-        Serial.println("Command: reset");
+        //Serial.println("Command: reset");
+         printk("Command: reset\n");
 
         context->to_initialization();
     }
@@ -126,7 +140,8 @@ void PreOperationalState::command(char c)
         input_mode = ENTER_KP;
         input_index = 0;
 
-        Serial.println("Enter Kp and press Enter:");
+        //Serial.println("Enter Kp and press Enter:");
+        printk("Enter Kp and press Enter:\n");
     }
 
     else if (c == 't' || c == 'T')
@@ -134,7 +149,8 @@ void PreOperationalState::command(char c)
         input_mode = ENTER_TI;
         input_index = 0;
 
-        Serial.println("Enter Ti and press Enter:");
+        //Serial.println("Enter Ti and press Enter:");
+        printk("Enter Ti and press Enter:\n");
     }
 
     // Select controller algorithm, the controller is passed to the motor controller
@@ -145,12 +161,14 @@ void PreOperationalState::command(char c)
         if (pi_selected)
         {
             context->set_controller(pi_controller);
-            Serial.println("Controller: PI");
+            //Serial.println("Controller: PI");
+            printk("Controller: PI\n");
         }
         else
         {
             context->set_controller(p_controller);
-            Serial.println("Controller: P");
+            //Serial.println("Controller: P");
+            printk("Controller: P\n");
         }
     }
 }

src/rotary_encoder_driver.cpp

@@ -1,84 +1,141 @@
 #include "rotary_encoder_driver.h"
-#include <avr/interrupt.h>
 
-static RotaryEncoderDriver* driver = nullptr;
+#include <errno.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/util.h>
+
+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)
-    , eA(2)
-    , eB(5)
+    : count(0),
+      delta(0),
+      speed_ready(false),
+      hist_idx(0)
 {
-    for (uint8_t i = 0; i < HIST_LEN; i++)
-    {
+    for (uint8_t i = 0; i < HIST_LEN; ++i) {
         history[i] = 0;
     }
 }
 
-
-void RotaryEncoderDriver::begin()
+int RotaryEncoderDriver::begin()
 {
-    driver = this;
+    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;
+    }
 
-    EICRA |= (1<<ISC01) | (1<<ISC00);
-    EIMSK |= (1<<INT0);
+    ret = gpio_pin_configure_dt(&encoder_c2, GPIO_INPUT);
+    if (ret < 0) {
+        return ret;
+    }
 
+    gpio_init_callback(
+        &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);
+    if (ret < 0) {
+        return ret;
+    }
+
+    timer.set_callback(timer_expired, this);
     timer.init(SAMPLE_PERIOD_US);
+
+    reset_speed_measurement();
+    return 0;
+}
+
+void RotaryEncoderDriver::gpio_edge_handler(
+    const struct device *dev,
+    struct gpio_callback *callback,
+    uint32_t pins)
+{
+    (void)dev; // This handler does not need the device pointer.
+
+    auto *encoder = CONTAINER_OF(
+        callback,
+        RotaryEncoderDriver,
+        encoder_c1_callback);
+
+    if ((pins & BIT(encoder_c1.pin)) != 0U) {
+        encoder->update();
+    }
+}
+
+void RotaryEncoderDriver::timer_expired(void *context)
+{
+    auto *encoder = static_cast<RotaryEncoderDriver *>(context);
+    encoder->update_delta();
 }
 
 void RotaryEncoderDriver::update()
 {
-    count += eB.is_hi() ? 1 : -1;
+    // 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 unsigned int key = irq_lock();
+    count += direction ? 1 : -1;
+    irq_unlock(key);
 }
 
 int32_t RotaryEncoderDriver::position()
 {
+    const unsigned int key = irq_lock();
+    const int32_t result = count;
+    irq_unlock(key);
 
-    uint8_t oldSREG = SREG;
-
-    cli();
-
-    int32_t local_count = count;
-
-    SREG = oldSREG;
-
-    return local_count;
+    return result;
 }
 
 void RotaryEncoderDriver::update_delta()
 {
+    const unsigned int key = irq_lock();
 
-    int32_t oldest = history[hist_idx];
-
-    history[hist_idx] = count;
+    const int32_t current_count = count;
+    const int32_t oldest_count = history[hist_idx];
 
+    history[hist_idx] = current_count;
     hist_idx = (hist_idx + 1) % HIST_LEN;
 
-    delta = (int16_t)(count - oldest);   // breyting yfir HIST_LEN * Ts (16 ms) glugga
-
+    delta = static_cast<int16_t>(current_count - oldest_count);
     speed_ready = true;
+
+    irq_unlock(key);
 }
 
 float RotaryEncoderDriver::count_per_sec()
 {
+    const unsigned int key = irq_lock();
+    const int16_t local_delta = delta;
+    irq_unlock(key);
 
-    uint8_t oldSREG = SREG;
+    constexpr float window_seconds =
+        HIST_LEN * SAMPLE_PERIOD_US * 1e-6f;
 
-    cli();
-
-    int16_t local_delta = delta;
-
-    SREG = oldSREG;
-
-    return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
+    return local_delta / window_seconds;
 }
 
-
 float RotaryEncoderDriver::rpm()
 {
     return count_per_sec() * 60.0f / COUNTS_PER_REV;
@@ -86,47 +143,29 @@ float RotaryEncoderDriver::rpm()
 
 bool RotaryEncoderDriver::new_speed_available()
 {
-    uint8_t oldSREG = SREG;
+    const unsigned int key = irq_lock();
+    const bool ready = speed_ready;
 
-    cli();
-
-    bool ready = speed_ready;
-
-    if (ready)
-    {
+    if (ready) {
         speed_ready = false;
     }
 
-    SREG = oldSREG;
-
+    irq_unlock(key);
     return ready;
 }
 
-
 void RotaryEncoderDriver::reset_speed_measurement()
 {
-    uint8_t oldSREG = SREG;
-
-    cli();
+    const unsigned int key = irq_lock();
 
     delta = 0;
     speed_ready = false;
 
-    for (uint8_t i = 0; i < HIST_LEN; i++)
-    {
+    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();
-}
+    irq_unlock(key);
+}

src/rotary_encoder_driver_gamli.cpp

@@ -0,0 +1,132 @@
+// #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,7 +1,10 @@
 #include "stopped_state.h"
 #include "motor_controller.h"
 
-#include <Arduino.h>
+//#include <Arduino.h>
+
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
 
 
 StoppedState::StoppedState()
@@ -13,7 +16,9 @@ StoppedState::StoppedState()
 
 void StoppedState::enter()
 {
-    Serial.println("State: Stopped");
+    //Serial.println("State: Stopped");
+
+    printk("State: Stopped\n");
 
     context->stop_motor();
 
@@ -21,14 +26,16 @@ void StoppedState::enter()
 
     context->led_off();
 
-    previous_time = millis();
+    //previous_time = millis();
+    previous_time = k_uptime_get();
 }
 
 // Controlling the blinking of the LED @ 2Hz
 
 void StoppedState::update()
 {
-    unsigned long current_time = millis();
+    //unsigned long current_time = millis();
+    const int64_t current_time = k_uptime_get();
 
     if (current_time - previous_time >= 250)
     {
@@ -54,21 +61,24 @@ void StoppedState::command(char c)
 {
     if (c == 'r' || c == 'R')
     {
-        Serial.println("Command: reset");
+        //Serial.println("Command: reset");
+        printk("Command: reset\n");
 
         context->to_initialization();
     }
 
     else if (c == 'o' || c == 'O')
     {
-        Serial.println("Command: set operational");
+        //Serial.println("Command: set operational");
+        printk("Command: set operational\n");
 
         context->to_operational();
     }
 
     else if (c == 'p' || c == 'P')
     {
-        Serial.println("Command: set pre-operational");
+        //Serial.println("Command: set pre-operational");
+        printk("Command: set pre-operational\n");
 
         context->to_pre_operational();
     }

src/timer_usec.cpp

@@ -1,14 +1,43 @@
 #include "timer_usec.h"
 
-Timer_usec::Timer_usec() = default;
+Timer_usec::Timer_usec() //= default;
+    : callback(nullptr),
+      callback_context(nullptr)
 
-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) { // 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) {
+        return;
+    }
+
+    k_timer_init(&timer, expiry_handler, nullptr);
+    k_timer_user_data_set(&timer, this);
+
+    // The timer fires first after period_us, then repeats every period_us.
+    k_timer_start(&timer, K_USEC(period_us), K_USEC(period_us));
+}
+
+void Timer_usec::expiry_handler(struct k_timer *expired_timer)
+{
+    auto *self =
+        static_cast<Timer_usec *>(k_timer_user_data_get(expired_timer));
+
+    if (self != nullptr && self->callback != nullptr) {
+        self->callback(self->callback_context);
+    }
+}