undirbúningur falla fyrir can samskipti og prófun með lyklaborði

src/CAN_FRAME_TEST.c

@@ -32,8 +32,8 @@ void can_frame_test_main(void)
 
     struct can_frame frame = {
         .flags = 0,
-        .id = 0x123,
-        .dlc = 8,
+        .id = 0x456,
+        .dlc = 4,
         .data = {1, 2, 3, 4, 5, 6, 7, 8},
     };
 

src/app.cpp

@@ -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/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)
 {
@@ -92,6 +98,8 @@ bool MotorController::run_speed_controller()
         return false;
     }
 
+    reference_rpm = target_rpm;
+
     const double actual = encoder.rpm();
 
     const double pwm_percent =
@@ -135,28 +143,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,6 +65,31 @@ 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());
+    }
 }
 
 

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)
 {
-    Ti = 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,9 +81,20 @@ 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("Ti set to: %.4f\n", value);
+                    printk("Target RPM set to: %.0f\n", 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());
         }
     }
 }