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

include/motor_controller.h

@@ -19,6 +19,7 @@ private:
     State* current_state;
 
     State& initialization_state;
+    State& pre_operational_state;
     State& operational_state;
     State& stopped_state;
 
@@ -37,6 +38,7 @@ public:
         MotorDriver& motor,
         P_controller& controller,
         State& initialization,
+        State& pre_operational,
         State& operational,
         State& stopped
     );
@@ -53,6 +55,8 @@ public:
 
     void to_initialization();
 
+    void to_pre_operational();
+
     void to_operational();
 
     void to_stopped();
@@ -68,6 +72,10 @@ public:
 
     void reset_controller();
 
+    void set_Kp(double value);
+
+    void set_Ti(double value);
+
     void led_on();
 
     void led_off();

include/p_controller.h

@@ -11,6 +11,16 @@ public:
     virtual ~P_controller() {}
     virtual double update(double ref, double actual);
     virtual void reset() {}
+
+    virtual void set_Kp(double value)
+    {
+        Kp = value;
+    }
+
+    virtual void set_Ti(double value)
+    {
+        // P controller does not use Ti
+    }
 };
 
 #endif

include/pi_controller.h

@@ -16,6 +16,8 @@ class PI_controller : public P_controller
         PI_controller(double Kp, double Ti, double T);
         double update(double ref, double actual) override;
         void reset() override;
+
+        void set_Ti(double value) override;
 };
 
 #endif

include/pre_operational_state.h

@@ -0,0 +1,21 @@
+#ifndef PRE_OPERATIONAL_STATE_H
+#define PRE_OPERATIONAL_STATE_H
+
+#include "state.h"
+
+class PreOperationalState : public State
+{
+private:
+    unsigned long previous_time;
+    bool led_state;
+
+public:
+    PreOperationalState();
+
+    void enter() override;
+    void update() override;
+    void command(char c) override;
+    void exit() override;
+};
+
+#endif

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