Stop bætt inn í UML diagram

docs/diagrams/src/motor_state.wsd

@@ -21,8 +21,18 @@ state Operational
     Operational : Exit / Stop motor
     Operational : Exit / LED off
 
+state Stopped
+    Stopped : Entry / Stop motor
+    Stopped : Do / Blink LED at 2 Hz
+    Stopped : Exit / LED off
+
+
 
 Initialization --> Operational : initialization complete
 Operational --> Initialization : reset
+Operational --> Stopped : fault detected
+
+Stopped --> Initialization : reset
+Stopped --> Operational : set operational
 
 @enduml

include/motor_controller.h

@@ -15,6 +15,7 @@ private:
 
     State* initialization_state;
     State* operational_state;
+    State* stopped_state;
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
@@ -33,7 +34,8 @@ public:
 
     void set_states(
         State* initialization,
-        State* operational
+        State* operational,
+        State* stopped
     );
 
 
@@ -50,6 +52,10 @@ public:
 
     void to_operational();
 
+    void to_stopped();
+
+    bool fault_active();
+
 
     void run_speed_controller();
 

include/motor_driver.h

@@ -9,10 +9,14 @@ public:
     void begin();
 
     void forward();
+
     void reverse();
+
     void stop();
 
     void set(double pwm);
+
+    bool fault_active();   // Fault detection for part 2
 };
 
 #endif

include/stopped_state.h

@@ -0,0 +1,25 @@
+#ifndef STOPPED_STATE_H
+#define STOPPED_STATE_H
+
+#include "state.h"
+
+class StoppedState : public State
+{
+private:
+    unsigned long previous_time; // Used for blinking LED
+    bool led_state;              // Ditto
+
+public:
+    StoppedState();
+
+    void enter() override;
+
+    void update() override;
+
+    void command(char c) override;
+
+    void exit() override;
+
+};
+
+#endif

src/main.cpp

@@ -9,6 +9,7 @@
 
 #include "initialization_state.h"
 #include "operational_state.h"
+#include "stopped_state.h"
 
 
 RotaryEncoderDriver encoder;
@@ -29,6 +30,7 @@ InitializationState initialization_state;
 
 OperationalState operational_state;
 
+StoppedState stopped_state;
 
 int main()
 {
@@ -53,7 +55,8 @@ int main()
 
     motor_controller.set_states(
         &initialization_state,
-        &operational_state
+        &operational_state,
+        &stopped_state
     );
 
 

src/motor_controller.cpp

@@ -12,6 +12,7 @@ MotorController::MotorController(
     : current_state(nullptr),
       initialization_state(nullptr),
       operational_state(nullptr),
+      stopped_state(nullptr),
       encoder(encoder),
       motor(motor),
       controller(controller),
@@ -22,11 +23,13 @@ MotorController::MotorController(
 
 void MotorController::set_states(
     State* initialization,
-    State* operational
+    State* operational,
+    State* stopped
 )
 {
     initialization_state = initialization;
     operational_state = operational;
+    stopped_state = stopped;
 }
 
 
@@ -108,3 +111,13 @@ void MotorController::reset_speed_measurement()
 {
     encoder.reset_speed_measurement();
 }
+
+void MotorController::to_stopped()
+{
+    transition_to(stopped_state);
+}
+
+bool MotorController::fault_active()
+{
+    return motor.fault_active();
+}

src/motor_driver.cpp

@@ -12,10 +12,16 @@ void MotorDriver::begin()
 {
     // Arduino D3  = PD3 = AIN1
     // Arduino D11 = PB3 = AIN2
+    // Arduino D4  = PD4 = FLT 
 
     DDRD |= (1 << DDD3);
     DDRB |= (1 << DDB3);
 
+    DDRD &= ~(1 << DDD4); 
+
+    PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
+
+
     // Timer2 Fast PWM
     TCCR2A = 0;
     TCCR2B = 0;
@@ -108,3 +114,9 @@ void MotorDriver::set(double pwm)
         stop();
     }
 }
+
+
+bool MotorDriver::fault_active()
+{
+    return !(PIND & (1 << PIND4));
+}

src/operational_state.cpp

@@ -11,9 +11,26 @@ void OperationalState::enter()
     digitalWrite(LED_BUILTIN, HIGH);
 }
 
+// Part 1 
+
+//void OperationalState::update()
+//{
+//    context->run_speed_controller();
+//}
+
+// Part 2
 
 void OperationalState::update()
 {
+    if (context->fault_active())
+    {
+        Serial.println("Fault detected");
+
+        context->to_stopped();
+
+        return;
+    }
+
     context->run_speed_controller();
 }
 

src/stopped_state.cpp

@@ -0,0 +1,70 @@
+#include "stopped_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+StoppedState::StoppedState()
+    : previous_time(0),
+      led_state(false)
+{
+}
+
+
+void StoppedState::enter()
+{
+    Serial.println("State: Stopped");
+
+    context->stop_motor();
+
+    led_state = false;
+
+    digitalWrite(LED_BUILTIN, LOW);
+
+    previous_time = millis();
+}
+
+// Controlling the blinking of the LED @ 2Hz
+
+void StoppedState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 250)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        digitalWrite(
+            LED_BUILTIN,
+            led_state ? HIGH : LOW
+        );
+    }
+}
+
+
+// Commands for Stopped [ r = reset and o for operational]
+
+void StoppedState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+}
+
+
+void StoppedState::exit()
+{
+    digitalWrite(LED_BUILTIN, LOW);
+}