Nýtt branch og kóðar plús breytingar á fyrri kóðum fyrir part 4

src/initialization_state.cpp

@@ -24,7 +24,7 @@ void InitializationState::update()
     // Non-blocking bið svo aðallykkjan haldi áfram að keyra
     if (millis() - enter_time_ms >= WAIT_MS)
     {
-        context->to_operational();
+        context->to_pre_operational();
     }
 }
 

src/main.cpp

@@ -11,6 +11,9 @@
 #include "motor_controller.h"
 
 #include "initialization_state.h"
+
+#include "pre_operational_state.h"
+
 #include "operational_state.h"
 #include "stopped_state.h"
 
@@ -29,6 +32,8 @@ PI_controller controller(Kp, Ti, T);
 
 InitializationState initialization_state;
 
+PreOperationalState pre_operational_state;
+
 OperationalState operational_state;
 
 StoppedState stopped_state;
@@ -39,6 +44,7 @@ MotorController motor_controller(
     motor,
     controller,
     initialization_state,
+    pre_operational_state,
     operational_state,
     stopped_state
 );

src/motor_controller.cpp

@@ -9,11 +9,13 @@ MotorController::MotorController(
     MotorDriver& motor,
     P_controller& 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),
@@ -23,6 +25,7 @@ MotorController::MotorController(
       reference_rpm(DEFAULT_REFERENCE_RPM)
 {
     initialization_state.set_context(this);
+    pre_operational_state.set_context(this);
     operational_state.set_context(this);
     stopped_state.set_context(this);
 }
@@ -85,6 +88,11 @@ void MotorController::to_initialization()
     transition_to(initialization_state);
 }
 
+void MotorController::to_pre_operational()
+{
+    transition_to(pre_operational_state);
+}
+
 
 void MotorController::to_operational()
 {
@@ -125,6 +133,16 @@ void MotorController::reset_controller()
     controller.reset();
 }
 
+void MotorController::set_Kp(double value)
+{
+    controller.set_Kp(value);
+}
+
+void MotorController::set_Ti(double value)
+{
+    controller.set_Ti(value);
+}
+
 void MotorController::to_stopped()
 {
     transition_to(stopped_state);

src/operational_state.cpp

@@ -45,6 +45,13 @@ void OperationalState::command(char c)
 
         context->to_initialization();
     }
+
+    else if (c == 'p' || c == 'P')
+    {
+        Serial.println("Command: set pre-operational");
+
+        context->to_pre_operational();
+    }
 }
 
 

src/pi_controller.cpp

@@ -30,3 +30,8 @@ void PI_controller::reset()
 {
     integral = 0.0;
 }
+
+void PI_controller::set_Ti(double value)
+{
+    Ti = value;
+}

src/pre_operational_state.cpp

@@ -0,0 +1,66 @@
+#include "pre_operational_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+PreOperationalState::PreOperationalState()
+    : previous_time(0),
+      led_state(false)
+{
+}
+
+void PreOperationalState::enter()
+{
+    Serial.println("State: Pre-operational");
+
+    context->stop_motor();
+
+    context->led_off();
+
+    led_state = false;
+
+    previous_time = millis();
+}
+
+void PreOperationalState::update()
+{
+    unsigned long current_time = millis();
+
+    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)
+{
+    if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+
+    else if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+}
+
+void PreOperationalState::exit()
+{
+    context->led_off();
+}