diff --git a/.gitignore b/.gitignore index 83834db..e4654ca 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,8 @@ __pycache__ *.csv *.typ *.pdf +scripts +*.bak # Vélarsértækar skrár sem PlatformIO býr til sjálft .vscode/c_cpp_properties.json diff --git a/docs/diagrams/src/motor_state.wsd b/docs/diagrams/src/motor_state.wsd index 5a26b55..6f04fa7 100644 --- a/docs/diagrams/src/motor_state.wsd +++ b/docs/diagrams/src/motor_state.wsd @@ -20,6 +20,7 @@ state PreOperational PreOperational : Entry / Stop motor PreOperational : Do / Blink LED at 1 Hz PreOperational : Do / Accept Kp and Ti configuration + PreOperational : Do / Select P or PI controller [c] PreOperational : Exit / LED off state Operational @@ -32,17 +33,18 @@ state Stopped Stopped : Do / Blink LED at 2 Hz Stopped : Exit / LED off -Initialization --> Operational : initialization complete +Initialization -r-> PreOperational : initialization\n complete +' Initialization -d-> Operational : initialization\n complete -PreOperational --> Operational : set operational [o] -PreOperational --> Initialization : reset [r] +PreOperational -d-> Operational : set operational [o] +PreOperational -l--> Initialization : reset [r] -Operational --> PreOperational : set pre-operational [p] -Operational --> Initialization : reset [r] +Operational -u-> PreOperational : set pre-operational [p] +Operational -> Initialization : reset [r] Operational --> Stopped : fault detected -Stopped --> Initialization : reset [r] -Stopped --> Operational : set operational [o] -Stopped --> PreOperational : set pre-operational [p] +Stopped -l-> Initialization : reset [r] +Stopped -> Operational : set operational [o] +Stopped -u-> PreOperational : set pre-operational [p] @enduml diff --git a/include/motor_controller.h b/include/motor_controller.h index d6ec394..f81ec22 100644 --- a/include/motor_controller.h +++ b/include/motor_controller.h @@ -25,12 +25,16 @@ private: RotaryEncoderDriver& encoder; MotorDriver& motor; - P_controller& controller; + P_controller* controller; Digital_out led; double reference_rpm; + // Síðasta sýni, notað fyrir CSV-útprentun + double last_actual_rpm; + double last_pwm; + public: MotorController( @@ -64,7 +68,12 @@ public: bool fault_active(); - void run_speed_controller(); + // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna + bool run_speed_controller(); + + double get_reference_rpm() const { return reference_rpm; } + double get_actual_rpm() const { return last_actual_rpm; } + double get_pwm() const { return last_pwm; } void stop_motor(); @@ -76,6 +85,8 @@ public: void set_Ti(double value); + void set_controller(P_controller& new_controller); + void led_on(); void led_off(); diff --git a/include/operational_state.h b/include/operational_state.h index f66ef07..f213ce9 100644 --- a/include/operational_state.h +++ b/include/operational_state.h @@ -2,10 +2,20 @@ #define OPERATIONAL_STATE_H #include "state.h" +#include + +// Sett á 1 til að prenta CSV (time_ms,ref_rpm,actual_rpm,pwm) í Operational, +// t.d. fyrir step og load response. Einnig hægt með build_flags = -D LOG_CSV=1 +#ifndef LOG_CSV +#define LOG_CSV 0 +#endif class OperationalState : public State { +private: + uint32_t sample_count = 0; + public: void enter() override; diff --git a/include/pre_operational_state.h b/include/pre_operational_state.h index 7d968f6..7ad5cc6 100644 --- a/include/pre_operational_state.h +++ b/include/pre_operational_state.h @@ -4,9 +4,15 @@ #include "state.h" #include "stdint.h" +class P_controller; + class PreOperationalState : public State { private: + P_controller& p_controller; + P_controller& pi_controller; + bool pi_selected; + unsigned long previous_time; bool led_state; @@ -26,7 +32,7 @@ private: uint8_t input_index; public: - PreOperationalState(); + PreOperationalState(P_controller& p_controller, P_controller& pi_controller); void enter() override; void update() override; diff --git a/platformio.ini b/platformio.ini index 3730474..e0d8b0b 100644 --- a/platformio.ini +++ b/platformio.ini @@ -17,9 +17,5 @@ monitor_speed = 115200 test_framework = unity test_build_src = yes - -; [env:native] -; platform = native -; test_framework = unity -; test_build_src = yes -; build_src_filter = -<*> + + +; CSV-útprentun í Operational fyrir step/load response (sjá capture_serial.py) +; build_flags = -D LOG_CSV=1 diff --git a/src/initialization_state.cpp b/src/initialization_state.cpp index 58e4bf0..70d5d5f 100644 --- a/src/initialization_state.cpp +++ b/src/initialization_state.cpp @@ -25,6 +25,7 @@ void InitializationState::update() if (millis() - enter_time_ms >= WAIT_MS) { context->to_pre_operational(); + // context->to_operational(); } } diff --git a/src/main.cpp b/src/main.cpp index af7f497..4dc9246 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,27 +22,27 @@ RotaryEncoderDriver encoder; MotorDriver motor; -const double Kp = 12.5; -const double Ti = 1.0; +const double Kp_P = 10.0; // P-controller from Project 2 + +const double Kp = 10.125; // Ziegler-Nichols PI +const double Ti = 0.0334; const double T = 0.004; -// P_controller controller(Kp); -PI_controller controller(Kp, Ti, T); +P_controller p_controller(Kp_P); +PI_controller pi_controller(Kp, Ti, T); InitializationState initialization_state; - -PreOperationalState pre_operational_state; - +// Pre-operational velur á milli stýringanna með 'c' +PreOperationalState pre_operational_state(p_controller, pi_controller); OperationalState operational_state; - StoppedState stopped_state; // Stöðurnar verða að vera skilgreindar á undan motor_controller MotorController motor_controller( encoder, motor, - controller, + pi_controller, // Sjálfgefin stýring initialization_state, pre_operational_state, operational_state, @@ -53,38 +53,24 @@ MotorController motor_controller( int main() { init(); - Serial.begin(115200); - - cli(); encoder.begin(); - motor.begin(); sei(); - - // motor_controller.set_states( - // &initialization_state, - // &operational_state, - // &stopped_state - // ); - motor_controller.begin(); - while (true) { if (Serial.available() > 0) { char c = Serial.read(); - //if (c != '\n' && c != '\r') // Tók þetta út svo það - //{ // svo það væri hægt að setja - motor_controller.command(c); // inn gildi fyrir Kp og Ti - //} + motor_controller.command(c); + } @@ -95,7 +81,4 @@ int main() return 0; } -// int main(){ -// return 0; -// } -#endif // PIO_UNIT_TESTING +#endif diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index d2043e3..9d31ea1 100644 --- a/src/motor_controller.cpp +++ b/src/motor_controller.cpp @@ -20,9 +20,11 @@ MotorController::MotorController( stopped_state(stopped), encoder(encoder), motor(motor), - controller(controller), + controller(&controller), led(LED_PIN), - reference_rpm(DEFAULT_REFERENCE_RPM) + reference_rpm(DEFAULT_REFERENCE_RPM), + last_actual_rpm(0.0), + last_pwm(0.0) { initialization_state.set_context(this); pre_operational_state.set_context(this); @@ -100,20 +102,37 @@ void MotorController::to_operational() } -void MotorController::run_speed_controller() +bool MotorController::run_speed_controller() { - if (encoder.new_speed_available()) + if (!encoder.new_speed_available()) { - double actual = encoder.rpm(); - - double pwm_percent = - controller.update(reference_rpm, actual); - - double pwm = - pwm_percent * PWM_PER_PERCENT; - - motor.set(pwm); + return false; } + + double actual = encoder.rpm(); + + double pwm_percent = + controller->update(reference_rpm, actual); + + double pwm = + pwm_percent * PWM_PER_PERCENT; + + // Klippt hér svo last_pwm sé nákvæmlega það sem fer á mótorinn + if (pwm > 255.0) + { + pwm = 255.0; + } + else if (pwm < -255.0) + { + pwm = -255.0; + } + + motor.set(pwm); + + last_actual_rpm = actual; + last_pwm = pwm; + + return true; } @@ -130,17 +149,24 @@ void MotorController::reset_speed_measurement() void MotorController::reset_controller() { - controller.reset(); + controller->reset(); } void MotorController::set_Kp(double value) { - controller.set_Kp(value); + controller->set_Kp(value); } void MotorController::set_Ti(double value) { - controller.set_Ti(value); + controller->set_Ti(value); +} + +void MotorController::set_controller(P_controller& new_controller) +{ + controller = &new_controller; + + controller->reset(); } void MotorController::to_stopped() diff --git a/src/operational_state.cpp b/src/operational_state.cpp index 72fcc69..59c6673 100644 --- a/src/operational_state.cpp +++ b/src/operational_state.cpp @@ -11,6 +11,12 @@ void OperationalState::enter() context->reset_controller(); context->led_on(); + + sample_count = 0; + +#if LOG_CSV + Serial.println("time_ms,ref_rpm,actual_rpm,pwm"); +#endif } // Part 1 @@ -33,7 +39,21 @@ void OperationalState::update() return; } - context->run_speed_controller(); + if (context->run_speed_controller()) + { + sample_count++; + +#if LOG_CSV + // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational + Serial.print(sample_count * 4UL); + Serial.print(','); + Serial.print(context->get_reference_rpm(), 1); + Serial.print(','); + Serial.print(context->get_actual_rpm(), 2); + Serial.print(','); + Serial.println(context->get_pwm(), 1); +#endif + } } diff --git a/src/pre_operational_state.cpp b/src/pre_operational_state.cpp index e74b385..3bb4342 100644 --- a/src/pre_operational_state.cpp +++ b/src/pre_operational_state.cpp @@ -4,8 +4,11 @@ #include #include -PreOperationalState::PreOperationalState() - : previous_time(0), +PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller) + : p_controller(p_controller), + pi_controller(pi_controller), + pi_selected(true), + previous_time(0), led_state(false), //waiting_for_kp(false), //waiting_for_ti(false), @@ -133,6 +136,23 @@ void PreOperationalState::command(char c) Serial.println("Enter Ti and press Enter:"); } + + // Select controller algorithm, the controller is passed to the motor controller + else if (c == 'c' || c == 'C') + { + pi_selected = !pi_selected; + + if (pi_selected) + { + context->set_controller(pi_controller); + Serial.println("Controller: PI"); + } + else + { + context->set_controller(p_controller); + Serial.println("Controller: P"); + } + } } void PreOperationalState::exit() diff --git a/src/stopped_state.cpp b/src/stopped_state.cpp index c531a3b..421d3bd 100644 --- a/src/stopped_state.cpp +++ b/src/stopped_state.cpp @@ -19,7 +19,7 @@ void StoppedState::enter() led_state = false; - digitalWrite(LED_BUILTIN, LOW); + context->led_off(); previous_time = millis(); } @@ -36,10 +36,14 @@ void StoppedState::update() led_state = !led_state; - digitalWrite( - LED_BUILTIN, - led_state ? HIGH : LOW - ); + if (led_state) + { + context->led_on(); + } + else + { + context->led_off(); + } } } @@ -73,5 +77,5 @@ void StoppedState::command(char c) void StoppedState::exit() { - digitalWrite(LED_BUILTIN, LOW); + context->led_off(); } \ No newline at end of file