From 5a71cbf87170cb1461ab8d4c4c34e44d9a204f0c Mon Sep 17 00:00:00 2001 From: joshua-8 Date: Sat, 26 Nov 2022 01:43:08 -0500 Subject: [PATCH] speed PID, warning blink --- .gitignore | 3 +- .vscode/settings.json | 3 +- platformio.ini | 2 ++ src/font.h | 6 +++- src/pid.h | 78 ++++++++++++++++++++++++++++++++++++++++ src/src.ino | 83 +++++++++++++++++++++++++++++++++++++------ src/timer.h | 6 +++- 7 files changed, 166 insertions(+), 15 deletions(-) create mode 100644 src/pid.h diff --git a/.gitignore b/.gitignore index 2b16c31..2e84197 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,5 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch -.DS_Store \ No newline at end of file +.DS_Store +src/secret.h diff --git a/.vscode/settings.json b/.vscode/settings.json index e554f53..7d38b4c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,6 +3,7 @@ "INTFLAG", "PIEZO", "setpoint", - "spinup" + "spinup", + "ssid" ] } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 1d3999c..d10ffca 100644 --- a/platformio.ini +++ b/platformio.ini @@ -21,3 +21,5 @@ framework = arduino monitor_speed = 115200 lib_deps = fastled/FastLED@3.5.0 ; https://github.com/FastLED/FastLED/ + arduino-libraries/WiFi101@0.16.1 + diff --git a/src/font.h b/src/font.h index 4d8bd36..682fe82 100644 --- a/src/font.h +++ b/src/font.h @@ -1,3 +1,6 @@ +#ifndef FONT_H +#define FONT_H + #include #include @@ -262,4 +265,5 @@ const char font[] = { 0x00, 0x19, 0x1D, 0x17, 0x12, // 0xFD 0x00, 0x3C, 0x3C, 0x3C, 0x3C, // 0xFE 0x00, 0x00, 0x00, 0x00, 0x00 // 0xFF -}; \ No newline at end of file +}; +#endif \ No newline at end of file diff --git a/src/pid.h b/src/pid.h new file mode 100644 index 0000000..714a8a9 --- /dev/null +++ b/src/pid.h @@ -0,0 +1,78 @@ +#ifndef PID_H +#define PID_H +#include +// extern char debugText[]; +/** + * @brief A class implementing PID control. + * @note Uses 32 bit integer math instead of floats. + */ +class PID { +public: + int32_t K; + int32_t F; + int32_t P; + int32_t I; + int32_t D; + + int32_t out_low; + int32_t out_high; + uint8_t out_devisor_pow; + + int32_t sum_error; + int32_t last_calc_micros; + int32_t last_error; + +public: + PID() + { + sum_error = 0; + last_calc_micros = 0; + last_error = 0; + + K = 0; + F = 0; + P = 0; + I = 0; + D = 0; + out_low = 0; + out_high = 0; + out_devisor_pow = 0; + } + PID(int32_t k, int32_t f, int32_t p, int32_t i, int32_t d, int32_t _out_low, int32_t _out_high, uint8_t _out_devisor_pow) + { + PID(); + K = k; + F = f; + P = p; + I = i; + D = d; + out_low = _out_low; + out_high = _out_high; + out_devisor_pow = _out_devisor_pow; + } + void initialize_time(unsigned long micros) + { + last_calc_micros = micros; + } + int32_t calculate(int32_t setpoint, int32_t input, unsigned long micros) + { + unsigned long dt = micros - last_calc_micros; + last_calc_micros = micros; + + int32_t error = setpoint - input; + + sum_error += I * error * (int32_t)dt / 1000000; + + int64_t output = P * (error) + (sum_error) + D * (error - last_error) / (int32_t)dt; // PID + + output += K + setpoint * F; // add constant offset, and feedforward terms + + // sprintf(debugText, "error: %li, input: %li, motor: %li, sumerr: %li", error, input, (int32_t)((int32_t)output / (int32_t)(1 << out_devisor_pow)), sum_error); // TODO: remove + + last_error = error; + + return constrain((int32_t)((int32_t)output / (int32_t)(1 << out_devisor_pow)), out_low, out_high); + } +}; + +#endif // PID_H diff --git a/src/src.ino b/src/src.ino index e67268f..bdfc06e 100644 --- a/src/src.ino +++ b/src/src.ino @@ -2,13 +2,24 @@ #include "FastLED.h" //https://github.com/FastLED/FastLED/ #include "font.h" +#include "pid.h" +#include "secret.h" #include "timer.h" +#include +#include + +PID motorPid; + +// char debugText[200]; const unsigned long wait_interval_micros = 2000000; const unsigned long spin_down_time_micros = 1500000; -unsigned long speed_setpoint_k_rpm = 1000 * 12; const unsigned long spinup_timeout = 5000000; -const unsigned int spinup_divider = 4000; +const unsigned int spinup_divider = 2000; + +const uint8_t speed_unit_devisor_power = 12; +const uint32_t speed_unit_devisor = (1 << speed_unit_devisor_power); +int32_t speed_setpoint = speed_unit_devisor * 12 / 1; // the second two numbers represent a fractional Rotations Per Second value const byte START_BUTTON_PIN = 1; const byte STOP_BUTTON_PIN = 0; @@ -22,7 +33,7 @@ const byte LEDS_CLOCK_PIN = 9; const byte image_height = 8; CRGB leds[image_height]; -const int image_width = 125; +const int image_width = 125; // do not set to above 2100 (causes overflow in calculation of isrRate) CRGB staged_image[image_width][image_height] = { 0 }; CRGB current_image[image_width][image_height]; volatile bool staged_image_new; // we want this to be atomic @@ -31,6 +42,10 @@ volatile unsigned long last_beam_break_micros; volatile int column_counter; volatile unsigned long start_micros; // variable for FSM +int status = WL_IDLE_STATUS; // the WiFi radio's status +WiFiServer server(23); +WiFiClient client; + enum State { s01_MOTOR_OFF = 1, s02_WAIT = 2, @@ -62,6 +77,8 @@ void setup() FastLED.addLeds(leds, image_height); // https://learn.sparkfun.com/tutorials/lumenati-hookup-guide#example-using-a-samd21-mini-breakout + motorPid = PID(0, 18, 100, 10, 0, 0, 255, speed_unit_devisor_power); + staged_image_new = false; last_rotation_micros = 0; column_counter = 0; @@ -73,6 +90,20 @@ void setup() fsm_input.start_button = false; fsm_input.stop_button = false; + // Serial.begin(115200); + // delay(5000); + // Serial.print("Attempting to connect to: "); + // Serial.println(wifi_ssid); + // WiFi.hostname("spin-clock"); + // status = WiFi.begin(wifi_ssid, wifi_pass); // wifi + // while (status != WL_CONNECTED) { + // delay(1000); + // } + // IPAddress ip = WiFi.localIP(); + // Serial.print("IP Address: "); + // Serial.println(ip); + // server.begin(); + setupTimer(); attachInterrupt(BEAM_BREAK_PIN, beamBreakIsr, FALLING); @@ -103,6 +134,12 @@ void loop() staged_image_new = true; + // client = server.available(); + // if (client) { + // client.flush(); + // client.println(debugText); + // } + delay(100); } @@ -126,6 +163,8 @@ State updateFSM(State state, FsmInput fsm_input) last_rotation_micros = 0; start_micros = fsm_input.micros; state = State::s03_SPINNING_UP; + } else { // 2-2 self loop + dangerBlink(); } return state; break; @@ -134,17 +173,19 @@ State updateFSM(State state, FsmInput fsm_input) analogWrite(MOTOR_CTRL_PIN, 0); tone(PIEZO_PIN, 1000); state = State::s05_SPINNING_DOWN; - } else if (fsm_input.rotation_interval != 0 && fsm_input.rotation_interval <= 1000000 * 1000 / speed_setpoint_k_rpm) { // transition 3-4 + } else if (fsm_input.rotation_interval != 0 && fsm_input.rotation_interval <= (int64_t)1000000 * speed_unit_devisor / speed_setpoint) { // transition 3-4 // fast enough noTone(PIEZO_PIN); state = State::s04_RUNNING; + motorPid.initialize_time(fsm_input.micros); } else if (fsm_input.micros - start_micros > spinup_timeout) { // transition 3-5b timeout analogWrite(MOTOR_CTRL_PIN, 0); tone(PIEZO_PIN, 1000); state = State::s05_SPINNING_DOWN; - } else { - analogWrite(MOTOR_CTRL_PIN, constrain((fsm_input.micros - start_micros) / spinup_divider, 0, 255)); + } else { // self loop + dangerBlink(); + analogWrite(MOTOR_CTRL_PIN, constrain((fsm_input.micros - start_micros) / spinup_divider, 0, 255)); // ramp to full power } return state; break; @@ -156,15 +197,19 @@ State updateFSM(State state, FsmInput fsm_input) FastLED.clear(true); state = State::s05_SPINNING_DOWN; } else { // 4-4 self loop - // TODO: speed control loop - analogWrite(MOTOR_CTRL_PIN, 220); + int32_t speed = (int64_t)1000000 * speed_unit_devisor / fsm_input.rotation_interval; + int32_t motor_control = motorPid.calculate(speed_setpoint, speed, fsm_input.micros); + analogWrite(MOTOR_CTRL_PIN, motor_control); } return state; break; case State::s05_SPINNING_DOWN: if (fsm_input.micros - fsm_input.last_beam_break > spin_down_time_micros) { noTone(PIEZO_PIN); + FastLED.clear(true); state = State::s01_MOTOR_OFF; + } else { + dangerBlink(); } return state; break; @@ -194,12 +239,18 @@ void beamBreakIsr() memcpy(current_image, staged_image, sizeof(CRGB) * image_width * image_height); staged_image_new = false; } - setTimerISRRate((int32_t)image_width * 1000000 / last_rotation_micros); // TODO: ENSURE NOT OUT OF RANGE OR DIV/0 (minimum frequency is 30Hz) + if (last_beam_break_micros == 0) { // shouldn't happen, but protects from div/0 + stopTimerInterrupts(); + return; + } + int32_t isrRate = (int32_t)image_width * 1000000 / last_rotation_micros; + isrRate = max(30, isrRate); // minimum frequency that setTimerISRRate supports is 30Hz + setTimerISRRate(isrRate); } } -void TC3_Handler() -{ // timerISR +void TC3_Handler() // timerISR +{ int temp_column_counter = constrain(column_counter, 0, image_width - 1); for (int i = 0; i < image_height; i++) { leds[i] = current_image[temp_column_counter][i]; @@ -231,3 +282,13 @@ void startButtonIsr() fsm_input.start_button = true; state = updateFSM(state, fsm_input); } + +void dangerBlink() +{ + static bool blinkVar = false; + blinkVar = !blinkVar; + for (int i = 0; i < image_height; i++) { + leds[i] = (blinkVar) ? CRGB(255, 100, 0) : CRGB(0, 0, 0); + } + FastLED.show(); +} diff --git a/src/timer.h b/src/timer.h index 0bbdd14..2667b8c 100644 --- a/src/timer.h +++ b/src/timer.h @@ -1,7 +1,10 @@ +#ifndef TIMER_H +#define TIMER_H + #include // Timer code -------------------------------- // -const int CLOCKFREQ = 1000000; // clock divider of 4 +const int CLOCKFREQ = 1000000; // unlike the lab, here we use a clock divider of 4 to allow for slower speeds void setupTimer() { @@ -69,3 +72,4 @@ void stopTimerInterrupts() ; // Serial.println("Timer interrupts stopped!"); } +#endif \ No newline at end of file