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

.gitattributes

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

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

docs/diagrams/src/motor_state.wsd

@@ -1,50 +0,0 @@
-@startuml
-' skinparam linetype ortho
-' skinparam nodesep 100
-skinparam ranksep 80
-
-skinparam state {
-    BackgroundColor LightBlue
-}
-
-[*] --> Initialization
-
-state Initialization 
-    Initialization : Entry / Stop motor 
-    Initialization : Entry / Initialize internal parameters
-    Initialization : Entry / Reset speed measurement
-    Initialization : Do / Wait 2 seconds
-    Initialization : Exit / Send Boot-up message
-
-state PreOperational
-    PreOperational : Entry / Stop motor
-    PreOperational : Do / Blink LED at 1 Hz
-    PreOperational : Do / Accept Kp and Ti configuration
-    PreOperational : Do / Select P or PI controller [c]
-    PreOperational : Exit / LED off
-
-state Operational 
-    Operational : Entry / LED on
-    Operational : Do / Run speed controller
-    Operational : Exit / Stop motor and LED off
-
-state Stopped
-    Stopped : Entry / Stop motor
-    Stopped : Do / Blink LED at 2 Hz
-    Stopped : Exit / LED off
-
-Initialization -r-> PreOperational : initialization\n complete
-' Initialization -d-> Operational : initialization\n complete
-
-PreOperational -d-> Operational : set operational [o]
-PreOperational -l--> Initialization : reset [r]
-
-Operational -u-> PreOperational : set pre-operational [p]
-Operational -> Initialization : reset [r]
-Operational --> Stopped : fault detected
-
-Stopped -l-> Initialization : reset [r]
-Stopped -> Operational : set operational [o]
-Stopped -u-> PreOperational : set pre-operational [p]
-
-@enduml

include/initialization_state.h

@@ -22,6 +22,8 @@ public:
     void command(char c) override;
 
     void exit() override;
+
+    NodeState id() const override { return NodeState::INITIALIZATION; }
 };
 
 #endif

include/motor_controller.h

@@ -7,14 +7,15 @@
 #include "motor_driver.h"
 #include "p_controller.h"
 #include "digital_out.h"
+#include "node_types.h"
+#include "node_application.h"
 
 class State;
 
-
-class MotorController
+class MotorController : public NodeApplication
 {
 private:
-    static constexpr double DEFAULT_REFERENCE_RPM = 25.0;
+    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;
 
@@ -27,22 +28,32 @@ private:
 
     RotaryEncoderDriver &encoder;
     MotorDriver &motor;
-    P_controller *controller;
+
+    P_controller &p_controller;
+    P_controller &pi_controller;
+    P_controller *controller;   // Bendir á p_controller eða pi_controller
 
     Digital_out led;
 
-    double reference_rpm;
+    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 &controller,
+        P_controller &p_controller,
+        P_controller &pi_controller,
         State &initialization,
         State &pre_operational,
         State &operational,
@@ -59,13 +70,15 @@ public:
     void transition_to(State &new_state);
 
 
-    void to_initialization();
+    void to_initialization() override;
 
-    void to_pre_operational();
+    void to_pre_operational() override;
 
-    void to_operational();
+    void to_operational() override;
 
-    void to_stopped();
+    void to_stopped() override;
+
+    NodeState get_state() const override;
 
     bool fault_active();
 
@@ -73,21 +86,34 @@ public:
     // 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 { return last_actual_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);
+    void set_Kp(double value) override;
+    double get_Kp() const override { return controller->get_Kp(); }
 
-    void set_Ti(double value);
+    bool set_Ti(double value) override;
+    double get_Ti() const override { return controller->get_Ti(); }
 
-    void set_controller(P_controller &new_controller);
+    void select_controller(ControllerType type) override;
+    ControllerType get_controller_type() const override;
 
     void led_on();
 

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

@@ -24,6 +24,8 @@ public:
     void command(char c) override;
 
     void exit() override;
+
+    NodeState id() const override { return NodeState::OPERATIONAL; }
 };
 
 #endif

include/p_controller.h

@@ -5,21 +5,27 @@ 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 void set_Ti(double value)
+    virtual bool set_Ti(double value)
     {
         // P controller does not use Ti
+        (void)value;
+        return true;
     }
 };
 

include/pi_controller.h

@@ -10,14 +10,17 @@ class PI_controller : public P_controller
         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;
 
-        void set_Ti(double value) override;
+        bool set_Ti(double value) override;
+        double get_Ti() const override { return Ti; }
 };
 
 #endif

