made encoder work for zephyr

.gitignore

@@ -6,4 +6,44 @@ Thumbs.db
 .vscode/.browse.c_cpp.db*
 .vscode/c_cpp_properties.json
 .vscode/launch.json
-.vscode/ipch
+.vscode/ipch
+.pio
+
+build/CMakeCache.txt
+build/CMakeFiles/cmake.check_cache
+build/CMakeFiles/CMakeOutput.log
+build/CMakeFiles/3.20.21032501-MSVC_2/CMakeCCompiler.cmake
+build/CMakeFiles/3.20.21032501-MSVC_2/CMakeCXXCompiler.cmake
+build/CMakeFiles/3.20.21032501-MSVC_2/CMakeDetermineCompilerABI_C.bin
+build/CMakeFiles/3.20.21032501-MSVC_2/CMakeDetermineCompilerABI_CXX.bin
+build/CMakeFiles/3.20.21032501-MSVC_2/CMakeRCCompiler.cmake
+build/CMakeFiles/3.20.21032501-MSVC_2/CMakeSystem.cmake
+build/CMakeFiles/3.20.21032501-MSVC_2/VCTargetsPath.txt
+build/CMakeFiles/3.20.21032501-MSVC_2/VCTargetsPath.vcxproj
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/CMakeCCompilerId.c
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/CompilerIdC.exe
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/CompilerIdC.vcxproj
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CMakeCCompilerId.obj
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.exe.recipe
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/CL.command.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/CL.read.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/CL.write.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/CompilerIdC.lastbuildstate
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/link.command.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/link.read.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdC/Debug/CompilerIdC.tlog/link.write.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/CMakeCXXCompilerId.cpp
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/CompilerIdCXX.exe
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/CompilerIdCXX.vcxproj
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CMakeCXXCompilerId.obj
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.exe.recipe
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/CL.command.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/CL.read.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/CL.write.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/CompilerIdCXX.lastbuildstate
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/link.command.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/link.read.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/CompilerIdCXX/Debug/CompilerIdCXX.tlog/link.write.1.tlog
+build/CMakeFiles/3.20.21032501-MSVC_2/x64/Debug/VCTargetsPath.recipe
+build/CMakeFiles/3.20.21032501-MSVC_2/x64/Debug/VCTargetsPath.tlog/VCTargetsPath.lastbuildstate
+build/.cmake/api/v1/query/client-vscode/query.json

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_in.h

@@ -1,14 +1,14 @@
 #ifndef DIGITAL_IN_H
 #define DIGITAL_IN_H
 
-#include <stdint.h>
+#include <zephyr/drivers/gpio.h>
 
 class Digital_in
 {
 public:
     // pin is the PORTB bit number:
     // PB0 = D8, PB1 = D9, ..., PB5 = D13
-    Digital_in(uint8_t pin);
+    Digital_in();
 
     void init();
 
@@ -16,7 +16,7 @@ public:
     bool is_lo() const;
 
 private:
-    uint8_t pinMask;
+    struct gpio_dt_spec pin;
 };
 
 #endif

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_out.h

@@ -1,14 +1,14 @@
 #ifndef DIGITAL_OUT_H
 #define DIGITAL_OUT_H
 
-#include <stdint.h>
+#include <zephyr/drivers/gpio.h>
 
 class Digital_out
 {
 public:
     // pin is the PORTB bit number:
     // PB0 = D8, ..., PB5 = D13
-    Digital_out(uint8_t pin);
+    Digital_out();
 
     void init();
 
@@ -17,7 +17,7 @@ public:
     void toggle();
 
 private:
-    uint8_t pinMask;
+    struct gpio_dt_spec pin;
 };
 
 #endif

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/encoder.h

@@ -3,11 +3,12 @@
 
 #include <stdint.h>
 #include <digital_in.h>
+#include <zephyr/drivers/gpio.h>
 
 class Encoder
 {
 public:
-    Encoder(uint8_t c1Pin, uint8_t c2Pin);
+    Encoder();
 
     void init();
     void update();
@@ -15,13 +16,14 @@ public:
     int32_t position() const;
 
 private:
+    static void callback(const struct device *port,struct gpio_callback *cb,uint32_t pins);
+
     Digital_in c1;
     Digital_in c2;
 
-    uint8_t c1Pin;
+    struct gpio_callback gpioCallback;
 
     bool previousC1;
-
     volatile int32_t count;
 };
 

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_in.cpp

