bætti aftur við lyklaborðinu til þess að geta prófað allt saman

Allt virkar upp að vissu marki.

.gitattributes

@@ -0,0 +1,2 @@
+* text=auto eol=lf
+*.bat text eol=crlf

.gitignore

@@ -0,0 +1,8 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
+scripts
+__pycache__/
+*.typ

.vscode/extensions.json

@@ -0,0 +1,11 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack",
+        "pioarduino.pioarduino-ide"
+    ]
+}

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "C:/Users/helgi/H26/ESP/Projects/ESP_Project4_H26/zephyr"
+}

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

docs/diagrams/src/P4-architecture.wsd

@@ -0,0 +1,80 @@
+@startuml P4-architecture
+' Project 4, Part 2.2: Layers of the reduced CANopen node (Node ID 1).
+' Arrows point from the user to the thing it uses.
+
+' skinparam packageStyle rectangle
+skinparam classAttributeIconSize 0
+hide empty members
+
+package "Application layer (uses OSI 7)" {
+    class MotorController {
+        + to_operational()
+        + to_pre_operational()
+        + to_stopped()
+    }
+    abstract class State
+    class SetpointGenerator
+    class PI_controller
+}
+
+package "CANopen layer (OSI 5-7)" {
+    class CanopenNode {
+        + on_frame(frame)
+        + poll(now_ms)
+    }
+    class SdoServer
+    class ObjectDictionary
+    interface NodeApplication {
+        + on_nmt_command(cmd)
+        + set_target_velocity(rpm)
+        + get_velocity() : rpm
+        + get_duty() : 0.1 %
+    }
+    interface CanBus {
+        + send(frame)
+    }
+}
+
+package "Platform layer (Zephyr)" {
+    class ZephyrCanBus
+    class MotorDriver
+    class RotaryEncoderDriver
+}
+
+package "Hardware (OSI 1-2)" {
+    class "CAN controller + transceiver" as CAN
+}
+
+MotorController -> State
+MotorController --> SetpointGenerator
+MotorController --> PI_controller
+MotorController ..|> NodeApplication
+MotorController --> MotorDriver
+MotorController --> RotaryEncoderDriver
+
+CanopenNode --> SdoServer
+SdoServer --> ObjectDictionary
+CanopenNode --> ObjectDictionary
+CanopenNode --> NodeApplication
+CanopenNode --> CanBus
+
+ZephyrCanBus ..|> CanBus
+ZephyrCanBus --> CAN
+
+note top of CanopenNode
+  NMT (0x000), boot-up (0x701),
+  RPDO1 (0x201), TPDO1 (0x181)
+end note
+
+note left of SdoServer
+  0x601 / 0x581, expedited only
+  abort codes 0x06020000, 0x08000022
+end note
+
+note right of ObjectDictionary
+  0x1000, 0x2000-0x2010, 0x606C,
+  0x6083, 0x6084, 0x60FF
+  access rules, little-endian packing
+end note
+
+@enduml

docs/diagrams/src/P4-class-diagram.wsd

@@ -0,0 +1,29 @@
+@startuml 
+' skinparam linetype ortho
+' skinparam nodesep 100
+skinparam ranksep 80
+
+skinparam state {
+    BackgroundColor LightBlue
+}
+
+' hide empty members
+
+Application <|-- CANopen
+
+class Application {
+    {field} state machine
+    {field} controllers
+    {field} setpoint generator
+    {method} hello world
+}
+
+CANopen : Zephyr
+
+Class01 <-- Class02
+Class03 *-- Class04
+Class05 o-- Class06
+Class07 .. Class08
+Class09 -- Class10
+
+@enduml

include/README