include/pre_operational_state.h

@@ -4,14 +4,14 @@
 #include "state.h"
 #include "stdint.h"
 
-class P_controller;
+// class P_controller;
 
 class PreOperationalState : public State
 {
 private:
-    P_controller &p_controller;
-    P_controller &pi_controller;
-    bool pi_selected;
+    // P_controller &p_controller;
+    // P_controller &pi_controller;
+    // bool pi_selected;
 
     int64_t previous_time;
     bool led_state;
@@ -21,7 +21,8 @@ private:
     {
         NONE,
         ENTER_KP,
-        ENTER_TI
+        ENTER_TI,
+        ENTER_RPM
     };
 
     InputMode input_mode;
@@ -30,12 +31,15 @@ private:
     uint8_t input_index;
 
 public:
-    PreOperationalState(P_controller& p_controller, P_controller& pi_controller);
+    // 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/state.h

@@ -1,6 +1,8 @@
 #ifndef STATE_H
 #define STATE_H
 
+#include "node_types.h"
+
 class MotorController;
 
 class State
@@ -25,6 +27,8 @@ public:
     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

@@ -21,6 +21,8 @@ public:
     void command(char c) override;
 
     void exit() override;
+
+    NodeState id() const override { return NodeState::STOPPED; }
 };
 
 #endif

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

@@ -21,3 +21,21 @@ upload_flags =
 
 ; 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

@@ -12,12 +12,12 @@
 #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;
@@ -34,7 +34,7 @@ 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 PreOperationalState pre_operational_state;
 static OperationalState operational_state;
 static StoppedState stopped_state;
 
@@ -42,6 +42,7 @@ static StoppedState stopped_state;
 static MotorController motor_controller(
     encoder,
     motor,
+    p_controller,
     pi_controller,      // Sjálfgefin stýring
     initialization_state,
     pre_operational_state,
@@ -107,6 +108,15 @@ extern "C" void motor_application_main(void)
 
         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.

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/initialization_state.cpp

@@ -24,13 +24,11 @@ void InitializationState::update()
     }
 }
 
-
 void InitializationState::command(char c)
 {
     (void)c;
 } 
 
-
 void InitializationState::exit()
 {
     printk("Boot-up\n");

src/main.c

@@ -1,9 +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

@@ -4,7 +4,8 @@
 MotorController::MotorController(
     RotaryEncoderDriver &encoder,
     MotorDriver &motor,
-    P_controller &controller,
+    P_controller &p_controller,
+    P_controller &pi_controller,
     State &initialization,
     State &pre_operational,
     State &operational,
@@ -17,9 +18,14 @@ MotorController::MotorController(
       stopped_state(stopped),
       encoder(encoder),
       motor(motor),
-      controller(&controller),
+      p_controller(p_controller),
+      pi_controller(pi_controller),
+      controller(&pi_controller),   // PI er sjálfgefin stýring
       led(LED_PIN),
-      reference_rpm(DEFAULT_REFERENCE_RPM),
+      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)
 {
@@ -58,7 +64,6 @@ void MotorController::update()
     }
 }
 
-
 void MotorController::command(char c)
 {
     if (current_state != nullptr)
@@ -67,7 +72,6 @@ void MotorController::command(char c)
     }
 }
 
-
 void MotorController::to_initialization()
 {
     transition_to(initialization_state);
@@ -92,6 +96,8 @@ bool MotorController::run_speed_controller()
         return false;
     }
 
+    reference_rpm = target_rpm;
+
     const double actual = encoder.rpm();
 
     const double pwm_percent =
@@ -135,28 +141,71 @@ 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);
 }
 
-void MotorController::set_Ti(double value)
+bool MotorController::set_Ti(double value)
 {
-    controller->set_Ti(value);
+    return controller->set_Ti(value);
 }
 
-void MotorController::set_controller(P_controller& new_controller)
+void MotorController::select_controller(ControllerType type)
 {
-    controller = &new_controller;
+    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();

src/operational_state.cpp

@@ -13,6 +13,10 @@ void OperationalState::enter()
 
     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
@@ -50,7 +54,7 @@ void OperationalState::command(char c)
 {
     if (c == 'r' || c == 'R')
     {
-        printk("Command: reset\n");
+        printk("Command: reset communication\n"); // Heldur Kp og Ti gildum
 
         context->to_initialization();
     }
@@ -61,8 +65,32 @@ void OperationalState::command(char c)
 
         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()
 {

src/p_controller.cpp

@@ -1,7 +1,8 @@
 #include "p_controller.h"
 
 P_controller::P_controller(double Kp)
-    : Kp(Kp)
+    : Kp(Kp),
+    default_Kp(Kp)
 {
 }
 
@@ -11,3 +12,9 @@ double P_controller::update(double ref, double actual)
 
     return Kp * error;
 }
+
+void P_controller::restore_defaults()
+{
+    Kp = default_Kp;
+    reset();
+}

src/pi_controller.cpp

@@ -1,6 +1,6 @@
 #include "pi_controller.h"
 
-PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), T(T), integral(0.0)
+PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), default_Ti(Ti), T(T), integral(0.0)
 {
 }
 