@@ -1,26 +1,19 @@
 #include <digital_in.h>
-#include <avr/io.h>
+#define INPUT_NODE DT_ALIAS(sw1)
 
-Digital_in::Digital_in(uint8_t pin)
+Digital_in::Digital_in(): pin(GPIO_DT_SPEC_GET(INPUT_NODE, gpios)){}
-{
-    pinMask = static_cast<uint8_t>(1U << pin);
-}
 
 void Digital_in::init()
 {
-    // Input
+    gpio_pin_configure_dt(&pin, GPIO_INPUT);
-    DDRB &= static_cast<uint8_t>(~pinMask);
-
-    // Internal pull-up
-    PORTB |= pinMask;
 }
 
 bool Digital_in::is_hi() const
 {
-    return (PINB & pinMask) != 0;
+    return gpio_pin_get_dt(&pin) > 0;
 }
 
 bool Digital_in::is_lo() const
 {
-    return (PINB & pinMask) == 0;
+    return gpio_pin_get_dt(&pin) == 0;
 }

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_out.cpp

@@ -1,27 +1,24 @@
 #include <digital_out.h>
-#include <avr/io.h>
+#define LED_NODE DT_ALIAS(led0)
 
-Digital_out::Digital_out(uint8_t pin)
+Digital_out::Digital_out():pin(GPIO_DT_SPEC_GET(LED_NODE, gpios)){}
-{
-    pinMask = static_cast<uint8_t>(1U << pin);
-}
 
 void Digital_out::init()
 {
-    DDRB |= pinMask;
+    gpio_pin_configure_dt(&pin, GPIO_OUTPUT_INACTIVE);
 }
 
 void Digital_out::set_hi()
 {
-    PORTB |= pinMask;
+    gpio_pin_set_dt(&pin, 1);
 }
 
 void Digital_out::set_lo()
 {
-    PORTB &= static_cast<uint8_t>(~pinMask);
+    gpio_pin_set_dt(&pin, 0);
 }
 
 void Digital_out::toggle()
 {
-    PORTB ^= pinMask;
+    gpio_pin_toggle_dt(&pin);
 }

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/encoder.cpp

@@ -1,22 +1,28 @@
 #include <encoder.h>
 
-#include <avr/io.h>
+#include <zephyr/kernel.h>
-#include <avr/interrupt.h>
+#include <zephyr/sys/util.h>
 
-static Encoder* activeEncoder = nullptr;
+#define ENCODER_C1_NODE DT_ALIAS(encoder_c1)
+#define ENCODER_C2_NODE DT_ALIAS(encoder_c2)
 
-ISR(PCINT0_vect)
+static const struct gpio_dt_spec encoderC1 =
+    GPIO_DT_SPEC_GET(ENCODER_C1_NODE, gpios);
+
+static const struct gpio_dt_spec encoderC2 =
+    GPIO_DT_SPEC_GET(ENCODER_C2_NODE, gpios);
+
+
+
+void Encoder::callback(const struct device *port,struct gpio_callback *cb,uint32_t pins)
 {
-    if (activeEncoder != nullptr)
+    Encoder *encoder = CONTAINER_OF(cb, Encoder, gpioCallback);
-    {
+    encoder->update();
-        activeEncoder->update();
-    }
 }
 
-Encoder::Encoder(uint8_t c1Pin, uint8_t c2Pin)
+Encoder::Encoder()
-    : c1(c1Pin),
+    : c1(&encoderC1),
-      c2(c2Pin),
+      c2(&encoderC2),
-      c1Pin(c1Pin),
       previousC1(false),
       count(0)
 {
@@ -33,13 +39,13 @@ void Encoder::init()
     count = 0;
 
     // Clear pending PORTB pin-change interrupt
-    PCIFR |= (1 << PCIF0);
+    gpio_init_callback(&gpioCallback,Encoder::callback,BIT(encoderC1.pin));
 
     // Enable PORTB pin-change interrupts
-    PCICR |= (1 << PCIE0);
+    gpio_add_callback(encoderC1.port, &gpioCallback);
 
     // Enable interrupt on C1
-    PCMSK0 |= (1 << c1Pin);
+    gpio_pin_interrupt_configure_dt(&encoderC1, GPIO_INT_EDGE_BOTH);
 }
 
 void Encoder::update()
