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