@@ -0,0 +1,37 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the convention is to give header files names that end with `.h'.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

include/digital_in.h

@@ -0,0 +1,31 @@
+#ifndef DIGITAL_IN_H
+#define DIGITAL_IN_H
+
+#include <stdint.h>
+#include <zephyr/drivers/gpio.h>
+
+class Digital_in {
+public:
+    enum : uint8_t {
+        ENCODER_C1 = 0,
+        ENCODER_C2 = 1
+    };
+
+    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

@@ -0,0 +1,23 @@
+#ifndef DIGITAL_OUT_H
+#define DIGITAL_OUT_H
+
+#include<stdint.h>
+#include <zephyr/drivers/gpio.h>
+
+struct Digital_out {
+    enum : uint8_t {
+        STATUS_LED = 0
+    };
+
+    explicit Digital_out(uint8_t pin_id);
+
+    void init();
+    void toggle();
+    void set_hi();
+    void set_lo();
+
+private:
+    const struct gpio_dt_spec *gpio;
+};
+
+#endif

include/initialization_state.h

@@ -0,0 +1,29 @@
+#ifndef INITIALIZATION_STATE_H
+#define INITIALIZATION_STATE_H
+
+#include <stdint.h>
+
+#include "state.h"
+
+class InitializationState : public State
+{
+private:
+
+
+    static constexpr int64_t WAIT_MS = 2000;
+    int64_t enter_time_ms = 0;
+
+
+public:
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+
+    NodeState id() const override { return NodeState::INITIALIZATION; }
+};
+
+#endif

include/motor_controller.h

@@ -0,0 +1,123 @@
+#ifndef MOTOR_CONTROLLER_H
+#define MOTOR_CONTROLLER_H
+
+#include <stdint.h>
+
+#include "rotary_encoder_driver.h"
+#include "motor_driver.h"
+#include "p_controller.h"
+#include "digital_out.h"
+#include "node_types.h"
+#include "node_application.h"
+
+class State;
+
+class MotorController : public NodeApplication
+{
+private:
+    static constexpr double DEFAULT_TARGET_RPM = 0.0;
+    static constexpr double PWM_PER_PERCENT = 255.0 / 100.0;
+    static constexpr uint8_t LED_PIN = Digital_out::STATUS_LED;
+
+    State* current_state;
+
+    State& initialization_state;
+    State& pre_operational_state;
+    State& operational_state;
+    State& stopped_state;
+
+    RotaryEncoderDriver &encoder;
+    MotorDriver &motor;
+
+    P_controller &p_controller;
+    P_controller &pi_controller;
+    P_controller *controller;   // Bendir á p_controller eða pi_controller
+
+    Digital_out led;
+
+    double target_rpm;  // Það sem beðið er um (RPDO1)
+    double reference_rpm;   // Það sem stýringin fær
+
+    uint32_t profile_acceleration; // RPM/s
+    uint32_t profile_deceleration;
+
+    // Síðasta sýni, notað fyrir CSV-útprentun
+    double last_actual_rpm;
+    double last_pwm;
+
+
+
+
+public:
+    MotorController(
+        RotaryEncoderDriver &encoder,
+        MotorDriver &motor,
+        P_controller &p_controller,
+        P_controller &pi_controller,
+        State &initialization,
+        State &pre_operational,
+        State &operational,
+        State &stopped
+    );
+
+
+    void begin();
+
+    void update();
+
+    void command(char c);
+
+    void transition_to(State &new_state);
+
+
+    void to_initialization() override;
+
+    void to_pre_operational() override;
+
+    void to_operational() override;
+
+    void to_stopped() override;
+
+    NodeState get_state() const override;
+
+    bool fault_active();
+
+
+    // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna
+    bool run_speed_controller();
+
+    void set_target_rpm(double rpm) override { target_rpm = rpm; }
+    double get_target_rpm() const override { return target_rpm; }
+    double get_reference_rpm() const { return reference_rpm; }
+    double get_actual_rpm() const override { return last_actual_rpm; }
+
+    double get_pwm() const { return last_pwm; }
+    double get_duty_percent() const override { return last_pwm / PWM_PER_PERCENT; }
+
+    void set_profile_acceleration(uint32_t value) override {profile_acceleration = value; }
+    uint32_t get_profile_acceleration() const override { return profile_acceleration; }
+    void set_profile_deceleration(uint32_t value) override {profile_deceleration = value; }
+    uint32_t get_profile_deceleration() const override { return profile_deceleration; }
+
+    void stop_motor();
+
+    void reset_speed_measurement();
+
+    void reset_controller();
+    void reset_parameters() override;
+
+    void set_Kp(double value) override;
+    double get_Kp() const override { return controller->get_Kp(); }
+
+    bool set_Ti(double value) override;
+    double get_Ti() const override { return controller->get_Ti(); }
+
+    void select_controller(ControllerType type) override;
+    ControllerType get_controller_type() const override;
+
+    void led_on();
+
+    void led_off();
+};
+
+#endif

include/motor_driver.h

@@ -0,0 +1,25 @@
+#ifndef MOTOR_DRIVER_H
+#define MOTOR_DRIVER_H
+
+class MotorDriver
+{
+public:
+    MotorDriver();
+
+    void begin();
+
+    void forward();
+
+    void reverse();
+
+    void stop();
+
+    void set(double pwm);
+
+    bool fault_active();   // Fault detection for part 2
+
+private:
+    bool initialized; // Flag that ignores motor commands if setup fails
+};
+
+#endif

include/node_application.h

@@ -0,0 +1,51 @@
+#ifndef NODE_APPLICATION_H
+#define NODE_APPLICATION_H
+
+#include <stdint.h>
+#include "node_types.h"
+
+class NodeApplication
+{
+public:
+    virtual ~NodeApplication() {}
+
+    // NMT
+    virtual void to_initialization() = 0;
+    virtual void to_pre_operational() = 0;
+    virtual void to_operational() = 0;
+    virtual void to_stopped() = 0;
+    virtual void reset_parameters() = 0;
+    virtual NodeState get_state() const = 0;
+
+    // 0x2000
+    virtual void select_controller(ControllerType type) = 0;
+    virtual ControllerType get_controller_type() const = 0;
+
+    // 0x2001
+    virtual void set_Kp(double value) = 0;
+    virtual double get_Kp() const = 0;
+
+    // 0x2002
+    virtual bool set_Ti(double value) = 0;
+    virtual double get_Ti() const = 0;
+
+    // 0x2010
+    virtual double get_duty_percent() const = 0;
+
+    // 0x606C
+    virtual double get_actual_rpm() const = 0;
+
+    // 0x6083
+    virtual void set_profile_acceleration(uint32_t value) = 0;
+    virtual uint32_t get_profile_acceleration() const = 0;
+    // 0x6084
+    virtual void set_profile_deceleration(uint32_t value) = 0;
+    virtual uint32_t get_profile_deceleration() const = 0;
+
+    // 0x60FF (RPDO1)
+    virtual void set_target_rpm(double value) = 0;
+    virtual double get_target_rpm() const = 0;
+
+};
+
+#endif

include/node_types.h

@@ -0,0 +1,18 @@
+#ifndef NODE_TYPES_H
+#define NODE_TYPES_H
+
+enum class ControllerType
+{
+    P,
+    PI
+};
+
+enum class NodeState
+{
+    INITIALIZATION,
+    PRE_OPERATIONAL,
+    OPERATIONAL,
+    STOPPED
+};
+
+#endif 

include/operational_state.h

@@ -0,0 +1,31 @@
+#ifndef OPERATIONAL_STATE_H
+#define OPERATIONAL_STATE_H
+
+#include "state.h"
+#include <stdint.h>
+
+// Sett á 1 til að prenta CSV (time_ms,ref_rpm,actual_rpm,pwm) í Operational,
+// t.d. fyrir step og load response. Einnig hægt með build_flags = -D LOG_CSV=1
+#ifndef LOG_CSV
+#define LOG_CSV 0
+#endif
+
+
+class OperationalState : public State
+{
+private:
+    uint32_t sample_count = 0;
+
+public:
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+
+    NodeState id() const override { return NodeState::OPERATIONAL; }
+};
+
+#endif

include/p_controller.h

@@ -0,0 +1,32 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+class P_controller
+{
+protected:
+    double Kp;
+    double default_Kp;  // Gildið úr constructornum, notað af restore_defaults()
+
+public:
+    P_controller(double Kp);
+    virtual ~P_controller() {}
+    virtual double update(double ref, double actual);
+    virtual void reset() {}
+    virtual void restore_defaults(); //
+    virtual double get_Kp() const { return Kp; }
+    virtual double get_Ti() const { return 0.0; }
+
+    virtual void set_Kp(double value)
+    {
+        Kp = value;
+    }
+
+    virtual bool set_Ti(double value)
+    {
+        // P controller does not use Ti
+        (void)value;
+        return true;
+    }
+};
+
+#endif

include/pi_controller.h

@@ -0,0 +1,26 @@
+#ifndef PI_CONTROLLER_H
+#define PI_CONTROLLER_H
+
+#include "p_controller.h"
+
+class PI_controller : public P_controller
+{
+    private:
+        static constexpr double U_MAX = 100.0;  // %
+        static constexpr double U_MIN = -100.0; // %
+
+        double Ti;
+        double default_Ti;
+        double T;
+        double integral;
+    public:
+        PI_controller(double Kp, double Ti, double T);
+        double update(double ref, double actual) override;
+        void reset() override;
+        void restore_defaults() override;
+
+        bool set_Ti(double value) override;
+        double get_Ti() const override { return Ti; }
+};
+
+#endif

include/pre_operational_state.h

@@ -0,0 +1,45 @@
+#ifndef PRE_OPERATIONAL_STATE_H
+#define PRE_OPERATIONAL_STATE_H
+
+#include "state.h"
+#include "stdint.h"
+
+// class P_controller;
+
+class PreOperationalState : public State
+{
+private:
+    // P_controller &p_controller;
+    // P_controller &pi_controller;
+    // bool pi_selected;
+
+    int64_t previous_time;
+    bool led_state;
+
+
+    enum InputMode
+    {
+        NONE,
+        ENTER_KP,
+        ENTER_TI,
+        ENTER_RPM
+    };
+
+    InputMode input_mode;
+
+    char input_buffer[16];
+    uint8_t input_index;
+
+public:
+    // PreOperationalState(P_controller& p_controller, P_controller& pi_controller);
+    PreOperationalState();
+
+    void enter() override;
+    void update() override;
+    void command(char c) override;
+    void exit() override;
+
+    NodeState id() const override { return NodeState::PRE_OPERATIONAL; }
+};
+
+#endif

include/rotary_encoder_driver.h

@@ -0,0 +1,58 @@
+#ifndef ROTARY_ENCODER_DRIVER_H
+#define ROTARY_ENCODER_DRIVER_H
+
+#include <stdint.h>
+#include <zephyr/drivers/gpio.h>
+#include <zephyr/kernel.h>
+
+#include "digital_in.h"
+#include "timer_usec.h"
+
+class RotaryEncoderDriver {
+public:
+    static constexpr uint16_t SAMPLE_PERIOD_US = 4000;   // Ts = 4 ms
+    static constexpr float COUNTS_PER_REV = 1050.0f;
+
+    RotaryEncoderDriver();
+
+    // Returns 0 on success, or a negative Zephyr error code.
+    int begin();
+
+    void update();
+    int32_t position();
+    void update_delta();
+    float count_per_sec();
+    float rpm();
+    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
+
+    volatile int32_t count;
+    volatile int16_t delta;
+    volatile bool speed_ready;
+
+    volatile int32_t history[HIST_LEN];
+    volatile uint8_t hist_idx;
+
+    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(
+        const struct device *dev,
+        struct gpio_callback *callback,
+        uint32_t pins);
+
+    static void timer_expired(void *context);
+};
+
+#endif

include/state.h

@@ -0,0 +1,34 @@
+#ifndef STATE_H
+#define STATE_H
+
+#include "node_types.h"
+
+class MotorController;
+
+class State
+{
+protected:
+    MotorController* context = nullptr;
+
+public:
+    virtual ~State()
+    {
+    }
+
+    void set_context(MotorController* context)
+    {
+        this->context = context;
+    }
+
+    virtual void enter() = 0;   // LED ON
+
+    virtual void update() = 0;  // run motor controller
+
+    virtual void command(char c) = 0;   // Listen for reset
+
+    virtual void exit() = 0;  // Stop motor
+
+    virtual NodeState id() const = 0; //
+};
+
+#endif

include/stopped_state.h

@@ -0,0 +1,28 @@
+#ifndef STOPPED_STATE_H
+#define STOPPED_STATE_H
+
+#include <stdint.h>
+
+#include "state.h"
+
+class StoppedState : public State
+{
+private:
+    int64_t previous_time;
+    bool led_state;
+
+public:
+    StoppedState();
+
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+
+    NodeState id() const override { return NodeState::STOPPED; }
+};
+
+#endif

include/timer_usec.h

@@ -0,0 +1,24 @@
+#ifndef TIMER_USEC_H
+#define TIMER_USEC_H
+
+#include <stdint.h>
+#include <zephyr/kernel.h>
+
+class Timer_usec {
+public:
+    using callback_t = void (*)(void *context);
+
+    Timer_usec();
+
+    void set_callback(void (*callback)(void *), void *user_data);
+    void init(uint16_t period_us);
+
+private:
+    struct k_timer timer;
+    callback_t callback;
+    void *callback_context;
+
+    static void expiry_handler(struct k_timer *timer);
+};
+
+#endif

lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into the executable file.
+
+The source code of each library should be placed in a separate directory
+("lib/your_library_name/[Code]").
+
+For example, see the structure of the following example libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+Example contents of `src/main.c` using Foo and Bar:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+The PlatformIO Library Dependency Finder will find automatically dependent
+libraries by scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

pi/can_up.sh

@@ -0,0 +1,8 @@
+#!/bin/sh
+# Brings up can0 at the same bitrate as the Nucleo (125000 in src/CAN_FRAME_TEST.c and src/can_send_test.c).
+# Usage: ./can_up.sh [bitrate]   (default 125000)
+BITRATE=${1:-125000}
+
+sudo ip link set can0 down 2>/dev/null
+sudo ip link set can0 up type can bitrate "$BITRATE"
+ip -details -statistics link show can0

platformio.ini

@@ -0,0 +1,41 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:nucleo_l432kc]
+platform = ststm32
+board = nucleo_l432kc
+framework = zephyr
+
+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
+
+; Part 2.1: CAN bring-up test (src/can_send_test.c) instead of the motor app.
+; Build/upload: pio run -e can_test -t upload
+; [env:can_test]
+; extends = env:nucleo_l432kc
+; build_flags = -D CAN_SEND_TEST=1
+
+; L4.4 example (src/CAN_FRAME_TEST.c) instead of the motor app.
+; Build/upload: pio run -e can_frame_test -t upload
+; [env:can_frame_test]
+; extends = env:nucleo_l432kc
+; build_flags = -D CAN_FRAME_TEST=1
+
+; CAN reception with a filter (src/can_rx_test.c) instead of the motor app.
+; Build/upload: pio run -e can_rx_test -t upload
+; [env:can_rx_test]
+; extends = env:nucleo_l432kc
+; build_flags = -D CAN_RX_TEST=1

src/CAN_FRAME_TEST.c

@@ -0,0 +1,49 @@
+#include <zephyr/device.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/can.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
+
+/* Devicetree node selected by zephyr,canbus in the overlay */
+#define CANBUS_NODE DT_CHOSEN(zephyr_canbus)
+
+// L4.4 example. Built only with the can_frame_test environment
+// (-D CAN_FRAME_TEST=1); main() in main.c calls this function.
+void can_frame_test_main(void)
+{
+    const struct device *dev = DEVICE_DT_GET(CANBUS_NODE);
+
+    if (!device_is_ready(dev)) {
+        printk("CAN device not ready\n");
+        return;
+    }
+
+    int err = can_set_bitrate(dev, 125000);
+    if (err != 0) {
+        printk("Error setting CAN bitrate (err %d)\n", err);
+        return;
+    }
+
+    err = can_start(dev);
+    if (err != 0) {
+        printk("Error starting CAN controller (err %d)\n", err);
+        return;
+    }
+
+    struct can_frame frame = {
+        .flags = 0,
+        .id = 0x456,
+        .dlc = 4,
+        .data = {1, 2, 3, 4, 5, 6, 7, 8},
+    };
+
+    while (1) {
+        k_msleep(500);
+
+        err = can_send(dev, &frame, K_MSEC(100), NULL, NULL);
+        if (err != 0) {
+            printk("Sending failed (err %d)\n", err);
+            return;
+        }
+    }
+}

src/app.cpp

@@ -0,0 +1,125 @@
+
+#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"
+
+#include "node_application.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;
+static OperationalState operational_state;
+static StoppedState stopped_state;
+
+// Stöðurnar verða að vera skilgreindar á undan motor_controller
+static MotorController motor_controller(
+    encoder,
+    motor,
+    p_controller,
+    pi_controller,      // Sjálfgefin stýring
+    initialization_state,
+    pre_operational_state,
+    operational_state,
+    stopped_state
+);
+
+
+static const struct device *const console_dev =
+    DEVICE_DT_GET(DT_CHOSEN(zephyr_console));
+
+// Received characters are queued by the UART ISR, like the RX buffer that
+// Arduino's Serial had. The USART has a single receive register, so
+// polling it from the main loop loses characters that arrive in a burst.
+K_MSGQ_DEFINE(rx_queue, sizeof(char), 64, 1);
+
+static void uart_rx_isr(const struct device *dev, void *user_data)
+{
+    (void)user_data;
+
+    if (!uart_irq_update(dev)) {
+        return;
+    }
+
+    while (uart_irq_rx_ready(dev)) {
+        unsigned char c;
+
+        if (uart_fifo_read(dev, &c, 1) != 1) {
+            break;
+        }
+
+        // Drop the character if the queue is full.
+        (void)k_msgq_put(&rx_queue, &c, K_NO_WAIT);
+    }
+}
+
+extern "C" void motor_application_main(void)
+{
+    if (!device_is_ready(console_dev)) {
+        printk("Console UART is not ready\n");
+        return;
+    }
+
+    uart_irq_callback_user_data_set(console_dev, uart_rx_isr, nullptr);
+    uart_irq_rx_enable(console_dev);
+
+    const int encoder_result = encoder.begin();
+    if (encoder_result < 0) {
+        printk("Encoder setup failed: %d\n", encoder_result);
+        return;
+    }
+
+    motor.begin();
+    motor_controller.begin();
+
+    while (true) {
+        char received_char;
+
+        // Handle every character the ISR has queued since the last pass.
+        while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) {
+            motor_controller.command(received_char);
+        }
+
+        motor_controller.update();
+
+        static NodeState last_state = NodeState::INITIALIZATION;
+
+        const NodeState now = motor_controller.get_state();
+        if (now != last_state)
+        {
+            printk("get_state: %d -> %d\n", (int)last_state, (int)now);
+            last_state = now;
+        }
+
+        // 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));
+    }
+}

src/can_rx_test.c

@@ -0,0 +1,80 @@
+// Part 2.1: CAN reception with a filter. Only frames with id RX_CAN_ID are
+// accepted. The driver puts them in a message queue from interrupt context,
+// and the main loop takes them out and prints them.
+// Built only with the can_rx_test environment (-D CAN_RX_TEST=1).
+//
+// Test from the Pi:
+//   cansend can0 321#DEADBEEF   -> printed by the Nucleo
+//   cansend can0 322#DEADBEEF   -> rejected by the filter, nothing printed
+
+#include <zephyr/device.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/can.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
+
+#define CANBUS_NODE  DT_CHOSEN(zephyr_canbus)
+#define CAN_BITRATE  125000  // Must match the bitrate of can0 on the Pi
+#define RX_CAN_ID    0x321   // The only id this node accepts
+
+// Queue for up to 10 received frames. The CAN driver fills it in the
+// receive interrupt; nothing else runs in interrupt context.
+CAN_MSGQ_DEFINE(rx_msgq, 10);
+
+void can_rx_test_main(void)
+{
+    const struct device *dev = DEVICE_DT_GET(CANBUS_NODE);
+
+    if (!device_is_ready(dev)) {
+        printk("CAN device not ready\n");
+        return;
+    }
+
+    int err = can_set_bitrate(dev, CAN_BITRATE);
+    if (err != 0) {
+        printk("Error setting CAN bitrate (err %d)\n", err);
+        return;
+    }
+
+    // Accept only RX_CAN_ID: all 11 bits of the id must match the mask.
+    const struct can_filter filter = {
+        .id = RX_CAN_ID,
+        .mask = CAN_STD_ID_MASK,
+#ifdef CAN_FILTER_DATA
+        // Older Zephyr versions must be told to match data frames.
+        .flags = CAN_FILTER_DATA,
+#else
+        .flags = 0,
+#endif
+    };
+
+    // Returns a filter id (>= 0) on success.
+    int filter_id = can_add_rx_filter_msgq(dev, &rx_msgq, &filter);
+    if (filter_id < 0) {
+        printk("Error adding RX filter (err %d)\n", filter_id);
+        return;
+    }
+
+    err = can_start(dev);
+    if (err != 0) {
+        printk("Error starting CAN controller (err %d)\n", err);
+        return;
+    }
+
+    printk("CAN RX test: %d bit/s, accepting id 0x%03X\n",
+           CAN_BITRATE, RX_CAN_ID);
+
+    struct can_frame frame;
+
+    while (1) {
+        // Sleeps until a frame is in the queue, then processes it here in
+        // the main thread, where printing is allowed.
+        k_msgq_get(&rx_msgq, &frame, K_FOREVER);
+
+        printk("Received id 0x%03X [%d]:", frame.id, frame.dlc);
+        for (int i = 0; i < can_dlc_to_bytes(frame.dlc); i++) {
+            printk(" %02X", frame.data[i]);
+        }
+        printk("\n");
+    }
+}

src/can_send_test.c

@@ -0,0 +1,61 @@
+// Part 2.1: CAN bring-up. Sends one frame every 100 ms so it can be seen
+// on the oscilloscope and with `candump can0` on the Raspberry Pi.
+// Built only with the can_test environment (-D CAN_SEND_TEST=1).
+
+#include <zephyr/device.h>
+#include <zephyr/drivers/can.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
+
+// Change these to see the difference in candump.
+#define TEST_CAN_ID   0x123   // 11-bit standard id (0x000 - 0x7FF)
+#define TEST_CAN_DLC  8       // Number of data bytes (0 - 8)
+#define CAN_BITRATE   125000  // Must match the bitrate of can0 on the Pi
+
+static const struct device *const can_dev = DEVICE_DT_GET(DT_NODELABEL(can1));
+
+void can_send_test_main(void)
+{
+    if (!device_is_ready(can_dev)) {
+        printk("CAN device is not ready\n");
+        return;
+    }
+
+    int err = can_set_bitrate(can_dev, CAN_BITRATE);
+    if (err != 0) {
+        printk("can_set_bitrate failed: %d\n", err);
+        return;
+    }
+
+    err = can_start(can_dev);
+    if (err != 0) {
+        printk("can_start failed: %d\n", err);
+        return;
+    }
+
+    printk("CAN started at %d bit/s, sending id 0x%03X\n",
+           CAN_BITRATE, TEST_CAN_ID);
+
+    struct can_frame frame = {
+        .id = TEST_CAN_ID,
+        .dlc = TEST_CAN_DLC,
+        .flags = 0,           // Standard id, data frame
+    };
+    uint8_t counter = 0;
+
+    while (true) {
+        // Byte 0 counts up so you can see new frames arriving.
+        frame.data[0] = counter++;
+        for (int i = 1; i < TEST_CAN_DLC; i++) {
+            frame.data[i] = i;
+        }
+
+        // Blocks until the frame is sent (and acknowledged by another node).
+        err = can_send(can_dev, &frame, K_MSEC(100), NULL, NULL);
+        if (err != 0) {
+            printk("can_send failed: %d\n", err);
+        }
+
+        k_sleep(K_MSEC(100));
+    }
+}

src/digital_in.cpp

@@ -0,0 +1,74 @@
+#include "digital_in.h"
+
+#include <errno.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/sys/util.h>
+
+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;
+    }
+}
+
+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();
+}
+
+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

@@ -0,0 +1,41 @@
+#include "digital_out.h"
+
+#include <zephyr/devicetree.h>
+
+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/initialization_state.cpp

@@ -0,0 +1,35 @@
+#include "initialization_state.h"
+#include "motor_controller.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
+
+void InitializationState::enter()
+{
+    printk("\nState: Initialization\n");
+
+    context->led_off();
+    context->stop_motor();
+    context->reset_speed_measurement();
+
+    enter_time_ms = k_uptime_get();
+}
+
+
+void InitializationState::update()
+{
+    // Check the time without blocking the application.
+    if ((k_uptime_get() - enter_time_ms) >= WAIT_MS) {
+        context->to_pre_operational();
+    }
+}
+
+void InitializationState::command(char c)
+{
+    (void)c;
+} 
+
+void InitializationState::exit()
+{
+    printk("Boot-up\n");
+}

src/main.c

@@ -0,0 +1,20 @@
+#include <zephyr/kernel.h>
+
+void motor_application_main(void);
+void can_send_test_main(void);
+void can_frame_test_main(void);
+void can_rx_test_main(void);
+
+int main(void)
+{
+#if CAN_SEND_TEST
+    can_send_test_main();
+#elif CAN_FRAME_TEST
+    can_frame_test_main();
+#elif CAN_RX_TEST
+    can_rx_test_main();
+#else
+    motor_application_main();
+#endif
+    return 0;
+}

src/motor_controller.cpp

@@ -0,0 +1,223 @@
+#include "motor_controller.h"
+#include "state.h"
+
+MotorController::MotorController(
+    RotaryEncoderDriver &encoder,
+    MotorDriver &motor,
+    P_controller &p_controller,
+    P_controller &pi_controller,
+    State &initialization,
+    State &pre_operational,
+    State &operational,
+    State &stopped
+)
+    : current_state(nullptr),
+      initialization_state(initialization),
+      pre_operational_state(pre_operational),
+      operational_state(operational),
+      stopped_state(stopped),
+      encoder(encoder),
+      motor(motor),
+      p_controller(p_controller),
+      pi_controller(pi_controller),
+      controller(&pi_controller),   // PI er sjálfgefin stýring
+      led(LED_PIN),
+      target_rpm(DEFAULT_TARGET_RPM),
+      reference_rpm(DEFAULT_TARGET_RPM),
+      profile_acceleration(0),
+      profile_deceleration(0),
+      last_actual_rpm(0.0),
+      last_pwm(0.0)
+{
+    initialization_state.set_context(this);
+    pre_operational_state.set_context(this);
+    operational_state.set_context(this);
+    stopped_state.set_context(this);
+}
+
+
+void MotorController::begin()
+{
+    led.init();
+    led.set_lo();
+
+    transition_to(initialization_state);
+}
+
+
+void MotorController::transition_to(State &new_state)
+{
+    if (current_state != nullptr) {
+        current_state->exit();
+    }
+
+    current_state = &new_state;
+    current_state->enter();
+}
+
+
+void MotorController::update()
+{
+    if (current_state != nullptr)
+    {
+        current_state->update();
+    }
+}
+
+void MotorController::command(char c)
+{
+    if (current_state != nullptr)
+    {
+        current_state->command(c);
+    }
+}
+
+void MotorController::to_initialization()
+{
+    transition_to(initialization_state);
+}
+
+void MotorController::to_pre_operational()
+{
+    transition_to(pre_operational_state);
+}
+
+
+void MotorController::to_operational()
+{
+    transition_to(operational_state);
+}
+
+
+bool MotorController::run_speed_controller()
+{
+    if (!encoder.new_speed_available())
+    {
+        return false;
+    }
+
+    reference_rpm = target_rpm;
+
+    const double actual = encoder.rpm();
+
+    const double pwm_percent =
+        controller->update(reference_rpm, actual);
+
+    double pwm =
+        pwm_percent * PWM_PER_PERCENT;
+
+    // Klippt hér svo last_pwm sé nákvæmlega það sem fer á mótorinn
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+    else if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+    motor.set(pwm);
+
+    last_actual_rpm = actual;
+    last_pwm = pwm;
+
+    return true;
+}
+
+
+void MotorController::stop_motor()
+{
+    motor.stop();
+}
+
+
+void MotorController::reset_speed_measurement()
+{
+    encoder.reset_speed_measurement();
+}
+
+void MotorController::reset_controller()
+{
+    controller->reset();
+}
+
+void MotorController::reset_parameters()
+{
+    p_controller.restore_defaults();
+    pi_controller.restore_defaults();
+
+    select_controller(ControllerType::PI);
+    set_target_rpm(DEFAULT_TARGET_RPM);
+
+    set_profile_acceleration(0);
+    set_profile_deceleration(0);
+
+}
+
+void MotorController::set_Kp(double value)
+{
+    controller->set_Kp(value);
+}
+
+bool MotorController::set_Ti(double value)
+{
+    return controller->set_Ti(value);
+}
+
+void MotorController::select_controller(ControllerType type)
+{
+    if (type == ControllerType::PI) 
+    {
+        controller = &pi_controller;
+    }
+    else
+    {
+        controller = &p_controller;
+    }
+
+    controller->reset();
+}
+
+ControllerType MotorController::get_controller_type() const
+{
+    if (controller == &pi_controller)
+    {
+        return ControllerType::PI; 
+    }
+    else
+    {
+        return ControllerType::P;
+    }
+}
+
+void MotorController::to_stopped()
+{
+    transition_to(stopped_state);
+}
+
+NodeState MotorController::get_state() const
+{
+    if (current_state == nullptr)
+    {
+        return NodeState::INITIALIZATION;
+    }
+    else{
+        return current_state->id();
+    }
+}
+
+bool MotorController::fault_active()
+{
+    return motor.fault_active();
+}
+
+void MotorController::led_on()
+{
+   led.set_hi();
+}
+
+
+void MotorController::led_off()
+{
+   led.set_lo();
+}

src/motor_driver.cpp

@@ -0,0 +1,156 @@
+#include "motor_driver.h"
+
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/gpio.h>
+#include <zephyr/drivers/pwm.h>
+
+static const struct pwm_dt_spec bin1_pwm =
+    PWM_DT_SPEC_GET(DT_ALIAS(motor_bin1_pwm));
+
+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)
+{
+}
+
+
+void MotorDriver::begin()
+{
+    initialized = false;
+
+    if (!pwm_is_ready_dt(&bin1_pwm) ||
+        !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(&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;
+    }
+
+    // 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 ||
+        pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0) < 0) {
+        return;
+    }
+    initialized = true;
+    stop();
+}
+
+
+void MotorDriver::forward()
+{
+    set(255.0);
+}
+
+
+void MotorDriver::reverse()
+{
+    set(-255.0);
+}
+
+
+void MotorDriver::stop()
+{
+    if (!initialized) {
+        return;
+    }
+
+    // BIN1 low and BIN2 low puts the DRV8833 in coast mode.
+    pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0);
+    pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0);
+}
+
+void MotorDriver::set(double pwm)
+{
+    if (!initialized) {
+        return;
+    }
+
+    // Limit PWM command
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    } else if (pwm < -255.0) {
+        pwm = -255.0;
+    }
+
+    if (pwm == 0.0) {
+        stop();
+        return;
+    }
+
+
+    const double magnitude = pwm > 0.0 ? pwm : -pwm;
+
+    // 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<uint32_t>(
+        static_cast<double>(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: BIN1 PWM, BIN2 low.
+        pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0);
+        pwm_set_dt(&bin1_pwm, bin1_pwm.period, pulse);
+    } else {
+        // Reverse: BIN1 low, BIN2 PWM.
+        pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0);
+        pwm_set_dt(&bin2_pwm, bin2_pwm.period, 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;
+// }
+
+bool MotorDriver::fault_active()
+{
+    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 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;
+}

src/operational_state.cpp

@@ -0,0 +1,100 @@
+#include "operational_state.h"
+#include "motor_controller.h"
+
+#include <zephyr/sys/printk.h>
+
+void OperationalState::enter()
+{
+    printk("State: Operational\n");
+
+    context->reset_controller();
+
+    context->led_on();
+
+    sample_count = 0;
+
+    // Tímabundið til þess að prófa undirbúning CAN communication
+    printk("Kp=%.4f Ti=%.4f Target=%.0f RPM\n",
+        context->get_Kp(), context->get_Ti(), context->get_target_rpm());
+
+#if LOG_CSV
+    printk("time_ms,ref_rpm,actual_rpm,pwm\n");
+#endif
+}
+
+
+void OperationalState::update()
+{
+    if (context->fault_active())
+    {
+        printk("Fault detected\n");
+
+        context->to_stopped();
+
+        return;
+    }
+
+    if (context->run_speed_controller())
+    {
+        ++sample_count;
+
+#if LOG_CSV
+        // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
+        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
+    }
+}
+
+
+void OperationalState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        printk("Command: reset communication\n"); // Heldur Kp og Ti gildum
+
+        context->to_initialization();
+    }
+
+    else if (c == 'p' || c == 'P')
+    {
+         printk("Command: set pre-operational\n");
+
+        context->to_pre_operational();
+    }
+
+    else if (c == '+') // Bætti þessu við til þess að prófa að rampa rpm í operational state
+    {
+        context->set_target_rpm(context->get_target_rpm() + 10);
+        printk("Target: %.0f RPM\n", context->get_target_rpm());
+    }
+    else if (c == '-') 
+    {
+        context->set_target_rpm(context->get_target_rpm() - 10);
+        printk("Target: %.0f RPM\n", context->get_target_rpm());
+    }
+
+    // Tímabundið til þess að testa reset parameters
+    else if (c == 'x' || c == 'X')
+    {
+        printk("Command: reset node\n");
+        context->reset_parameters();
+        context->to_initialization();
+    }
+
+    else if (c == '?')
+    {
+        printk("Actual=%.0f RPM Duty=%.1f\n",
+            context->get_actual_rpm(), context->get_duty_percent());
+    }
+}
+
+void OperationalState::exit()
+{
+    context->stop_motor();
+
+    context->led_off();
+}

src/p_controller.cpp

@@ -0,0 +1,20 @@
+#include "p_controller.h"
+
+P_controller::P_controller(double Kp)
+    : Kp(Kp),
+    default_Kp(Kp)
+{
+}
+
+double P_controller::update(double ref, double actual)
+{
+    double error = ref - actual;
+
+    return Kp * error;
+}
+
+void P_controller::restore_defaults()
+{
+    Kp = default_Kp;
+    reset();
+}

src/pi_controller.cpp

@@ -0,0 +1,50 @@
+#include "pi_controller.h"
+
+PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), default_Ti(Ti), T(T), integral(0.0)
+{
+}
+
+double PI_controller::update(double ref, double actual)
+{
+    double e = ref - actual;
+    double new_integral = integral + e * T;
+    double u = Kp * (e + new_integral / Ti);
+
+    if (u > U_MAX)
+    {
+        u = U_MAX;
+    }
+    else if (u < U_MIN)
+    {
+        u = U_MIN;
+    }
+    else
+    {
+        integral = new_integral;
+    }
+    return u;
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}
+
+bool PI_controller::set_Ti(double value)
+{
+    if (value > 0.0)
+    {
+        Ti = value;
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void PI_controller::restore_defaults()
+{
+    P_controller::restore_defaults();
+    Ti = default_Ti;
+}

src/pre_operational_state.cpp

@@ -0,0 +1,183 @@
+#include "pre_operational_state.h"
+#include "motor_controller.h"
+
+// #include "p_controller.h"
+
+#include <stdlib.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
+
+// PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller)
+PreOperationalState::PreOperationalState()
+    // : p_controller(p_controller),
+    //   pi_controller(pi_controller),
+    //   pi_selected(true),
+      : previous_time(0),
+      led_state(false),
+      input_mode(NONE),
+      input_index(0)
+{
+}
+
+void PreOperationalState::enter()
+{
+    printk("State: Pre-operational\n");
+
+    // The selection survives a reset, so report it (used by capture_serial.py).
+    printk("Controller: %s\n", (context->get_controller_type() == ControllerType::PI) ? "PI" : "P");
+    // Prófaði þetta, veit ekki hvort þetta skili bool
+
+    context->stop_motor();
+
+    context->led_off();
+
+    led_state = false;
+
+    previous_time = k_uptime_get();
+}
+
+void PreOperationalState::update()
+{
+    const int64_t current_time = k_uptime_get();
+
+    if (current_time - previous_time >= 500)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        if (led_state)
+        {
+            context->led_on();
+        }
+        else
+        {
+            context->led_off();
+        }
+    }
+}
+
+void PreOperationalState::command(char c)
+{
+    // We are currently entering a Kp or Ti value
+    if (input_mode != NONE)
+    {
+        // Enter pressed
+        if (c == '\n' || c == '\r')
+        {
+            if (input_index > 0)
+            {
+                // End the string
+                input_buffer[input_index] = '\0';
+
+                // Convert text to a number
+                const double value = atof(input_buffer);
+
+                if (input_mode == ENTER_KP)
+                {
+                    context->set_Kp(value);
+
+                    printk("Kp set to: %.4f\n", value);
+                }
+                else if (input_mode == ENTER_TI)
+                {
+                    if (context->set_Ti(value))
+                    {
+                        printk("Ti set to: %.4f\n", value);
+                    }
+                    else
+                    {
+                        printk("Ti rejected, must be > 0");
+                    }
+                }
+                else if (input_mode == ENTER_RPM)
+                {
+                    context->set_target_rpm(value);
+
+                    printk("Target RPM set to: %.0f\n", value);
+                }
+            }
+
+            // Finished entering a value
+            input_index = 0;
+            input_mode = NONE;
+
+            return;
+        }
+
+        // Store number characters
+        if ((c >= '0' && c <= '9') ||
+            c == '.' ||
+            c == '-')
+        {
+            if (input_index < sizeof(input_buffer) - 1)
+            {
+                input_buffer[input_index++] = c;
+            }
+        }
+
+        return;
+    }
+
+
+    // Normal state commands
+    if (c == 'o' || c == 'O')
+    {
+        printk("Command: set operational\n");
+
+        context->to_operational();
+    }
+
+    else if (c == 'r' || c == 'R')
+    {
+         printk("Command: reset\n");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'k' || c == 'K')
+    {
+        input_mode = ENTER_KP;
+        input_index = 0;
+
+        printk("Enter Kp and press Enter:\n");
+    }
+
+    else if (c == 't' || c == 'T')
+    {
+        input_mode = ENTER_TI;
+        input_index = 0;
+
+        printk("Enter Ti and press Enter:\n");
+    }
+
+    else if (c == 'v' || c == 'V')
+    {
+        input_mode = ENTER_RPM;
+        input_index = 0;
+
+        printk("Enter RPM and press Enter:\n");
+    }
+
+    // Select controller algorithm, the controller is passed to the motor controller
+    else if (c == 'c' || c == 'C')
+    {
+        if (context->get_controller_type() == ControllerType::P)
+        {
+            context->select_controller(ControllerType::PI);
+            printk("Controller: PI, Kp=%.4f, Ti=%.4f\n",
+                context->get_Kp(), context->get_Ti());
+        }
+        else
+        {
+            context->select_controller(ControllerType::P);
+            printk("Controller: P, Kp=%.4f\n",
+                context->get_Kp());
+        }
+    }
+}
+
+void PreOperationalState::exit()
+{
+    context->led_off();
+}

src/rotary_encoder_driver.cpp

@@ -0,0 +1,155 @@
+#include "rotary_encoder_driver.h"
+
+#include <errno.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/util.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/gpio.h>
+
+RotaryEncoderDriver::RotaryEncoderDriver()
+    : count(0),
+      delta(0),
+      speed_ready(false),
+      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;
+    }
+}
+
+int RotaryEncoderDriver::begin()
+{
+    eA.init();
+    eB.init();
+
+    // 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);
+
+    const int ret = eA.enable_rising_edge_interrupt(
+        &encoder_c1_callback,
+        gpio_edge_handler);
+    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)
+{
+    // 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);
+
+    encoder->update();
+}
+
+void RotaryEncoderDriver::timer_expired(void *context)
+{
+    auto *encoder = static_cast<RotaryEncoderDriver *>(context);
+    encoder->update_delta();
+}
+
+void RotaryEncoderDriver::update()
+{
+    // C2's level at the C1 edge tells us the direction.
+    const bool forward = eB.is_hi();
+
+    const unsigned int key = irq_lock();
+    count += forward ? 1 : -1;
+    irq_unlock(key);
+}
+
+int32_t RotaryEncoderDriver::position()
+{
+    const unsigned int key = irq_lock();
+    const int32_t result = count;
+    irq_unlock(key);
+
+    return result;
+}
+
+void RotaryEncoderDriver::update_delta()
+{
+    const unsigned int key = irq_lock();
+
+    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 = static_cast<int16_t>(current_count - oldest_count);
+    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()
+{
+    const unsigned int key = irq_lock();
+    const int16_t local_delta = delta;
+    irq_unlock(key);
+
+    constexpr float window_seconds =
+        HIST_LEN * SAMPLE_PERIOD_US * 1e-6f;
+
+    return local_delta / window_seconds;
+}
+
+float RotaryEncoderDriver::rpm()
+{
+    return count_per_sec() * 60.0f / COUNTS_PER_REV;
+}
+
+bool RotaryEncoderDriver::new_speed_available()
+{
+    const unsigned int key = irq_lock();
+    const bool ready = speed_ready;
+
+    if (ready) {
+        speed_ready = false;
+    }
+
+    irq_unlock(key);
+    return ready;
+}
+
+void RotaryEncoderDriver::reset_speed_measurement()
+{
+    const unsigned int key = irq_lock();
+
+    delta = 0;
+    speed_ready = false;
+
+    for (uint8_t i = 0; i < HIST_LEN; ++i) {
+        history[i] = count;
+    }
+
+    hist_idx = 0;
+
+    irq_unlock(key);
+}

src/stopped_state.cpp

@@ -0,0 +1,79 @@
+#include "stopped_state.h"
+#include "motor_controller.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/sys/printk.h>
+
+StoppedState::StoppedState()
+    : previous_time(0),
+      led_state(false)
+{
+}
+
+
+void StoppedState::enter()
+{
+    printk("State: Stopped\n");
+
+    context->stop_motor();
+
+    led_state = false;
+
+    context->led_off();
+
+    previous_time = k_uptime_get();
+}
+
+// Controlling the blinking of the LED @ 2Hz
+
+void StoppedState::update()
+{
+    const int64_t current_time = k_uptime_get();
+
+    if (current_time - previous_time >= 250)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        if (led_state)
+        {
+            context->led_on();
+        }
+        else
+        {
+            context->led_off();
+        }
+    }
+}
+
+
+// Commands for Stopped [ r = reset and o for operational]
+void StoppedState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        printk("Command: reset\n");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'o' || c == 'O')
+    {
+        printk("Command: set operational\n");
+
+        context->to_operational();
+    }
+
+    else if (c == 'p' || c == 'P')
+    {
+        printk("Command: set pre-operational\n");
+
+        context->to_pre_operational();
+    }
+}
+
+void StoppedState::exit()
+{
+    context->led_off();
+}

src/timer_usec.cpp

@@ -0,0 +1,39 @@
+#include "timer_usec.h"
+
+Timer_usec::Timer_usec()
+    : callback(nullptr),
+      callback_context(nullptr)
+{
+}
+
+void Timer_usec::set_callback(
+    callback_t callback_function,
+    void *user_data)
+{
+    callback = callback_function;
+    callback_context = user_data;
+}
+
+
+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);
+    }
+}

test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html

zephyr/CMakeLists.txt

@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 3.13.1)
+include($ENV{ZEPHYR_BASE}/cmake/app/boilerplate.cmake NO_POLICY_SCOPE)
+project(ESP_Project4_H26)
+
+FILE(GLOB app_sources ../src/*.c*)
+target_sources(app PRIVATE ${app_sources})

zephyr/boards/nucleo_l432kc.overlay

@@ -0,0 +1,90 @@
+#include <zephyr/dt-bindings/gpio/gpio.h>
+#include <zephyr/dt-bindings/pwm/pwm.h>
+#include <zephyr/dt-bindings/pwm/stm32_pwm.h>
+
+/ {
+    chosen {
+        zephyr,canbus = &can1;
+    };
+
+    aliases {
+        encoder-c1 = &encoder_c1;
+        encoder-c2 = &encoder_c2;
+
+        motor-bin1-pwm = &motor_bin1_pwm;
+        motor-bin2-pwm = &motor_bin2_pwm;
+        motor-flt = &motor_flt;
+        motor-slp = &motor_slp;
+        fault-test-button = &fault_test_button;
+    };
+
+    buttons {
+        compatible = "gpio-keys";
+
+        encoder_c1: encoder_c1 {
+            gpios = <&gpiob 7 GPIO_ACTIVE_HIGH>; /* D4, PB7 */
+            label = "Encoder C1";
+        };
+
+        encoder_c2: encoder_c2 {
+            gpios = <&gpiob 6 GPIO_ACTIVE_HIGH>; /* D5, PB6 */
+            label = "Encoder C2";
+        };
+
+        motor_flt: motor_flt {
+            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_slp: motor_slp {
+            gpios = <&gpioa 1 GPIO_ACTIVE_HIGH>; /* A1, PA1 */
+            label = "Motor sleep";
+        };
+    };
+
+    motor_pwm_outputs {
+        compatible = "pwm-leds";
+
+        motor_bin1_pwm: motor_bin1_pwm {
+            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";
+    /* 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 &tim1_ch3n_pb1>;
+        pinctrl-names = "default";
+    };
+};
+
+/* CAN1 to the transceiver: RX on D10 (PA11), TX on D2 (PA12).
+ * The bitrate is set in the code with can_set_bitrate(). */
+&can1 {
+    pinctrl-0 = <&can1_rx_pa11 &can1_tx_pa12>;
+    pinctrl-names = "default";
+    status = "okay";
+};

zephyr/prj.conf

@@ -0,0 +1,12 @@
+CONFIG_CPP=y
+
+CONFIG_GPIO=y
+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
+CONFIG_CAN=y