Leggja lokahönd á verkefnið og plot-a fleiri myndir

src/pre_operational_state.cpp

@@ -4,8 +4,11 @@
 #include <Arduino.h>
 #include <stdlib.h>
 
-PreOperationalState::PreOperationalState()
-    : previous_time(0),
+PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
+    : p_controller(p_controller),
+      pi_controller(pi_controller),
+      pi_selected(true),
+      previous_time(0),
       led_state(false),
       //waiting_for_kp(false),
       //waiting_for_ti(false),
@@ -133,6 +136,23 @@ void PreOperationalState::command(char c)
 
         Serial.println("Enter Ti and press Enter:");
     }
+
+    // Select controller algorithm, the controller is passed to the motor controller
+    else if (c == 'c' || c == 'C')
+    {
+        pi_selected = !pi_selected;
+
+        if (pi_selected)
+        {
+            context->set_controller(pi_controller);
+            Serial.println("Controller: PI");
+        }
+        else
+        {
+            context->set_controller(p_controller);
+            Serial.println("Controller: P");
+        }
+    }
 }
 
 void PreOperationalState::exit()