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

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());
         }
     }
 }