@@ -65,11 +71,9 @@ void Encoder::update()
 
 int32_t Encoder::position() const
 {
-    uint8_t oldSREG = SREG;
+    unsigned int key = irq_lock();
-
-    cli();
     int32_t currentCount = count;
-    SREG = oldSREG;
+    irq_unlock(key);
 
     return currentCount;
 }

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/main.cpp

@@ -0,0 +1,190 @@
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/uart.h>
+
+#include <digital_in.h>
+#include <digital_out.h>
+#include <encoder.h>
+
+#include <control_timer.h>
+#include <speed_estimator.h>
+
+#include <motor_driver.h>
+
+#include <p_controller.h>
+#include <speed_controller.h>
+
+#include <context.h>
+#include <initialization.h>
+#include <operational.h>
+#include <stopped.h>
+
+// --------------------------------------------------
+// Hardware / drivers
+// --------------------------------------------------
+
+// Encoder:
+// PB0 = D8
+// PB1 = D9
+Encoder encoder;
+
+// Emergency-stop:
+// PB2 = D10
+// Internal pull-up enabled.
+// LOW = emergency stop pressed.
+Digital_in emergencyStop;
+
+// Status LED:
+// PB5 = D13
+Digital_out statusLED;
+
+Control_timer controlTimer;
+
+Motor_driver motorDriver;
+
+// 2100 counts/revolution,
+// 4 ms fixed controller period
+Speed_estimator speedEstimator(
+    2100.0,
+    0.004
+);
+
+P_controller pController(40.0);
+
+Speed_controller speedController(
+    encoder,
+    speedEstimator,
+    motorDriver,
+    pController
+);
+
+// --------------------------------------------------
+// State machine
+// --------------------------------------------------
+
+Initialization initialization(
+    speedController,
+    statusLED
+);
+
+Operational operational(
+    speedController,
+    statusLED
+);
+
+Stopped stopped(
+    speedController,
+    statusLED
+);
+
+Context* stateMachine = nullptr;
+
+static const struct device* uartDevice =
+    DEVICE_DT_GET(DT_CHOSEN(zephyr_console));
+
+int main()
+{
+
+    if (!device_is_ready(uartDevice))
+    {
+        printk("Console UART is not ready\n");
+        return 0;
+    
+    }
+
+    statusLED.init();
+
+    emergencyStop.init();
+
+    encoder.init();
+
+    motorDriver.init();
+
+    speedController.set_reference(
+        50.0
+    );
+
+    // --------------------------------------------------
+    // State transitions
+    // --------------------------------------------------
+
+    initialization.set_operational_state(
+        operational
+    );
+
+    operational.set_initialization_state(
+        initialization
+    );
+
+    operational.set_stopped_state(
+        stopped
+    );
+
+    stopped.set_initialization_state(
+        initialization
+    );
+
+    static Context context(
+        initialization
+    );
+
+    stateMachine = &context;
+
+    controlTimer.init();
+
+    printk("\n");
+    printk("Project 3 - Part 2\n");
+    printk("Commands:\n");
+    printk("o = set operational\n");
+    printk("r = reset\n");
+    printk("Emergency stop or motor fault = fault\n");
+
+
+    while (true)
+        {
+        // --------------------------------------------------
+        // Emergency-stop fault
+        // --------------------------------------------------
+
+        if (emergencyStop.is_hi()) // was is_lo
+        {
+            stateMachine->fault();
+        }
+
+        if (motorDriver.has_fault())
+        {
+            stateMachine->fault();
+        }
+
+        // --------------------------------------------------
+        // Serial commands
+        // --------------------------------------------------
+
+        unsigned char command;
+
+        if (uart_poll_in(uartDevice, &command) == 0)
+        {
+            if (command == 'o')
+            {
+                stateMachine->
+                    set_operational();
+            }
+            else if (command == 'r')
+            {
+                stateMachine->
+                    reset();
+            }
+        }
+        
+
+        // --------------------------------------------------
+        // Stable 250 Hz state activity
+        // --------------------------------------------------
+
+        if (controlTimer.take_tick())
+        {
+            stateMachine->do_work();
+        }
+        k_sleep(K_MSEC(1));
+    }
+    return 0;
+}

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/motor_driver.cpp

@@ -1,50 +1,27 @@
 #include <motor_driver.h>