@@ -30,7 +30,21 @@ void PI_controller::reset()
     integral = 0.0;
 }
 
-void PI_controller::set_Ti(double value)
+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

@@ -1,17 +1,18 @@
 #include "pre_operational_state.h"
 #include "motor_controller.h"
 
-#include "p_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)
-    : p_controller(p_controller),
-      pi_controller(pi_controller),
-      pi_selected(true),
-      previous_time(0),
+// 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)
@@ -23,7 +24,8 @@ 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", pi_selected ? "PI" : "P");
+    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();
 
@@ -79,10 +81,21 @@ void PreOperationalState::command(char c)
                 }
                 else if (input_mode == ENTER_TI)
                 {
-                    context->set_Ti(value);
-
+                    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
@@ -138,20 +151,28 @@ void PreOperationalState::command(char c)
         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')
     {
-        pi_selected = !pi_selected;
-
-        if (pi_selected)
+        if (context->get_controller_type() == ControllerType::P)
         {
-            context->set_controller(pi_controller);
-            printk("Controller: PI\n");
+            context->select_controller(ControllerType::PI);
+            printk("Controller: PI, Kp=%.4f, Ti=%.4f\n",
+                context->get_Kp(), context->get_Ti());
         }
         else
         {
-            context->set_controller(p_controller);
-            printk("Controller: P\n");
+            context->select_controller(ControllerType::P);
+            printk("Controller: P, Kp=%.4f\n",
+                context->get_Kp());
         }
     }
 }

src/rotary_encoder_driver.cpp

@@ -6,11 +6,6 @@
 #include <zephyr/devicetree.h>
 #include <zephyr/drivers/gpio.h>
 
-//#define ISR_PROBE_NODE DT_PATH(zephyr_user)
-
-static const struct gpio_dt_spec isr_probe =
-    GPIO_DT_SPEC_GET(DT_ALIAS(encoder_isr_probe), gpios);
-
 RotaryEncoderDriver::RotaryEncoderDriver()
     : count(0),
       delta(0),

src/stopped_state.cpp

@@ -49,7 +49,6 @@ void StoppedState::update()
 
 
 // Commands for Stopped [ r = reset and o for operational]
-
 void StoppedState::command(char c)
 {
     if (c == 'r' || c == 'R')
@@ -74,7 +73,6 @@ void StoppedState::command(char c)
     }
 }
 
-
 void StoppedState::exit()
 {
     context->led_off();

zephyr/boards/nucleo_l432kc.overlay

@@ -3,6 +3,10 @@
 #include <zephyr/dt-bindings/pwm/stm32_pwm.h>
 
 / {
+    chosen {
+        zephyr,canbus = &can1;
+    };
+
     aliases {
         encoder-c1 = &encoder_c1;
         encoder-c2 = &encoder_c2;
@@ -45,11 +49,6 @@
             gpios = <&gpioa 1 GPIO_ACTIVE_HIGH>; /* A1, PA1 */
             label = "Motor sleep";
         };
-
-        encoder_isr_probe: encoder_isr_probe {
-            gpios = <&gpioa 12 GPIO_ACTIVE_HIGH>; /* D2, PA12 */
-            label = "Encoder ISR probe";
-        };
     };
 
     motor_pwm_outputs {
@@ -68,9 +67,6 @@
             label = "Motor BIN2 PWM";
         };
     };
-        zephyr,user {
-        encoder-isr-probe-gpios = <&gpioa 12 GPIO_ACTIVE_HIGH>; /* D2, PA12 */
-    };
 };
 
 &timers1 {
@@ -84,3 +80,11 @@
         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

@@ -9,3 +9,4 @@ CONFIG_UART_CONSOLE=y
 CONFIG_UART_INTERRUPT_DRIVEN=y
 CONFIG_PRINTK=y
 CONFIG_CBPRINTF_FP_SUPPORT=y
+CONFIG_CAN=y