From d96da75bdf7988e609fe77dd78f5f80aeb27a26a Mon Sep 17 00:00:00 2001 From: isakg22 Date: Wed, 30 Sep 2026 11:52:06 +0000 Subject: [PATCH 1/2] =?UTF-8?q?Part=202=20n=C3=BDjir=20k=C3=B3=C3=B0ar,=20?= =?UTF-8?q?fyrir=20STOP=20og=20Fault=20kerfi?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/motor_controller.h | 8 ++++- include/motor_driver.h | 4 +++ include/stopped_state.h | 25 ++++++++++++++ src/main.cpp | 5 ++- src/motor_controller.cpp | 15 +++++++- src/motor_driver.cpp | 14 +++++++- src/operational_state.cpp | 17 +++++++++ src/stopped_state.cpp | 70 ++++++++++++++++++++++++++++++++++++++ 8 files changed, 154 insertions(+), 4 deletions(-) create mode 100644 include/stopped_state.h create mode 100644 src/stopped_state.cpp diff --git a/include/motor_controller.h b/include/motor_controller.h index c085214..bbb80cd 100644 --- a/include/motor_controller.h +++ b/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(); diff --git a/include/motor_driver.h b/include/motor_driver.h index d69cc6d..9b4e54b 100644 --- a/include/motor_driver.h +++ b/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 \ No newline at end of file diff --git a/include/stopped_state.h b/include/stopped_state.h new file mode 100644 index 0000000..b42c686 --- /dev/null +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 20aad4c..17b4bef 100644 --- a/src/main.cpp +++ b/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 ); diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index c5358cc..0781d66 100644 --- a/src/motor_controller.cpp +++ b/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; } @@ -107,4 +110,14 @@ void MotorController::stop_motor() 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(); } \ No newline at end of file diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index 8dae91a..a004cc5 100644 --- a/src/motor_driver.cpp +++ b/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; @@ -107,4 +113,10 @@ void MotorDriver::set(double pwm) { stop(); } -} \ No newline at end of file +} + + +bool MotorDriver::fault_active() +{ + return !(PIND & (1 << PIND4)); +} diff --git a/src/operational_state.cpp b/src/operational_state.cpp index e052095..e663fd2 100644 --- a/src/operational_state.cpp +++ b/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(); } diff --git a/src/stopped_state.cpp b/src/stopped_state.cpp new file mode 100644 index 0000000..cf72752 --- /dev/null +++ b/src/stopped_state.cpp @@ -0,0 +1,70 @@ +#include "stopped_state.h" +#include "motor_controller.h" + +#include + + +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); +} \ No newline at end of file From 484686f8d17a5b6571a13a9e5502691e2ad4b534 Mon Sep 17 00:00:00 2001 From: isakg22 Date: Wed, 30 Sep 2026 11:59:05 +0000 Subject: [PATCH 2/2] =?UTF-8?q?Stop=20b=C3=A6tt=20inn=20=C3=AD=20UML=20dia?= =?UTF-8?q?gram?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/diagrams/src/motor_state.wsd | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/diagrams/src/motor_state.wsd b/docs/diagrams/src/motor_state.wsd index fea22e6..7974ccf 100644 --- a/docs/diagrams/src/motor_state.wsd +++ b/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