+#include <zephyr/drivers/pwm.h>
+#include <zephyr/devicetree.h>
+
+#define AIN1_NODE DT_ALIAS(motor_ain1)
+#define AIN2_NODE DT_ALIAS(motor_ain2)
+
+static const struct pwm_dt_spec ain1 = PWM_DT_SPEC_GET(AIN1_NODE);
+
+static const struct pwm_dt_spec ain2 = PWM_DT_SPEC_GET(AIN2_NODE);
 
-#include <avr/io.h>
 
 void Motor_driver::init()
 {
     currentCommand = 0.0;
 
-    // D11 / PB3 / OC2A -> AIN1
+    enable_pwm_outputs();
-    DDRB |= (1 << PB3);
 
-    // D3 / PD3 / OC2B -> AIN2
+    coast();
-    DDRD |= (1 << PD3);
-
-    PORTB &= ~(1 << PB3);
-    PORTD &= ~(1 << PD3);
-
-    // Timer2 Fast PWM
-    TCCR2A = 0;
-    TCCR2B = 0;
-    TCNT2 = 0;
-
-    OCR2A = 0;
-    OCR2B = 0;
-
-    TCCR2A =
-        (1 << COM2A1) |
-        (1 << COM2B1) |
-        (1 << WGM21) |
-        (1 << WGM20);
-
-    // Prescaler = 8
-    // PWM frequency ~= 7.8 kHz
-    TCCR2B =
-        (1 << CS21);
 }
 
 void Motor_driver::enable_pwm_outputs()
 {
-    // Non-inverting PWM on OC2A and OC2B
+    // The PWM peripheral is configured through devicetree.
-    TCCR2A |=
-        (1 << COM2A1) |
-        (1 << COM2B1);
-
-    TCCR2A &=
-        ~((1 << COM2A0) |
-          (1 << COM2B0));
 }
 
 unsigned char Motor_driver::magnitude(
@@ -83,23 +60,26 @@ void Motor_driver::set_command(double command)
     unsigned char pwm =
         magnitude(command);
 
+
     if (command > 0.0)
     {
         // Same direction that previously gave positive RPM:
         // AIN1 = LOW
         // AIN2 = PWM
-        OCR2A = 0;
+    pwm_set_dt(&ain1, ain1.period, 0);
-        OCR2B = pwm;
+    pwm_set_dt(&ain2, ain2.period,
+        static_cast<uint32_t>(ain2.period * pwm / 255.0));
     }
     else if (command < 0.0)
     {
-        OCR2A = pwm;
+        
-        OCR2B = 0;
+        pwm_set_dt(&ain1, ain1.period,
+            static_cast<uint32_t>(ain1.period * pwm / 255.0));
+        pwm_set_dt(&ain2, ain2.period, 0);
     }
     else
     {
-        OCR2A = 0;
+        coast();
-        OCR2B = 0;
     }
 }
 
@@ -107,26 +87,17 @@ void Motor_driver::coast()
 {
     currentCommand = 0.0;
 
-    enable_pwm_outputs();
+    pwm_set_dt(&ain1, ain1.period, 0);
-
+    pwm_set_dt(&ain2, ain2.period, 0);
-    OCR2A = 0;
-    OCR2B = 0;
 }
 
 void Motor_driver::brake()
 {
     currentCommand = 0.0;
 
-    // Disconnect Timer2 PWM from the pins.
-    TCCR2A &=
-        ~((1 << COM2A1) |
-          (1 << COM2A0) |
-          (1 << COM2B1) |
-          (1 << COM2B0));
-
     // DRV8833: both inputs HIGH gives braking.
-    PORTB |= (1 << PB3);
+    pwm_set_dt(&ain1, ain1.period, ain1.period);
-    PORTD |= (1 << PD3);
+    pwm_set_dt(&ain2, ain2.period, ain2.period);
 }
 
 double Motor_driver::command() const

Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/boards/nucleo_l432kc.overlay

@@ -39,12 +39,12 @@
 
     motor {
         motor_ain1: ain1 {
-            pwms = <&pwm1 1 PWM_MSEC(20) PWM_POLARITY_NORMAL>;
+            pwms = <&pwm1 1 PWM_HZ(8000) PWM_POLARITY_NORMAL>;
             label = "Motor AIN1";
         };
 
         motor_ain2: ain2 {
-            pwms = <&pwm2 1 PWM_MSEC(20) PWM_POLARITY_NORMAL>;
+            pwms = <&pwm2 1 PWM_HZ(8000) PWM_POLARITY_NORMAL>;
             label = "Motor AIN2";
         };