From 42c59a01b41065ea77574babe3ab289d4908bbf8 Mon Sep 17 00:00:00 2001 From: jenoack Date: Sun, 19 Mar 2023 19:18:23 +0100 Subject: [PATCH] Changed PID so that the output will not overflow --- .../Unendlichkeitsmaschine/include/main.hpp | 1 + vscode/Unendlichkeitsmaschine/src/main.cpp | 82 ++++++++++++------- 2 files changed, 54 insertions(+), 29 deletions(-) diff --git a/vscode/Unendlichkeitsmaschine/include/main.hpp b/vscode/Unendlichkeitsmaschine/include/main.hpp index 90d96aa..9a35163 100644 --- a/vscode/Unendlichkeitsmaschine/include/main.hpp +++ b/vscode/Unendlichkeitsmaschine/include/main.hpp @@ -60,6 +60,7 @@ const unsigned long UPDATE_ESC_EVERY_MS = 0; const uint8_t HALL_SENSORS_COUNT = 8; // input, sec, min, hour, day, week, month, year const uint8_t ALL_DATA_COUNT = HALL_SENSORS_COUNT; +const uint8_t HALL_NR_TURNS = 2; ///number of turns we wait before measuring each time const uint8_t HALL_TICKS_PER_TURN = 4; const uint8_t HALL_MIN_PULSE_MS = 50; diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp index 12bfcda..605a220 100644 --- a/vscode/Unendlichkeitsmaschine/src/main.cpp +++ b/vscode/Unendlichkeitsmaschine/src/main.cpp @@ -19,9 +19,9 @@ volatile double goal_speed = 0; volatile double current_speed = 0; volatile double err = 0; volatile double output = 0.0; -double kp = 1;//0.005; -double ki = 1000;//0.0008; -double kd = 5;//0.0001; +double kp = 0.005; +double ki = 0.0008; +double kd = 0.0001; //bei 2000 als speed: values: kp=0.020000 ki=0.005000 kd=0.000100 @@ -41,6 +41,7 @@ void data_check(); void data_init(); void speed_get(); void speed_set(); +void PID(); void count_secs(time_t* run_time); void get_serial_cmd(); @@ -80,10 +81,12 @@ void setup() data_init(); // Initialise timer + /* timer = timerBegin(0, 80, true); // Timer 0, Prescaler 80, zähle im Up-Modus timerAttachInterrupt(timer, &ISR_onTimer, true); // Setze den Interrupt-Handler und aktiviere den Interrupt auf steigende Flanke timerAlarmWrite(timer, 1000000/8, true); // Setze den Alarm auf 1/4 Sekunde und aktiviere ihn timerAlarmEnable(timer); // Aktiviere den Alarm + */ pinMode(39, INPUT_PULLDOWN); pinMode(HALL1_PIN, INPUT); @@ -129,7 +132,6 @@ void loop() #endif display.show_values(speed, MIN_SPEED, MAX_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP()); - } @@ -147,11 +149,52 @@ void count_secs(time_t* run_time) } } +double speed_map(double x, double in_min, double in_max, double out_min, double out_max) { + const double dividend = out_max - out_min; + const double divisor = in_max - in_min; + const double delta = x - in_min; + if(divisor == 0){ + log_e("Invalid map input range, min == max"); + return -1; //AVR returns -1, SAM returns 0 + } + return (delta * dividend + (divisor / 2)) / divisor + out_min; +} + +void PID() +{ + current_speed = HallData[0].value; + + //current_speed = map(HallData[0].value, MIN_SPEED, MAX_SPEED, MIN_ESC_SPEED, MAX_ESC_SPEED); + + double int_err = goal_speed - current_speed; + double int_integ = integ + int_err; + double int_derivative = (int_err - last_error); + double int_output = kp * int_err + ki * int_integ + kd * int_derivative; + if((int_output >= MIN_SPEED) && (int_output <= MAX_SPEED)) + { + err = int_err; + integ = int_integ; + derivative = int_derivative; + output = int_output; + } + + //Beschränkung der Ausgabe + esc_output = speed_map(output, MIN_SPEED, MAX_SPEED, MIN_ESC_SPEED, MAX_ESC_SPEED); + + //Serial.printf("PID ... (goal: %f, current: %06.3f, err: %06.3f, integ: %06.3f, derive: %06.3f) output=%06.3f \n", goal_speed, current_speed, err, integ, derivative, output); + + //Aktualisierung der letzten Fehler- und Zeitwerte + last_error = err; +} + + + void speed_set() { static unsigned long last_esc_write_ms = 0; if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS) { + PID(); Serial.printf(">goal:%06.3f\n>current:%06.3f\n>err:%06.3f\n>integ:%06.3f\n>derive:%06.3f\n>output:%06.3f\n>esc_output:%06.3f\n", goal_speed, current_speed, err, integ, derivative, output, esc_output); if(-1 == ser_esc_output ) { @@ -281,7 +324,7 @@ void data_check() && (HallData[nr].prev_update_ms != HallData[nr].act_update_ms) ) { - HallData[nr].value = HallData[nr].unit_factor / (HallData[nr].act_update_ms - HallData[nr].prev_update_ms) ; + HallData[nr].value = HallData[nr].unit_factor / ((HallData[nr].act_update_ms - HallData[nr].prev_update_ms)/ HALL_NR_TURNS) ; } } else{ @@ -309,6 +352,7 @@ void data_store() } } + void get_serial_cmd() { if(Serial.available()){ @@ -344,11 +388,11 @@ void get_serial_cmd() } else if(command.equals("max")){ ser_esc_output = MAX_ESC_SPEED; - Serial.printf("Set to maximal speed.\n"); + Serial.printf("Set to maximal speed. %f\n", ser_esc_output); } else if(command.equals("min")){ ser_esc_output = MIN_ESC_SPEED; - Serial.printf("Set to minimal speed.\n"); + Serial.printf("Set to minimal speed %f .\n", ser_esc_output ); } else if(command.equals("start")){ ser_esc_output = -1; @@ -359,7 +403,7 @@ void get_serial_cmd() Serial.printf("Stop drive.\n"); } else if(command.equals("help")){ - Serial.printf("Commands: kp,ki,kd=x.xxxx | params | control | max | min | start | stop | speed=x | help \n"); + Serial.printf("Commands: kp,ki,kd=x.xxxx | params | control | max | min | start | stop | speed=x | esc=x help \n"); } else{ Serial.printf("Invalid command."); @@ -375,7 +419,7 @@ void IRAM_ATTR ISR_HALL1() HallData[hallnr].timestamp = run_time; // each 4 ticks is one turn // so we calculate speed each 4 ticks... just in case we check for bigger ... - if(HallData[hallnr].period_ticks >= HALL_TICKS_PER_TURN){ + if(HallData[hallnr].period_ticks >= HALL_NR_TURNS*HALL_TICKS_PER_TURN){ HallData[hallnr].period_ticks = 0; HallData[hallnr].prev_update_ms = HallData[hallnr].act_update_ms; HallData[hallnr].act_update_ms = millis(); @@ -438,26 +482,6 @@ void IRAM_ATTR ISR_HALL8() HallData[hallnr].act_update_ms = millis(); } -void IRAM_ATTR ISR_onTimer() -{ - current_speed = HallData[0].value; - - //current_speed = map(HallData[0].value, MIN_SPEED, MAX_SPEED, MIN_ESC_SPEED, MAX_ESC_SPEED); - - err = goal_speed - current_speed; - integ = integ + err; - derivative = (err - last_error); - output = kp * err + ki * integ + kd * derivative; - - //Beschränkung der Ausgabe - esc_output = MIN_ESC_SPEED + constrain(output, 0, MAX_ESC_SPEED - MIN_ESC_SPEED); - - //Serial.printf("PID ... (goal: %f, current: %06.3f, err: %06.3f, integ: %06.3f, derive: %06.3f) output=%06.3f \n", goal_speed, current_speed, err, integ, derivative, output); - - //Aktualisierung der letzten Fehler- und Zeitwerte - last_error = err; -} - //this code is just for generating dummy data - in normal use not needed void data_generate() {