jenoack/Unendlichkeitsmaschine
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

PID

Browse source
This commit is contained in:
jenoack 2023-03-30 18:11:30 +02:00
parent 5db2e22563
commit 5e9046f13e
2 changed files with 31 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
vscode/Unendlichkeitsmaschine/include/json.hpp
View file

@ -20,7 +20,7 @@ typedef struct {
int max_speed; int max_speed;
int min_speed; int min_speed;
double max_esc_out; double max_esc_out;
double min_esc_out double min_esc_out;
}PIDStruct; }PIDStruct;
typedef struct typedef struct

41
vscode/Unendlichkeitsmaschine/src/main.cpp
View file

@ -24,6 +24,7 @@ void data_store();
void data_check(); void data_check();
void data_init(); void data_init();
void speed_set(); void speed_set();
void eval_max_min_speed();
void PID(); void PID();
unsigned long time_per_round_calc(); unsigned long time_per_round_calc();
@ -73,6 +74,7 @@ void setup()
ESC.write(0); ESC.write(0);
display.progressBar(100,"ETC init done!", true); display.progressBar(100,"ETC init done!", true);
delay(1000); delay(1000);
display.progressBar(100,"Init data and IRQ!", true);
data_init(); data_init();
#ifndef DUMMY_DATA #ifndef DUMMY_DATA
@ -98,6 +100,10 @@ void setup()
attachInterrupt(digitalPinToInterrupt(HALL8_PIN), ISR_HALL8, FALLING); attachInterrupt(digitalPinToInterrupt(HALL8_PIN), ISR_HALL8, FALLING);
#endif #endif
Serial.println(" Measuring min and max speed values ... ");
eval_max_min_speed();
Serial.println(" Auf gehts ... "); Serial.println(" Auf gehts ... ");
} }
@ -115,35 +121,47 @@ void loop()
mqtt.send_and_loop("infinity/data", json.create_json_string(AllData, ALL_DATA_COUNT, &Pid_params, run_time)); mqtt.send_and_loop("infinity/data", json.create_json_string(AllData, ALL_DATA_COUNT, &Pid_params, run_time));
#endif #endif
display.show_values(Pid_params.speed, MIN_SPEED, MAX_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP()); display.show_values(Pid_params.speed, Pid_params.min_speed, Pid_params.max_speed, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP());
} }
void eval_max_min_speed() void eval_max_min_speed()
{ {
display.progressBar(0,"Min/Max measure!", true);
Pid_params.min_esc_out = 0; Pid_params.min_esc_out = 0;
Pid_params.min_speed = 0; Pid_params.min_speed = 0;
for(uint8_t out = Pid_params.min_speed; out < Pid_params.max_esc_out; out++) for(uint8_t out = Pid_params.min_speed; out < Pid_params.max_esc_out; out++)
{ {
ESC.write(out); ESC.write(out);
String esc_str = String("esc_out = " + String(out));
String speed_str = String("speed_out = " + String(HallData[0].value));
delay(1000); delay(1000);
for(uint8_t i=0;i<10;i++) for(uint8_t i=0;i<10;i++)
{ {
display.progressBar(out*10+i,"Min/Max measure!", esc_str.c_str(), speed_str.c_str(), true);
data_check(); data_check();
delay(1000); delay(100);
if (0 == Pid_params.min_speed && HallData[0].value > 0) if (0 == Pid_params.min_speed && HallData[0].value > 0)
{ {
Pid_params.min_speed = HallData[0].value; Pid_params.min_speed = HallData[0].value;
Pid_params.min_esc_out = out Pid_params.min_esc_out = out;
break; break;
} }
} }
if (Pid_params.min_speed > 0)
{
break;
}
} }
display.progressBar(100,"Min speed/esc:", String("min_speed =" + String(Pid_params.min_speed) ).c_str(), String("min_esc_out = " + String(Pid_params.min_esc_out)).c_str(), true);
ESC.write(Pid_params.max_esc_out); ESC.write(Pid_params.max_esc_out);
delay(2000); delay(2000);
data_check();
Pid_params.max_speed = HallData[0].value; 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); 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 );
delay(2000); delay(2000);
} }
@ -174,7 +192,7 @@ void PID()
Pid_params.derivative = 0; Pid_params.derivative = 0;
} }
Pid_params.err = constrain(Pid_params.speed - Pid_params.current_speed, -100, 100); 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_integ = Pid_params.integ + Pid_params.err;
double int_derivative = Pid_params.err - last_error; double int_derivative = Pid_params.err - last_error;
if(Pid_params.ki != 0) if(Pid_params.ki != 0)
@ -220,7 +238,7 @@ void PID()
Pid_params.output = Pid_params.kp * Pid_params.err + Pid_params.ki * Pid_params.integ + Pid_params.kd * Pid_params.derivative; 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 ) if(-1 == ser_esc_output )
{ {
Pid_params.esc_output = constrain(Pid_params.output, MIN_ESC_SPEED, MAX_ESC_SPEED); Pid_params.esc_output = constrain(Pid_params.output, Pid_params.min_esc_out, Pid_params.max_esc_out);
} }
else else
{ {
@ -290,7 +308,8 @@ void speed_set()
if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS) if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS)
{ {
PID(); PID();
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); 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:%06.3f\n",Pid_params.speed, Pid_params.current_speed, Pid_params.esc_output);
ESC.write(Pid_params.esc_output); ESC.write(Pid_params.esc_output);
last_esc_write_ms = millis(); last_esc_write_ms = millis();
Pid_params.timestamp = last_esc_write_ms; Pid_params.timestamp = last_esc_write_ms;
@ -308,8 +327,8 @@ void data_init()
Pid_params.current_speed = 0.0; Pid_params.current_speed = 0.0;
Pid_params.err = 0; Pid_params.err = 0;
Pid_params.output = 0.0; Pid_params.output = 0.0;
Pid_params.kp = 0.001; Pid_params.kp = 0.002;
Pid_params.ki = 0.001; Pid_params.ki = 0.003;
Pid_params.kd = 0.001; Pid_params.kd = 0.001;
Pid_params.max_speed = 5000; Pid_params.max_speed = 5000;
Pid_params.min_speed = 0; Pid_params.min_speed = 0;
@ -384,7 +403,7 @@ void data_check()
else else
{ {
HallData[nr].value = HallData[nr].unit_factor / mid_time; HallData[nr].value = HallData[nr].unit_factor / mid_time;
HallData[nr].value = HallData[nr].value > MAX_SPEED? MAX_SPEED:HallData[nr].value; HallData[nr].value = HallData[nr].value > Pid_params.max_speed? Pid_params.max_speed:HallData[nr].value;
} }
} }
} }
@ -450,11 +469,11 @@ void get_serial_cmd()
Serial.printf("Set to drive PID control mode.\n"); Serial.printf("Set to drive PID control mode.\n");
} }
else if(command.equals("max")){ else if(command.equals("max")){
ser_esc_output = MAX_ESC_SPEED; ser_esc_output = Pid_params.max_esc_out;
Serial.printf("Set to maximal speed. %f\n", ser_esc_output); Serial.printf("Set to maximal speed. %f\n", ser_esc_output);
} }
else if(command.equals("min")){ else if(command.equals("min")){
ser_esc_output = MIN_ESC_SPEED; ser_esc_output = Pid_params.min_esc_out;
Serial.printf("Set to minimal speed %f .\n", ser_esc_output ); Serial.printf("Set to minimal speed %f .\n", ser_esc_output );
} }
else if(command.equals("start")){ else if(command.equals("start")){