From e6f164b25f72627e470de84f94ccea97d6834c5c Mon Sep 17 00:00:00 2001 From: Thorsteinngretar Date: Sat, 3 Oct 2026 14:20:27 +0000 Subject: [PATCH] Fixed file management setup and pushed completed part 2 code --- .../Part_1_Init_Operational/.gitignore | 5 + .../docs/diagrams/part1_state.wsd | 0 .../include/README | 0 .../include/context.h | 0 .../include/control_timer.h | 0 .../include/controller.h | 0 .../include/digital_in.h | 0 .../include/digital_out.h | 0 .../include/encoder.h | 0 .../include/initialization.h | 0 .../include/motor_driver.h | 0 .../include/operational.h | 2 +- .../include/p_controller.h | 0 .../include/pi_controller.h | 0 .../include/pre_operational.h | 0 .../include/speed_controller.h | 0 .../include/speed_estimator.h | 0 .../include/state.h | 0 .../include/stopped.h | 0 .../{ => Part_1_Init_Operational}/lib/README | 0 .../platformio.ini | 0 .../src/context.cpp | 0 .../src/control_timer.cpp | 0 .../src/digital_in.cpp | 0 .../src/digital_out.cpp | 0 .../src/encoder.cpp | 0 .../src/initialization.cpp | 0 .../src/main.cpp | 18 +- .../src/motor_driver.cpp | 0 .../src/operational.cpp | 2 +- .../src/p_controller.cpp | 0 .../src/pi_controller.cpp | 0 .../src/pre_operational.cpp | 0 .../src/speed_controller.cpp | 0 .../src/speed_estimator.cpp | 0 .../src/stopped.cpp | 0 .../{ => Part_1_Init_Operational}/test/README | 0 .../Part_2_Stopped_EStop/.gitignore | 5 + .../docs/diagrams/part1_state.wsd | 21 +++ .../docs/diagrams/part2_state.wsd | 30 +++ .../Part_2_Stopped_EStop/include/README | 37 ++++ .../Part_2_Stopped_EStop/include/context.h | 26 +++ .../include/control_timer.h | 17 ++ .../Part_2_Stopped_EStop/include/controller.h | 19 ++ .../Part_2_Stopped_EStop/include/digital_in.h | 22 +++ .../include/digital_out.h | 23 +++ .../Part_2_Stopped_EStop/include/encoder.h | 28 +++ .../include/initialization.h | 35 ++++ .../include/motor_driver.h | 26 +++ .../include/operational.h | 45 +++++ .../include/p_controller.h | 29 +++ .../include/pi_controller.h | 0 .../include/pre_operational.h | 0 .../include/speed_controller.h | 47 +++++ .../include/speed_estimator.h | 37 ++++ .../Part_2_Stopped_EStop/include/state.h | 43 +++++ .../Part_2_Stopped_EStop/include/stopped.h | 39 ++++ .../Part_2_Stopped_EStop/lib/README | 46 +++++ .../Part_2_Stopped_EStop/platformio.ini | 15 ++ .../Part_2_Stopped_EStop/src/context.cpp | 80 ++++++++ .../src/control_timer.cpp | 70 +++++++ .../Part_2_Stopped_EStop/src/digital_in.cpp | 26 +++ .../Part_2_Stopped_EStop/src/digital_out.cpp | 27 +++ .../Part_2_Stopped_EStop/src/encoder.cpp | 75 ++++++++ .../src/initialization.cpp | 57 ++++++ .../Part_2_Stopped_EStop/src/main.cpp | 172 ++++++++++++++++++ .../Part_2_Stopped_EStop/src/motor_driver.cpp | 135 ++++++++++++++ .../Part_2_Stopped_EStop/src/operational.cpp | 103 +++++++++++ .../Part_2_Stopped_EStop/src/p_controller.cpp | 55 ++++++ .../src/pi_controller.cpp | 0 .../src/pre_operational.cpp | 0 .../src/speed_controller.cpp | 106 +++++++++++ .../src/speed_estimator.cpp | 76 ++++++++ .../Part_2_Stopped_EStop/src/stopped.cpp | 75 ++++++++ .../Part_2_Stopped_EStop/test/README | 11 ++ .../Part_3_PI_Controller_TDD/.gitignore | 5 + .../Part_3_PI_Controller_TDD/platformio.ini | 15 ++ .../Part_4_PreOperational_Final/.gitignore | 5 + .../platformio.ini | 15 ++ 79 files changed, 1714 insertions(+), 11 deletions(-) create mode 100644 Project_3_Dynamic_Controller/Part_1_Init_Operational/.gitignore rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/docs/diagrams/part1_state.wsd (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/README (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/context.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/control_timer.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/controller.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/digital_in.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/digital_out.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/encoder.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/initialization.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/motor_driver.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/operational.h (98%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/p_controller.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/pi_controller.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/pre_operational.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/speed_controller.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/speed_estimator.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/state.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/include/stopped.h (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/lib/README (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/platformio.ini (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/context.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/control_timer.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/digital_in.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/digital_out.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/encoder.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/initialization.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/main.cpp (83%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/motor_driver.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/operational.cpp (99%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/p_controller.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/pi_controller.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/pre_operational.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/speed_controller.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/speed_estimator.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/src/stopped.cpp (100%) rename Project_3_Dynamic_Controller/{ => Part_1_Init_Operational}/test/README (100%) create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/.gitignore create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part1_state.wsd create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part2_state.wsd create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/README create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/context.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/control_timer.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/controller.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_in.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_out.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/encoder.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/initialization.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/motor_driver.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/operational.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/p_controller.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/pi_controller.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/pre_operational.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_controller.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_estimator.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/state.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/stopped.h create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/lib/README create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/platformio.ini create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/context.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/control_timer.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_in.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_out.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/encoder.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/initialization.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/main.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/motor_driver.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/operational.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/p_controller.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/pi_controller.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/pre_operational.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_controller.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_estimator.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/stopped.cpp create mode 100644 Project_3_Dynamic_Controller/Part_2_Stopped_EStop/test/README create mode 100644 Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/.gitignore create mode 100644 Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/platformio.ini create mode 100644 Project_3_Dynamic_Controller/Part_4_PreOperational_Final/.gitignore create mode 100644 Project_3_Dynamic_Controller/Part_4_PreOperational_Final/platformio.ini diff --git a/Project_3_Dynamic_Controller/Part_1_Init_Operational/.gitignore b/Project_3_Dynamic_Controller/Part_1_Init_Operational/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_1_Init_Operational/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Project_3_Dynamic_Controller/docs/diagrams/part1_state.wsd b/Project_3_Dynamic_Controller/Part_1_Init_Operational/docs/diagrams/part1_state.wsd similarity index 100% rename from Project_3_Dynamic_Controller/docs/diagrams/part1_state.wsd rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/docs/diagrams/part1_state.wsd diff --git a/Project_3_Dynamic_Controller/include/README b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/README similarity index 100% rename from Project_3_Dynamic_Controller/include/README rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/README diff --git a/Project_3_Dynamic_Controller/include/context.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/context.h similarity index 100% rename from Project_3_Dynamic_Controller/include/context.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/context.h diff --git a/Project_3_Dynamic_Controller/include/control_timer.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/control_timer.h similarity index 100% rename from Project_3_Dynamic_Controller/include/control_timer.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/control_timer.h diff --git a/Project_3_Dynamic_Controller/include/controller.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/controller.h similarity index 100% rename from Project_3_Dynamic_Controller/include/controller.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/controller.h diff --git a/Project_3_Dynamic_Controller/include/digital_in.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/digital_in.h similarity index 100% rename from Project_3_Dynamic_Controller/include/digital_in.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/digital_in.h diff --git a/Project_3_Dynamic_Controller/include/digital_out.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/digital_out.h similarity index 100% rename from Project_3_Dynamic_Controller/include/digital_out.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/digital_out.h diff --git a/Project_3_Dynamic_Controller/include/encoder.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/encoder.h similarity index 100% rename from Project_3_Dynamic_Controller/include/encoder.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/encoder.h diff --git a/Project_3_Dynamic_Controller/include/initialization.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/initialization.h similarity index 100% rename from Project_3_Dynamic_Controller/include/initialization.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/initialization.h diff --git a/Project_3_Dynamic_Controller/include/motor_driver.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/motor_driver.h similarity index 100% rename from Project_3_Dynamic_Controller/include/motor_driver.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/motor_driver.h diff --git a/Project_3_Dynamic_Controller/include/operational.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/operational.h similarity index 98% rename from Project_3_Dynamic_Controller/include/operational.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/operational.h index 959e2c6..a4d1267 100644 --- a/Project_3_Dynamic_Controller/include/operational.h +++ b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/operational.h @@ -36,4 +36,4 @@ private: uint8_t logCounter; }; -#endif \ No newline at end of file +#endif diff --git a/Project_3_Dynamic_Controller/include/p_controller.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/p_controller.h similarity index 100% rename from Project_3_Dynamic_Controller/include/p_controller.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/p_controller.h diff --git a/Project_3_Dynamic_Controller/include/pi_controller.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/pi_controller.h similarity index 100% rename from Project_3_Dynamic_Controller/include/pi_controller.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/pi_controller.h diff --git a/Project_3_Dynamic_Controller/include/pre_operational.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/pre_operational.h similarity index 100% rename from Project_3_Dynamic_Controller/include/pre_operational.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/pre_operational.h diff --git a/Project_3_Dynamic_Controller/include/speed_controller.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/speed_controller.h similarity index 100% rename from Project_3_Dynamic_Controller/include/speed_controller.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/speed_controller.h diff --git a/Project_3_Dynamic_Controller/include/speed_estimator.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/speed_estimator.h similarity index 100% rename from Project_3_Dynamic_Controller/include/speed_estimator.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/speed_estimator.h diff --git a/Project_3_Dynamic_Controller/include/state.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/state.h similarity index 100% rename from Project_3_Dynamic_Controller/include/state.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/state.h diff --git a/Project_3_Dynamic_Controller/include/stopped.h b/Project_3_Dynamic_Controller/Part_1_Init_Operational/include/stopped.h similarity index 100% rename from Project_3_Dynamic_Controller/include/stopped.h rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/include/stopped.h diff --git a/Project_3_Dynamic_Controller/lib/README b/Project_3_Dynamic_Controller/Part_1_Init_Operational/lib/README similarity index 100% rename from Project_3_Dynamic_Controller/lib/README rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/lib/README diff --git a/Project_3_Dynamic_Controller/platformio.ini b/Project_3_Dynamic_Controller/Part_1_Init_Operational/platformio.ini similarity index 100% rename from Project_3_Dynamic_Controller/platformio.ini rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/platformio.ini diff --git a/Project_3_Dynamic_Controller/src/context.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/context.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/context.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/context.cpp diff --git a/Project_3_Dynamic_Controller/src/control_timer.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/control_timer.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/control_timer.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/control_timer.cpp diff --git a/Project_3_Dynamic_Controller/src/digital_in.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/digital_in.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/digital_in.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/digital_in.cpp diff --git a/Project_3_Dynamic_Controller/src/digital_out.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/digital_out.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/digital_out.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/digital_out.cpp diff --git a/Project_3_Dynamic_Controller/src/encoder.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/encoder.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/encoder.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/encoder.cpp diff --git a/Project_3_Dynamic_Controller/src/initialization.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/initialization.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/initialization.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/initialization.cpp diff --git a/Project_3_Dynamic_Controller/src/main.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/main.cpp similarity index 83% rename from Project_3_Dynamic_Controller/src/main.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/main.cpp index f228c56..6d49190 100644 --- a/Project_3_Dynamic_Controller/src/main.cpp +++ b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/main.cpp @@ -15,9 +15,9 @@ #include #include -// -------------------------------------------------- +// -------------------- // Hardware / drivers -// -------------------------------------------------- +// -------------------- // Encoder: // PB0 = D8 @@ -49,9 +49,9 @@ Speed_controller speedController( pController ); -// -------------------------------------------------- +// -------------------- // State machine states -// -------------------------------------------------- +// -------------------- Initialization initialization( speedController, @@ -109,9 +109,9 @@ void setup() void loop() { - // ---------------------------------------------- + // ------------- // Serial events - // ---------------------------------------------- + // ------------- if (Serial.available() > 0) { @@ -130,12 +130,12 @@ void loop() } } - // ---------------------------------------------- + // ---------------------------- // Stable 250 Hz state activity - // ---------------------------------------------- + // ---------------------------- if (controlTimer.take_tick()) { stateMachine->do_work(); } -} \ No newline at end of file +} diff --git a/Project_3_Dynamic_Controller/src/motor_driver.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/motor_driver.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/motor_driver.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/motor_driver.cpp diff --git a/Project_3_Dynamic_Controller/src/operational.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/operational.cpp similarity index 99% rename from Project_3_Dynamic_Controller/src/operational.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/operational.cpp index 7b51df3..7d77bd7 100644 --- a/Project_3_Dynamic_Controller/src/operational.cpp +++ b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/operational.cpp @@ -81,4 +81,4 @@ void Operational::set_initialization_state( ) { initializationState = &state; -} \ No newline at end of file +} diff --git a/Project_3_Dynamic_Controller/src/p_controller.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/p_controller.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/p_controller.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/p_controller.cpp diff --git a/Project_3_Dynamic_Controller/src/pi_controller.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/pi_controller.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/pi_controller.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/pi_controller.cpp diff --git a/Project_3_Dynamic_Controller/src/pre_operational.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/pre_operational.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/pre_operational.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/pre_operational.cpp diff --git a/Project_3_Dynamic_Controller/src/speed_controller.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/speed_controller.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/speed_controller.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/speed_controller.cpp diff --git a/Project_3_Dynamic_Controller/src/speed_estimator.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/speed_estimator.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/speed_estimator.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/speed_estimator.cpp diff --git a/Project_3_Dynamic_Controller/src/stopped.cpp b/Project_3_Dynamic_Controller/Part_1_Init_Operational/src/stopped.cpp similarity index 100% rename from Project_3_Dynamic_Controller/src/stopped.cpp rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/src/stopped.cpp diff --git a/Project_3_Dynamic_Controller/test/README b/Project_3_Dynamic_Controller/Part_1_Init_Operational/test/README similarity index 100% rename from Project_3_Dynamic_Controller/test/README rename to Project_3_Dynamic_Controller/Part_1_Init_Operational/test/README diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/.gitignore b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part1_state.wsd b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part1_state.wsd new file mode 100644 index 0000000..ad04a18 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part1_state.wsd @@ -0,0 +1,21 @@ +@startuml + +[*] --> Initialization + +state Initialization +state Operational + +Initialization : entry / initialize parameters +Initialization : entry / send Boot-up +Initialization : do / wait for command + +Operational : entry / enable motor control +Operational : entry / LED ON +Operational : do / update speed controller at 250 Hz +Operational : exit / stop motor control +Operational : exit / LED OFF + +Initialization --> Operational : set operational / 'o' +Operational --> Initialization : reset / 'r' + +@enduml \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part2_state.wsd b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part2_state.wsd new file mode 100644 index 0000000..0a5400d --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/docs/diagrams/part2_state.wsd @@ -0,0 +1,30 @@ +@startuml + +[*] --> Initialization + +state Initialization +state Operational +state Stopped + +Initialization : entry / initialize parameters +Initialization : entry / send Boot-up +Initialization : do / wait for command + +Operational : entry / enable motor control +Operational : entry / LED ON +Operational : do / update speed controller at 250 Hz +Operational : exit / stop motor control +Operational : exit / LED OFF + +Stopped : entry / brake motor +Stopped : entry / disable normal controller activity +Stopped : do / LED blink at 2 Hz +Stopped : exit / LED OFF + +Initialization --> Operational : set operational / 'o' +Operational --> Initialization : reset / 'r' + +Operational --> Stopped : emergency stop / fault +Stopped --> Initialization : reset / 'r' + +@enduml \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/README b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/README new file mode 100644 index 0000000..49819c0 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/README @@ -0,0 +1,37 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the convention is to give header files names that end with `.h'. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/context.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/context.h new file mode 100644 index 0000000..941c07d --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/context.h @@ -0,0 +1,26 @@ +#ifndef CONTEXT_H +#define CONTEXT_H + +#include + +class Context +{ +public: + Context(State& initialState); + + void transition_to(State& newState); + + void do_work(); + + void reset(); + void set_operational(); + void set_pre_operational(); + void fault(); + + const char* current_state_name() const; + +private: + State* state; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/control_timer.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/control_timer.h new file mode 100644 index 0000000..9b00f4d --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/control_timer.h @@ -0,0 +1,17 @@ +#ifndef CONTROL_TIMER_H +#define CONTROL_TIMER_H + +#include + +class Control_timer +{ +public: + void init(); + + // Returns true once for every 4 ms control tick. + bool take_tick(); + + uint32_t tick_count() const; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/controller.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/controller.h new file mode 100644 index 0000000..d71a59a --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/controller.h @@ -0,0 +1,19 @@ +#ifndef CONTROLLER_H +#define CONTROLLER_H + +class Controller +{ +public: + virtual ~Controller() = default; + + virtual double update( + double reference, + double actual + ) = 0; + + virtual void reset() = 0; + + virtual const char* name() const = 0; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_in.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_in.h new file mode 100644 index 0000000..2218aad --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_in.h @@ -0,0 +1,22 @@ +#ifndef DIGITAL_IN_H +#define DIGITAL_IN_H + +#include + +class Digital_in +{ +public: + // pin is the PORTB bit number: + // PB0 = D8, PB1 = D9, ..., PB5 = D13 + Digital_in(uint8_t pin); + + void init(); + + bool is_hi() const; + bool is_lo() const; + +private: + uint8_t pinMask; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_out.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_out.h new file mode 100644 index 0000000..485541b --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/digital_out.h @@ -0,0 +1,23 @@ +#ifndef DIGITAL_OUT_H +#define DIGITAL_OUT_H + +#include + +class Digital_out +{ +public: + // pin is the PORTB bit number: + // PB0 = D8, ..., PB5 = D13 + Digital_out(uint8_t pin); + + void init(); + + void set_hi(); + void set_lo(); + void toggle(); + +private: + uint8_t pinMask; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/encoder.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/encoder.h new file mode 100644 index 0000000..84689a4 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/encoder.h @@ -0,0 +1,28 @@ +#ifndef ENCODER_H +#define ENCODER_H + +#include +#include + +class Encoder +{ +public: + Encoder(uint8_t c1Pin, uint8_t c2Pin); + + void init(); + void update(); + + int32_t position() const; + +private: + Digital_in c1; + Digital_in c2; + + uint8_t c1Pin; + + bool previousC1; + + volatile int32_t count; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/initialization.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/initialization.h new file mode 100644 index 0000000..2ce676c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/initialization.h @@ -0,0 +1,35 @@ +#ifndef INITIALIZATION_H +#define INITIALIZATION_H + +#include +#include +#include + +class Initialization : public State +{ +public: + Initialization( + Speed_controller& speedController, + Digital_out& statusLED + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void set_operational() override; + + void set_operational_state( + State& operationalState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + State* operationalState; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/motor_driver.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/motor_driver.h new file mode 100644 index 0000000..9e2c8b4 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/motor_driver.h @@ -0,0 +1,26 @@ +#ifndef MOTOR_DRIVER_H +#define MOTOR_DRIVER_H + +class Motor_driver +{ +public: + void init(); + + // Range: -255 ... +255 + void set_command(double command); + + void coast(); + + void brake(); + + double command() const; + +private: + double currentCommand; + + void enable_pwm_outputs(); + + unsigned char magnitude(double value) const; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/operational.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/operational.h new file mode 100644 index 0000000..271b9bd --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/operational.h @@ -0,0 +1,45 @@ +#ifndef OPERATIONAL_H +#define OPERATIONAL_H + +#include + +#include +#include +#include + +class Operational : public State +{ +public: + Operational( + Speed_controller& speedController, + Digital_out& statusLED + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void reset() override; + void fault() override; + + void set_initialization_state( + State& initializationState + ); + + void set_stopped_state( + State& stoppedState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + State* initializationState; + State* stoppedState; + + uint8_t logCounter; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/p_controller.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/p_controller.h new file mode 100644 index 0000000..70ba115 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/p_controller.h @@ -0,0 +1,29 @@ +#ifndef P_CONTROLLER_H +#define P_CONTROLLER_H + +#include + +class P_controller : public Controller +{ +public: + P_controller(double kp); + + double update( + double reference, + double actual + ) override; + + void reset() override; + + const char* name() const override; + + void set_kp(double kp); + double get_kp() const; + +private: + double kp; + + double limit(double value) const; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/pi_controller.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/pi_controller.h new file mode 100644 index 0000000..e69de29 diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/pre_operational.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/pre_operational.h new file mode 100644 index 0000000..e69de29 diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_controller.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_controller.h new file mode 100644 index 0000000..0377e8c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_controller.h @@ -0,0 +1,47 @@ +#ifndef SPEED_CONTROLLER_H +#define SPEED_CONTROLLER_H + +#include +#include +#include +#include + +class Speed_controller +{ +public: + Speed_controller( + Encoder& encoder, + Speed_estimator& estimator, + Motor_driver& motor, + Controller& controller + ); + + void start(); + void stop(); + void brake(); + + void update(); + + void set_reference(double rpm); + double reference() const; + + void set_controller(Controller& controller); + + double actual_rpm() const; + double control_output() const; + +private: + Encoder& encoder; + Speed_estimator& estimator; + Motor_driver& motor; + + Controller* controller; + + bool running; + + double referenceRPM; + double actualRPM; + double controlOutput; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_estimator.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_estimator.h new file mode 100644 index 0000000..b11057e --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/speed_estimator.h @@ -0,0 +1,37 @@ +#ifndef SPEED_ESTIMATOR_H +#define SPEED_ESTIMATOR_H + +#include + +class Speed_estimator +{ +public: + Speed_estimator( + double countsPerRevolution, + double sampleTimeSeconds + ); + + void reset(int32_t initialPosition); + + double update(int32_t currentPosition); + + double rpm() const; + +private: + static const uint8_t WINDOW_SIZE = 10; + + double countsPerRevolution; + double sampleTime; + + int32_t previousPosition; + + int32_t countHistory[WINDOW_SIZE]; + int32_t rollingCounts; + + uint8_t historyIndex; + uint8_t samplesFilled; + + double currentRPM; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/state.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/state.h new file mode 100644 index 0000000..f3a6600 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/state.h @@ -0,0 +1,43 @@ +#ifndef STATE_H +#define STATE_H + +class Context; + +class State +{ +protected: + Context* context_ = nullptr; + +public: + virtual ~State() = default; + + void set_context(Context* context) + { + context_ = context; + } + + virtual const char* name() const = 0; + + virtual void on_entry() = 0; + virtual void on_do() = 0; + virtual void on_exit() = 0; + + // Events + virtual void reset() + { + } + + virtual void set_operational() + { + } + + virtual void set_pre_operational() + { + } + + virtual void fault() + { + } +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/stopped.h b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/stopped.h new file mode 100644 index 0000000..bbab60c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/include/stopped.h @@ -0,0 +1,39 @@ +#ifndef STOPPED_H +#define STOPPED_H + +#include + +#include +#include +#include + +class Stopped : public State +{ +public: + Stopped( + Speed_controller& speedController, + Digital_out& statusLED + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void reset() override; + + void set_initialization_state( + State& initializationState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + State* initializationState; + + uint8_t ledCounter; +}; + +#endif \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/lib/README b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/lib/README new file mode 100644 index 0000000..9379397 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into the executable file. + +The source code of each library should be placed in a separate directory +("lib/your_library_name/[Code]"). + +For example, see the structure of the following example libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Example contents of `src/main.c` using Foo and Bar: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +The PlatformIO Library Dependency Finder will find automatically dependent +libraries by scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/platformio.ini b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/platformio.ini new file mode 100644 index 0000000..84867a1 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nanoatmega328new] +platform = atmelavr +board = nanoatmega328new +framework = arduino +monitor_speed = 115200 \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/context.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/context.cpp new file mode 100644 index 0000000..1c9e4a2 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/context.cpp @@ -0,0 +1,80 @@ +#include + +#include + +Context::Context(State& initialState) + : state(nullptr) +{ + transition_to(initialState); +} + +void Context::transition_to( + State& newState +) +{ + if (state != nullptr) + { + state->on_exit(); + + Serial.print("Transition: "); + Serial.print(state->name()); + Serial.print(" -> "); + Serial.println(newState.name()); + } + + state = &newState; + + state->set_context(this); + + state->on_entry(); +} + +void Context::do_work() +{ + if (state != nullptr) + { + state->on_do(); + } +} + +void Context::reset() +{ + if (state != nullptr) + { + state->reset(); + } +} + +void Context::set_operational() +{ + if (state != nullptr) + { + state->set_operational(); + } +} + +void Context::set_pre_operational() +{ + if (state != nullptr) + { + state->set_pre_operational(); + } +} + +void Context::fault() +{ + if (state != nullptr) + { + state->fault(); + } +} + +const char* Context::current_state_name() const +{ + if (state == nullptr) + { + return "None"; + } + + return state->name(); +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/control_timer.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/control_timer.cpp new file mode 100644 index 0000000..e3f7f62 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/control_timer.cpp @@ -0,0 +1,70 @@ +#include + +#include +#include +#include + +static volatile uint8_t pendingTicks = 0; +static volatile uint32_t totalTicks = 0; + +ISR(TIMER1_COMPA_vect) +{ + totalTicks++; + + if (pendingTicks < 255) + { + pendingTicks++; + } +} + +void Control_timer::init() +{ + pendingTicks = 0; + totalTicks = 0; + + // Timer1 CTC mode, 250 Hz + // 16 MHz / 64 / (999 + 1) = 250 Hz + + TCCR1A = 0; + TCCR1B = 0; + TCNT1 = 0; + + OCR1A = 999; + + TCCR1B |= (1 << WGM12); + + // Prescaler = 64 + TCCR1B |= + (1 << CS11) | + (1 << CS10); + + TIMSK1 |= (1 << OCIE1A); +} + +bool Control_timer::take_tick() +{ + bool tickAvailable = false; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + if (pendingTicks > 0) + { + pendingTicks--; + tickAvailable = true; + } + } + + return tickAvailable; +} + +uint32_t Control_timer::tick_count() const +{ + uint32_t ticks; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + ticks = totalTicks; + } + + return ticks; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_in.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_in.cpp new file mode 100644 index 0000000..4ac93ce --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_in.cpp @@ -0,0 +1,26 @@ +#include +#include + +Digital_in::Digital_in(uint8_t pin) +{ + pinMask = static_cast(1U << pin); +} + +void Digital_in::init() +{ + // Input + DDRB &= static_cast(~pinMask); + + // Internal pull-up + PORTB |= pinMask; +} + +bool Digital_in::is_hi() const +{ + return (PINB & pinMask) != 0; +} + +bool Digital_in::is_lo() const +{ + return (PINB & pinMask) == 0; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_out.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_out.cpp new file mode 100644 index 0000000..598ee0b --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/digital_out.cpp @@ -0,0 +1,27 @@ +#include +#include + +Digital_out::Digital_out(uint8_t pin) +{ + pinMask = static_cast(1U << pin); +} + +void Digital_out::init() +{ + DDRB |= pinMask; +} + +void Digital_out::set_hi() +{ + PORTB |= pinMask; +} + +void Digital_out::set_lo() +{ + PORTB &= static_cast(~pinMask); +} + +void Digital_out::toggle() +{ + PORTB ^= pinMask; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/encoder.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/encoder.cpp new file mode 100644 index 0000000..e497e54 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/encoder.cpp @@ -0,0 +1,75 @@ +#include + +#include +#include + +static Encoder* activeEncoder = nullptr; + +ISR(PCINT0_vect) +{ + if (activeEncoder != nullptr) + { + activeEncoder->update(); + } +} + +Encoder::Encoder(uint8_t c1Pin, uint8_t c2Pin) + : c1(c1Pin), + c2(c2Pin), + c1Pin(c1Pin), + previousC1(false), + count(0) +{ +} + +void Encoder::init() +{ + c1.init(); + c2.init(); + + activeEncoder = this; + + previousC1 = c1.is_hi(); + count = 0; + + // Clear pending PORTB pin-change interrupt + PCIFR |= (1 << PCIF0); + + // Enable PORTB pin-change interrupts + PCICR |= (1 << PCIE0); + + // Enable interrupt on C1 + PCMSK0 |= (1 << c1Pin); +} + +void Encoder::update() +{ + bool currentC1 = c1.is_hi(); + + if (currentC1 != previousC1) + { + bool currentC2 = c2.is_hi(); + + if (currentC1 == currentC2) + { + count++; + } + else + { + count--; + } + + previousC1 = currentC1; + } +} + +int32_t Encoder::position() const +{ + uint8_t oldSREG = SREG; + + cli(); + int32_t currentCount = count; + SREG = oldSREG; + + return currentCount; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/initialization.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/initialization.cpp new file mode 100644 index 0000000..3b83709 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/initialization.cpp @@ -0,0 +1,57 @@ +#include + +#include +#include + +Initialization::Initialization( + Speed_controller& speedController, + Digital_out& statusLED +) + : speedController(speedController), + statusLED(statusLED), + operationalState(nullptr) +{ +} + +const char* Initialization::name() const +{ + return "Initialization"; +} + +void Initialization::on_entry() +{ + speedController.stop(); + + statusLED.set_lo(); + + Serial.println(); + Serial.println("Initialization"); + Serial.println("Boot-up"); +} + +void Initialization::on_do() +{ + // No continuous activity required + // in Part 1 Initialization. +} + +void Initialization::on_exit() +{ +} + +void Initialization::set_operational() +{ + if (operationalState != nullptr) + { + context_->transition_to( + *operationalState + ); + } +} + +void Initialization::set_operational_state( + State& state +) +{ + operationalState = &state; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/main.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/main.cpp new file mode 100644 index 0000000..f406f6b --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/main.cpp @@ -0,0 +1,172 @@ +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +// -------------------------------------------------- +// Hardware / drivers +// -------------------------------------------------- + +// Encoder: +// PB0 = D8 +// PB1 = D9 +Encoder encoder(0, 1); + +// Emergency-stop: +// PB2 = D10 +// Internal pull-up enabled. +// LOW = emergency stop pressed. +Digital_in emergencyStop(2); + +// Status LED: +// PB5 = D13 +Digital_out statusLED(5); + +Control_timer controlTimer; + +Motor_driver motorDriver; + +// 2100 counts/revolution, +// 4 ms fixed controller period +Speed_estimator speedEstimator( + 2100.0, + 0.004 +); + +P_controller pController(40.0); + +Speed_controller speedController( + encoder, + speedEstimator, + motorDriver, + pController +); + +// -------------------------------------------------- +// State machine +// -------------------------------------------------- + +Initialization initialization( + speedController, + statusLED +); + +Operational operational( + speedController, + statusLED +); + +Stopped stopped( + speedController, + statusLED +); + +Context* stateMachine = nullptr; + +void setup() +{ + Serial.begin(115200); + + statusLED.init(); + + emergencyStop.init(); + + encoder.init(); + + motorDriver.init(); + + speedController.set_reference( + 50.0 + ); + + // -------------------------------------------------- + // State transitions + // -------------------------------------------------- + + initialization.set_operational_state( + operational + ); + + operational.set_initialization_state( + initialization + ); + + operational.set_stopped_state( + stopped + ); + + stopped.set_initialization_state( + initialization + ); + + static Context context( + initialization + ); + + stateMachine = &context; + + controlTimer.init(); + + Serial.println(); + Serial.println("Project 3 - Part 2"); + Serial.println("Commands:"); + Serial.println("o = set operational"); + Serial.println("r = reset"); + Serial.println("D10 LOW = emergency stop"); +} + +void loop() +{ + // -------------------------------------------------- + // Emergency-stop fault + // -------------------------------------------------- + + if (emergencyStop.is_lo()) + { + stateMachine->fault(); + } + + // -------------------------------------------------- + // Serial commands + // -------------------------------------------------- + + if (Serial.available() > 0) + { + char command = + Serial.read(); + + if (command == 'o') + { + stateMachine-> + set_operational(); + } + else if (command == 'r') + { + stateMachine-> + reset(); + } + } + + // -------------------------------------------------- + // Stable 250 Hz state activity + // -------------------------------------------------- + + if (controlTimer.take_tick()) + { + stateMachine->do_work(); + } +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/motor_driver.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/motor_driver.cpp new file mode 100644 index 0000000..6f247e6 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/motor_driver.cpp @@ -0,0 +1,135 @@ +#include + +#include + +void Motor_driver::init() +{ + currentCommand = 0.0; + + // D11 / PB3 / OC2A -> AIN1 + DDRB |= (1 << PB3); + + // D3 / PD3 / OC2B -> AIN2 + DDRD |= (1 << PD3); + + PORTB &= ~(1 << PB3); + PORTD &= ~(1 << PD3); + + // Timer2 Fast PWM + TCCR2A = 0; + TCCR2B = 0; + TCNT2 = 0; + + OCR2A = 0; + OCR2B = 0; + + TCCR2A = + (1 << COM2A1) | + (1 << COM2B1) | + (1 << WGM21) | + (1 << WGM20); + + // Prescaler = 8 + // PWM frequency ~= 7.8 kHz + TCCR2B = + (1 << CS21); +} + +void Motor_driver::enable_pwm_outputs() +{ + // Non-inverting PWM on OC2A and OC2B + TCCR2A |= + (1 << COM2A1) | + (1 << COM2B1); + + TCCR2A &= + ~((1 << COM2A0) | + (1 << COM2B0)); +} + +unsigned char Motor_driver::magnitude( + double value +) const +{ + if (value < 0.0) + { + value = -value; + } + + if (value > 255.0) + { + value = 255.0; + } + + return static_cast(value); +} + +void Motor_driver::set_command(double command) +{ + if (command > 255.0) + { + command = 255.0; + } + + if (command < -255.0) + { + command = -255.0; + } + + currentCommand = command; + + enable_pwm_outputs(); + + unsigned char pwm = + magnitude(command); + + if (command > 0.0) + { + // Same direction that previously gave positive RPM: + // AIN1 = LOW + // AIN2 = PWM + OCR2A = 0; + OCR2B = pwm; + } + else if (command < 0.0) + { + OCR2A = pwm; + OCR2B = 0; + } + else + { + OCR2A = 0; + OCR2B = 0; + } +} + +void Motor_driver::coast() +{ + currentCommand = 0.0; + + enable_pwm_outputs(); + + OCR2A = 0; + OCR2B = 0; +} + +void Motor_driver::brake() +{ + currentCommand = 0.0; + + // Disconnect Timer2 PWM from the pins. + TCCR2A &= + ~((1 << COM2A1) | + (1 << COM2A0) | + (1 << COM2B1) | + (1 << COM2B0)); + + // DRV8833: both inputs HIGH gives braking. + PORTB |= (1 << PB3); + PORTD |= (1 << PD3); +} + +double Motor_driver::command() const +{ + return currentCommand; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/operational.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/operational.cpp new file mode 100644 index 0000000..4d55988 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/operational.cpp @@ -0,0 +1,103 @@ +#include + +#include +#include + +Operational::Operational( + Speed_controller& speedController, + Digital_out& statusLED +) + : speedController(speedController), + statusLED(statusLED), + initializationState(nullptr), + stoppedState(nullptr), + logCounter(0) +{ +} + +const char* Operational::name() const +{ + return "Operational"; +} + +void Operational::on_entry() +{ + Serial.println("Operational"); + + statusLED.set_hi(); + + logCounter = 0; + + speedController.start(); +} + +void Operational::on_do() +{ + speedController.update(); + + logCounter++; + + // Print every 40 ms + if (logCounter >= 10) + { + logCounter = 0; + + Serial.print("Reference: "); + Serial.print( + speedController.reference() + ); + + Serial.print(" RPM, Actual: "); + Serial.print( + speedController.actual_rpm() + ); + + Serial.print(" RPM, Control: "); + Serial.println( + speedController.control_output() + ); + } +} + +void Operational::on_exit() +{ + speedController.stop(); + + statusLED.set_lo(); +} + +void Operational::reset() +{ + if (initializationState != nullptr) + { + context_->transition_to( + *initializationState + ); + } +} + +void Operational::fault() +{ + if (stoppedState != nullptr) + { + Serial.println("FAULT detected"); + + context_->transition_to( + *stoppedState + ); + } +} + +void Operational::set_initialization_state( + State& state +) +{ + initializationState = &state; +} + +void Operational::set_stopped_state( + State& state +) +{ + stoppedState = &state; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/p_controller.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/p_controller.cpp new file mode 100644 index 0000000..40df055 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/p_controller.cpp @@ -0,0 +1,55 @@ +#include + +P_controller::P_controller(double kp) + : kp(kp) +{ +} + +double P_controller::update( + double reference, + double actual +) +{ + double error = + reference - actual; + + double output = + kp * error; + + return limit(output); +} + +void P_controller::reset() +{ + // P controller has no stored dynamic state. +} + +const char* P_controller::name() const +{ + return "P"; +} + +void P_controller::set_kp(double newKp) +{ + kp = newKp; +} + +double P_controller::get_kp() const +{ + return kp; +} + +double P_controller::limit(double value) const +{ + if (value > 255.0) + { + return 255.0; + } + + if (value < 0.0) + { + return 0.0; + } + + return value; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/pi_controller.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/pi_controller.cpp new file mode 100644 index 0000000..e69de29 diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/pre_operational.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/pre_operational.cpp new file mode 100644 index 0000000..e69de29 diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_controller.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_controller.cpp new file mode 100644 index 0000000..992055c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_controller.cpp @@ -0,0 +1,106 @@ +#include + +Speed_controller::Speed_controller( + Encoder& encoder, + Speed_estimator& estimator, + Motor_driver& motor, + Controller& controller +) + : encoder(encoder), + estimator(estimator), + motor(motor), + controller(&controller), + running(false), + referenceRPM(0.0), + actualRPM(0.0), + controlOutput(0.0) +{ +} + +void Speed_controller::start() +{ + estimator.reset( + encoder.position() + ); + + controller->reset(); + + actualRPM = 0.0; + controlOutput = 0.0; + + running = true; + + motor.coast(); +} + +void Speed_controller::stop() +{ + running = false; + + controlOutput = 0.0; + + motor.coast(); +} + +void Speed_controller::brake() +{ + running = false; + + controlOutput = 0.0; + + motor.brake(); +} + +void Speed_controller::update() +{ + if (!running) + { + return; + } + + actualRPM = + estimator.update( + encoder.position() + ); + + controlOutput = + controller->update( + referenceRPM, + actualRPM + ); + + motor.set_command( + controlOutput + ); +} + +void Speed_controller::set_reference( + double rpm +) +{ + referenceRPM = rpm; +} + +double Speed_controller::reference() const +{ + return referenceRPM; +} + +void Speed_controller::set_controller( + Controller& newController +) +{ + controller = &newController; + + controller->reset(); +} + +double Speed_controller::actual_rpm() const +{ + return actualRPM; +} + +double Speed_controller::control_output() const +{ + return controlOutput; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_estimator.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_estimator.cpp new file mode 100644 index 0000000..a61d6ba --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/speed_estimator.cpp @@ -0,0 +1,76 @@ +#include + +Speed_estimator::Speed_estimator( + double countsPerRevolution, + double sampleTimeSeconds +) + : countsPerRevolution(countsPerRevolution), + sampleTime(sampleTimeSeconds), + previousPosition(0), + rollingCounts(0), + historyIndex(0), + samplesFilled(0), + currentRPM(0.0) +{ + for (uint8_t i = 0; i < WINDOW_SIZE; i++) + { + countHistory[i] = 0; + } +} + +void Speed_estimator::reset(int32_t initialPosition) +{ + previousPosition = initialPosition; + + rollingCounts = 0; + historyIndex = 0; + samplesFilled = 0; + currentRPM = 0.0; + + for (uint8_t i = 0; i < WINDOW_SIZE; i++) + { + countHistory[i] = 0; + } +} + +double Speed_estimator::update(int32_t currentPosition) +{ + int32_t deltaCount = + currentPosition - previousPosition; + + previousPosition = currentPosition; + + rollingCounts -= + countHistory[historyIndex]; + + countHistory[historyIndex] = + deltaCount; + + rollingCounts += deltaCount; + + historyIndex++; + + if (historyIndex >= WINDOW_SIZE) + { + historyIndex = 0; + } + + if (samplesFilled < WINDOW_SIZE) + { + samplesFilled++; + } + + double windowTime = + samplesFilled * sampleTime; + + currentRPM = + (rollingCounts * 60.0) / + (countsPerRevolution * windowTime); + + return currentRPM; +} + +double Speed_estimator::rpm() const +{ + return currentRPM; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/stopped.cpp b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/stopped.cpp new file mode 100644 index 0000000..ae1355e --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/src/stopped.cpp @@ -0,0 +1,75 @@ +#include + +#include +#include + +Stopped::Stopped( + Speed_controller& speedController, + Digital_out& statusLED +) + : speedController(speedController), + statusLED(statusLED), + initializationState(nullptr), + ledCounter(0) +{ +} + +const char* Stopped::name() const +{ + return "Stopped"; +} + +void Stopped::on_entry() +{ + Serial.println("Stopped"); + Serial.println("Emergency stop - motor brake active"); + + // Immediately stop normal controller operation + // and actively brake the motor. + speedController.brake(); + + statusLED.set_hi(); + + ledCounter = 0; +} + +void Stopped::on_do() +{ + // State activity runs every 4 ms. + // + // 2 Hz LED: + // full period = 500 ms + // toggle approximately every 250 ms + // + // 250 ms / 4 ms = 62.5 ticks + + ledCounter++; + + if (ledCounter >= 63) + { + ledCounter = 0; + statusLED.toggle(); + } +} + +void Stopped::on_exit() +{ + statusLED.set_lo(); +} + +void Stopped::reset() +{ + if (initializationState != nullptr) + { + context_->transition_to( + *initializationState + ); + } +} + +void Stopped::set_initialization_state( + State& state +) +{ + initializationState = &state; +} \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/test/README b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_2_Stopped_EStop/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/.gitignore b/Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/platformio.ini b/Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/platformio.ini new file mode 100644 index 0000000..84867a1 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_3_PI_Controller_TDD/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nanoatmega328new] +platform = atmelavr +board = nanoatmega328new +framework = arduino +monitor_speed = 115200 \ No newline at end of file diff --git a/Project_3_Dynamic_Controller/Part_4_PreOperational_Final/.gitignore b/Project_3_Dynamic_Controller/Part_4_PreOperational_Final/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_4_PreOperational_Final/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Project_3_Dynamic_Controller/Part_4_PreOperational_Final/platformio.ini b/Project_3_Dynamic_Controller/Part_4_PreOperational_Final/platformio.ini new file mode 100644 index 0000000..84867a1 --- /dev/null +++ b/Project_3_Dynamic_Controller/Part_4_PreOperational_Final/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nanoatmega328new] +platform = atmelavr +board = nanoatmega328new +framework = arduino +monitor_speed = 115200 \ No newline at end of file