diff --git a/docs/diagrams/src/motor_state.wsd b/docs/diagrams/src/motor_state.wsd index a3b5e6a..862e45c 100644 --- a/docs/diagrams/src/motor_state.wsd +++ b/docs/diagrams/src/motor_state.wsd @@ -21,7 +21,16 @@ state Operational Operational : Do / Run speed controller Operational : Exit / Stop motor and LED off -Initialization --> Operational : initialization\n complete +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 diff --git a/include/motor_controller.h b/include/motor_controller.h index 21543f5..80db0aa 100644 --- a/include/motor_controller.h +++ b/include/motor_controller.h @@ -18,8 +18,9 @@ private: State* current_state; - State& initialization_state; - State& operational_state; + State* initialization_state; + State* operational_state; + State* stopped_state; RotaryEncoderDriver& encoder; MotorDriver& motor; @@ -36,7 +37,8 @@ public: MotorDriver& motor, P_controller& controller, State& initialization, - State& operational + State& operational, + state& stopped ); @@ -53,6 +55,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 08c2f2d..a613548 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; @@ -24,6 +25,7 @@ InitializationState initialization_state; OperationalState operational_state; +StoppedState stopped_state; // Stöðurnar verða að vera skilgreindar á undan motor_controller MotorController motor_controller( @@ -51,6 +53,13 @@ int main() sei(); + motor_controller.set_states( + &initialization_state, + &operational_state, + &stopped_state + ); + + motor_controller.begin(); diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index feb5d88..bed7cde 100644 --- a/src/motor_controller.cpp +++ b/src/motor_controller.cpp @@ -14,6 +14,7 @@ MotorController::MotorController( : current_state(nullptr), initialization_state(initialization), operational_state(operational), + stopped_state(stopped), encoder(encoder), motor(motor), controller(controller), @@ -116,3 +117,12 @@ void MotorController::led_off() { led.set_lo(); } +void MotorController::to_stopped() +{ + transition_to(stopped_state); +} + +bool MotorController::fault_active() +{ + return motor.fault_active(); +} diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index e15b6e0..6b841a7 100644 --- a/src/motor_driver.cpp +++ b/src/motor_driver.cpp @@ -43,10 +43,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; @@ -116,3 +122,9 @@ void MotorDriver::set(double pwm) ain2_pwm(duty); } } + + +bool MotorDriver::fault_active() +{ + return !(PIND & (1 << PIND4)); +} diff --git a/src/operational_state.cpp b/src/operational_state.cpp index 30d0116..2f07ff3 100644 --- a/src/operational_state.cpp +++ b/src/operational_state.cpp @@ -11,9 +11,26 @@ void OperationalState::enter() context->led_on(); } +// 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