From b66196b184ba48ec2b74b4058d211e8b776230ea Mon Sep 17 00:00:00 2001 From: jenoack Date: Fri, 31 Mar 2023 10:47:41 +0200 Subject: [PATCH] updates start procedure and PI --- .../Unendlichkeitsmaschine/include/json.hpp | 8 +- .../Unendlichkeitsmaschine/include/main.hpp | 14 +- vscode/Unendlichkeitsmaschine/src/main.cpp | 121 ++++++++++-------- 3 files changed, 79 insertions(+), 64 deletions(-) diff --git a/vscode/Unendlichkeitsmaschine/include/json.hpp b/vscode/Unendlichkeitsmaschine/include/json.hpp index e969131..8377dd0 100644 --- a/vscode/Unendlichkeitsmaschine/include/json.hpp +++ b/vscode/Unendlichkeitsmaschine/include/json.hpp @@ -2,6 +2,7 @@ #define JSON_HPP #include +#include #include #include @@ -9,7 +10,7 @@ typedef struct { unsigned long timestamp; double integ; double derivative; - double esc_output; + int esc_output; double speed; double current_speed; double err; @@ -19,8 +20,9 @@ typedef struct { double kd; int max_speed; int min_speed; - double max_esc_out; - double min_esc_out; + int max_esc_out; + int min_esc_out; + bool start_done; }PIDStruct; typedef struct diff --git a/vscode/Unendlichkeitsmaschine/include/main.hpp b/vscode/Unendlichkeitsmaschine/include/main.hpp index 51cb8ce..b45e88f 100644 --- a/vscode/Unendlichkeitsmaschine/include/main.hpp +++ b/vscode/Unendlichkeitsmaschine/include/main.hpp @@ -55,7 +55,7 @@ Servo ESC; // Der ESC-Controller (Electronic Speed Controller bzw. elektroni Oled display; const unsigned long MAX_TIME_PER_TURN_MS = 200; -const unsigned long UPDATE_ESC_EVERY_MS = 100; +const unsigned long UPDATE_ESC_EVERY_MS = 50; const uint8_t HALL_SENSORS_COUNT = 8; // input, sec, min, hour, day, week, month, year const uint8_t ALL_DATA_COUNT = HALL_SENSORS_COUNT; @@ -107,11 +107,11 @@ const uint8_t TIME_PER_ROUND_VALS = 10; const uint8_t MAX_ESC_OUT = 100; const uint8_t MIN_ESC_OUT = 0; -const long MAX_D_VALUE = 10; -const long MIN_D_VALUE = -10; -const long MAX_I_VALUE = 10; -const long MIN_I_VALUE = -10; -const long MAX_P_VALUE = 10; -const long MIN_P_VALUE = -10; +const long MAX_D_VALUE = 50; +const long MIN_D_VALUE = -50; +const long MAX_I_VALUE = 50; +const long MIN_I_VALUE = -50; +const long MAX_P_VALUE = 50; +const long MIN_P_VALUE = -50; #endif //MAIN_HPP \ No newline at end of file diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp index d5be3c0..5f88dc4 100644 --- a/vscode/Unendlichkeitsmaschine/src/main.cpp +++ b/vscode/Unendlichkeitsmaschine/src/main.cpp @@ -27,6 +27,7 @@ void speed_set(); void eval_max_min_speed(); void PID(); +void limit_values(double calc_value, double *pvalue, double factor, double min_limit , double max_limit); unsigned long time_per_round_calc(); void count_secs(time_t* run_time); @@ -60,8 +61,8 @@ void setup() Serial.println("Init ETC ..."); ESC.attach(ESC_PIN,1000,2000); display.progressBar(0,"Init controller", true); - ESC.write(180); - delay(1000); + ESC.write(20); + delay(2000); display.progressBar(1,"Connect ETC!", "Please connnect the ETC", "controller to power!", true); delay(5000); display.progressBar(2,"Calibrating ETC", "You may hear some beeps.", "If not - check ETC connection.", true); @@ -104,6 +105,11 @@ void setup() Serial.println(" Measuring min and max speed values ... "); eval_max_min_speed(); + #ifdef HAS_MQTT + display.progressBar(100,"Init done!", "Connection to MQTT ...", true); + #else + display.progressBar(100,"Init done!", "Starting ...", true); + #endif Serial.println(" Auf gehts ... "); } @@ -117,11 +123,11 @@ void loop() data_check(); data_store(); + display.show_values(Pid_params.speed, Pid_params.min_speed, Pid_params.max_speed, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP()); + #ifdef HAS_MQTT mqtt.send_and_loop("infinity/data", json.create_json_string(AllData, ALL_DATA_COUNT, &Pid_params, run_time)); #endif - - display.show_values(Pid_params.speed, Pid_params.min_speed, Pid_params.max_speed, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP()); } @@ -130,7 +136,7 @@ void eval_max_min_speed() display.progressBar(0,"Min/Max measure!", true); Pid_params.min_esc_out = 0; Pid_params.min_speed = 0; - for(uint8_t out = Pid_params.min_speed; out < Pid_params.max_esc_out; out++) + for(int out = Pid_params.min_speed; out < Pid_params.max_esc_out; out++) { ESC.write(out); String esc_str = String("esc_out = " + String(out)); @@ -161,7 +167,11 @@ void eval_max_min_speed() Pid_params.max_speed = HallData[0].value; display.progressBar(100,"Max speed/esc:", String("max_speed =" + String(Pid_params.max_speed) ).c_str(), String("max_esc_out = " + String(Pid_params.max_esc_out)).c_str(), true); ESC.write(0); - Serial.printf(">min_speed:%d\n>max_speed:%d\n>min_esc_out:%d\n>max_esc_out:%d\n",Pid_params.min_speed, Pid_params.max_speed, Pid_params.min_esc_out, Pid_params.max_esc_out ); + Serial.printf(">min_speed:%d\n>max_speed:%d\n>min_esc_out:%f\n>max_esc_out:%f\n", + Pid_params.min_speed, + Pid_params.max_speed, + Pid_params.min_esc_out, + Pid_params.max_esc_out ); delay(2000); } @@ -179,72 +189,66 @@ void count_secs(time_t* run_time) } } -void PID() +void limit_values(double calc_value, double *pvalue, double factor, double min_limit , double max_limit) { - Pid_params.current_speed = HallData[0].value; - - if(Pid_params.ki==0) + if(factor != 0) { - Pid_params.integ = 0; - } - if(Pid_params.kd==0) - { - Pid_params.derivative = 0; - } - - Pid_params.err = constrain(Pid_params.speed - Pid_params.current_speed, -1*Pid_params.max_speed, Pid_params.current_speed); - double int_integ = Pid_params.integ + Pid_params.err; - double int_derivative = Pid_params.err - last_error; - if(Pid_params.ki != 0) - { - if(Pid_params.ki*int_integ > MAX_I_VALUE) + if(factor*calc_value > max_limit) { - Pid_params.integ = MAX_I_VALUE/Pid_params.ki; + *pvalue = max_limit/factor; } - else if(Pid_params.ki*int_integ < MIN_I_VALUE) + else if(factor*calc_value < min_limit) { - Pid_params.integ = MIN_I_VALUE/Pid_params.ki; + *pvalue = min_limit/factor; } else { - Pid_params.integ = int_integ; + *pvalue = calc_value; } } else { - Pid_params.integ = 0; + *pvalue = 0; } +} - if(Pid_params.kd != 0) - { - if(Pid_params.kd*int_derivative > MAX_D_VALUE) - { - Pid_params.derivative = MAX_D_VALUE/Pid_params.kd; - } - else if(Pid_params.kd*int_derivative < MIN_D_VALUE) - { - Pid_params.derivative = MIN_D_VALUE/Pid_params.kd; - } - else - { - Pid_params.derivative = int_derivative; - } - } - else - { - Pid_params.derivative = 0; - } +void PID() +{ + static unsigned long last_pid_ms = millis(); + + Pid_params.current_speed = HallData[0].value; + Pid_params.err = constrain(Pid_params.speed - Pid_params.current_speed, -1*Pid_params.max_speed, Pid_params.max_speed); + limit_values(Pid_params.integ + Pid_params.err, &Pid_params.integ, Pid_params.ki, MIN_I_VALUE , MAX_I_VALUE); + limit_values(Pid_params.err - last_error, &Pid_params.derivative, Pid_params.kd, MIN_D_VALUE , MAX_D_VALUE); + last_error = Pid_params.err; Pid_params.output = Pid_params.kp * Pid_params.err + Pid_params.ki * Pid_params.integ + Pid_params.kd * Pid_params.derivative; if(-1 == ser_esc_output ) { - Pid_params.esc_output = constrain(Pid_params.output, Pid_params.min_esc_out, Pid_params.max_esc_out); + if(Pid_params.start_done == true) + { + last_pid_ms = millis(); + Pid_params.esc_output = int(Pid_params.min_esc_out + constrain(Pid_params.output, 0, Pid_params.max_esc_out - Pid_params.min_esc_out)); + } + else + { + double rise = (double(Pid_params.max_esc_out - Pid_params.min_esc_out)/ double(Pid_params.max_speed - Pid_params.min_speed)); + double offset = double(Pid_params.max_esc_out) - rise*Pid_params.max_speed; + Pid_params.esc_output = int(rise*Pid_params.speed + offset); + Pid_params.derivative = 0; + Pid_params.integ = 0; + Serial.printf("Start speed ... rise=%6.3f offset=%6.3f esc_out=%d\n", rise, offset, Pid_params.esc_output); + if((millis() - last_pid_ms > 2000) || (abs(Pid_params.current_speed - Pid_params.speed)>= (0.1*Pid_params.speed)) ) + { + Pid_params.start_done = true; + } + } } else { Pid_params.esc_output = ser_esc_output; } - last_error = Pid_params.err; + } unsigned long time_per_round_calc() @@ -308,7 +312,13 @@ void speed_set() if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS) { PID(); - Serial.printf(">speed:%06.3f\n>current:%06.3f\n>esc:%06.3f\n>prop:%06.3f\n>derive:%06.3f\n>integ:%06.3f\n",Pid_params.speed, Pid_params.current_speed, Pid_params.esc_output, Pid_params.err, Pid_params.derivative, Pid_params.integ); + Serial.printf(">speed:%06.3f\n>current:%06.3f\n>esc:%06d\n>prop:%06.3f\n>derive:%06.3f\n>integ:%06.3f\n", + Pid_params.speed, + Pid_params.current_speed, + Pid_params.esc_output, + Pid_params.err * Pid_params.kp, + Pid_params.derivative* Pid_params.kd, + Pid_params.integ* Pid_params.ki); //Serial.printf(">speed:%06.3f\n>current:%06.3f\n>esc:%06.3f\n",Pid_params.speed, Pid_params.current_speed, Pid_params.esc_output); ESC.write(Pid_params.esc_output); last_esc_write_ms = millis(); @@ -319,21 +329,23 @@ void speed_set() void data_init() { Serial.println("Init internal data ..."); + //kp=0.006000 ki=0.002000 kd=0.006000 Pid_params.timestamp = millis(); Pid_params.integ = 0.0; Pid_params.derivative = 0.0; - Pid_params.esc_output = 0.0; + Pid_params.esc_output = 0; Pid_params.speed = 1000.0; Pid_params.current_speed = 0.0; Pid_params.err = 0; Pid_params.output = 0.0; - Pid_params.kp = 0.002; - Pid_params.ki = 0.003; - Pid_params.kd = 0.001; + Pid_params.kp = 0.006; + Pid_params.ki = 0.002; + Pid_params.kd = 0.006; Pid_params.max_speed = 5000; Pid_params.min_speed = 0; Pid_params.max_esc_out = MAX_ESC_OUT; Pid_params.min_esc_out = MIN_ESC_OUT; + Pid_params.start_done = false; #ifdef MACHINE_RESTART run_time = 0; start_time = capportal.epochTime(); @@ -448,6 +460,7 @@ void get_serial_cmd() Serial.printf("Set ki to %f\n", Pid_params.ki); } else if(command.startsWith("speed=")){ + Pid_params.start_done = false; Pid_params.speed = command.substring(command.indexOf("=")+1).toDouble(); Serial.printf("Set speed to %f\n", Pid_params.speed); }