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

Changed PID so that the output will not overflow

Browse source
This commit is contained in:
Jens Noack 2023-03-19 19:18:23 +01:00
parent 3a88aac74a
commit 42c59a01b4
2 changed files with 54 additions and 29 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
vscode/Unendlichkeitsmaschine/include/main.hpp
View file

@ -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;

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

@ -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);
@ -130,7 +133,6 @@ void loop()
display.show_values(speed, MIN_SPEED, MAX_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP());
}
void count_secs(time_t* run_time)
@ -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()
{