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

adpated paramters and PID

Browse source
This commit is contained in:
Jens Noack 2023-03-23 15:14:01 +01:00
parent c1e5cdfff1
commit 89a6180181
2 changed files with 71 additions and 76 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -2,7 +2,7 @@
#define MAIN_HPP #define MAIN_HPP
//#define MACHINE_RESTART //#define MACHINE_RESTART
//#define DUMMY_DATA #define DUMMY_DATA
#define EEPROM_NAME_TICK "hall_tick" #define EEPROM_NAME_TICK "hall_tick"
#define EEPROM_NAME_TIMESTAMP "timestamp" #define EEPROM_NAME_TIMESTAMP "timestamp"
@ -103,10 +103,10 @@ const uint8_t HALL8_PIN = 14;
const uint8_t TIME_PER_ROUND_VALS = 10; const uint8_t TIME_PER_ROUND_VALS = 10;
const uint16_t MIN_SPEED = 800; const uint16_t MIN_SPEED = 500;
const uint16_t MAX_SPEED = 5000; const uint16_t MAX_SPEED = 5000;
const uint8_t MAX_ESC_SPEED = 100; const uint8_t MAX_ESC_SPEED = 180;
const uint8_t MIN_ESC_SPEED = 10; const uint8_t MIN_ESC_SPEED = 0;
const uint16_t MAX_POTI_VALUE = 4095; const uint16_t MAX_POTI_VALUE = 4095;
const uint16_t JITTER_POTI_PERCENT = 10; const uint16_t JITTER_POTI_PERCENT = 10;

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

@ -21,9 +21,10 @@ volatile double goal_speed = 0;
volatile double current_speed = 0; volatile double current_speed = 0;
volatile double err = 0; volatile double err = 0;
volatile double output = 0.0; volatile double output = 0.0;
double kp = 0.005; double kp = 0.03;
double ki = 0.0008; double ki = 0.01;
double kd = 0.0001; double kd = 0.002;
//bei 2000 als speed: values: kp=0.020000 ki=0.005000 kd=0.000100 //bei 2000 als speed: values: kp=0.020000 ki=0.005000 kd=0.000100
@ -44,7 +45,7 @@ void data_init();
void speed_get(); void speed_get();
void speed_set(); void speed_set();
void PID(); void PID();
double time_per_round_calc(); unsigned long time_per_round_calc();
void count_secs(time_t* run_time); void count_secs(time_t* run_time);
void get_serial_cmd(); void get_serial_cmd();
@ -63,16 +64,6 @@ void setup()
display.begin(); display.begin();
/*
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->setFont(ArialMT_Plain_16);
display->drawString(64, 2, "Init controller");
display->setFont(ArialMT_Plain_10);
display->drawString(64, 40, "You may hear some beeps.");
display->drawString(64, 52, "That's OK! ;-)");
display->display();
*/
capportal.begin(); capportal.begin();
html_content = "<!DOCTYPE html><html><head><meta http-equiv=\"refresh\" content=\"0; url='/_ac'\" /></head><body></body></html>"; html_content = "<!DOCTYPE html><html><head><meta http-equiv=\"refresh\" content=\"0; url='/_ac'\" /></head><body></body></html>";
@ -101,13 +92,7 @@ void setup()
delay(1000); delay(1000);
data_init(); data_init();
// Initialise timer #ifndef DUMMY_DATA
/*
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(39, INPUT_PULLDOWN);
pinMode(HALL1_PIN, INPUT); pinMode(HALL1_PIN, INPUT);
@ -129,6 +114,8 @@ void setup()
attachInterrupt(digitalPinToInterrupt(HALL7_PIN), ISR_HALL7, FALLING); attachInterrupt(digitalPinToInterrupt(HALL7_PIN), ISR_HALL7, FALLING);
attachInterrupt(digitalPinToInterrupt(HALL8_PIN), ISR_HALL8, FALLING); attachInterrupt(digitalPinToInterrupt(HALL8_PIN), ISR_HALL8, FALLING);
#endif
Serial.println(" Auf gehts ... "); Serial.println(" Auf gehts ... ");
} }
@ -167,74 +154,60 @@ 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() void PID()
{ {
current_speed = HallData[0].value; current_speed = HallData[0].value;
//current_speed = map(HallData[0].value, MIN_SPEED, MAX_SPEED, MIN_ESC_SPEED, MAX_ESC_SPEED); if(ki==0)
{
integ = 0;
}
if(kd==0)
{
derivative = 0;
}
double int_err = goal_speed - current_speed; err = goal_speed - current_speed;
double int_integ = ki>0?integ + int_err:0; integ = ki>0?integ + err:0;
double int_derivative = kd>0?(int_err - last_error):0; derivative = kd>0?(err - last_error):0;
double int_output = kp * int_err + ki * int_integ + kd * int_derivative; output = kp * err + ki * integ + kd * derivative;
if( int_output <= MIN_ESC_SPEED)
{
output = MIN_ESC_SPEED;
}
else if( int_output > MAX_ESC_SPEED)
{
output = MAX_ESC_SPEED;
}
else if((int_output >= MIN_ESC_SPEED) && (int_output <= MAX_ESC_SPEED))
{
err = int_err;
integ = int_integ;
derivative = int_derivative;
output = int_output;
}
esc_output = constrain(output, MIN_ESC_SPEED, MAX_ESC_SPEED); esc_output = constrain(output, 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; last_error = err;
} }
double time_per_round_calc() unsigned long time_per_round_calc()
{ {
double mid_time = 0; unsigned long mid_time = 0;
double min_time = -1; unsigned long min_time = 0;
double max_time = 0; unsigned long max_time = 0;
uint8_t nr_times = 0; uint8_t nr_times = 0;
for(uint8_t i = 0; i < TIME_PER_ROUND_VALS; i++) for(uint8_t i = 0; i < TIME_PER_ROUND_VALS; i++)
{ {
if(time_per_round_ms[i] >= 0) unsigned long time_round_ms = time_per_round_ms[i]/HALL_NR_TURN;
if(time_round_ms > 0)
{ {
if(time_per_round_ms[i] < min_time || min_time == -1) if(time_round_ms < min_time || min_time == 0)
{ {
min_time = time_per_round_ms[i]; min_time = time_round_ms;
} }
else if(time_per_round_ms[i] > max_time) if(time_round_ms > max_time)
{ {
max_time = time_per_round_ms[i]; max_time = time_round_ms;
} }
nr_times++; nr_times++;
mid_time = mid_time + time_per_round_ms[i]; mid_time = mid_time + time_round_ms;
} }
} }
if(nr_times >= 3) if(nr_times >= 3)
{ {
mid_time = (mid_time - max_time - min_time)/(nr_times - 2); mid_time = (mid_time - max_time - min_time)/(nr_times - 2);
} }
static unsigned long last_mid_time = millis();
if(millis() - last_mid_time > 500)
{
Serial.printf(">turn_time_ms:%d\n", mid_time);
last_mid_time = millis();
}
return(mid_time); return(mid_time);
} }
@ -252,6 +225,7 @@ void speed_set()
else else
{ {
ESC.write(ser_esc_output); ESC.write(ser_esc_output);
ser_esc_output = -1;
} }
last_esc_write_ms = millis(); last_esc_write_ms = millis();
} }
@ -325,7 +299,7 @@ void data_init()
speed = 0; speed = 0;
for (uint8_t i= 0; i < TIME_PER_ROUND_VALS; i++) for (uint8_t i= 0; i < TIME_PER_ROUND_VALS; i++)
{ {
time_per_round_ms[i] = -1; time_per_round_ms[i] = 0;
} }
unsigned long current_millis = millis(); unsigned long current_millis = millis();
for (uint8_t i = 0; i < ALL_DATA_COUNT; i ++) for (uint8_t i = 0; i < ALL_DATA_COUNT; i ++)
@ -372,10 +346,10 @@ void data_check()
{ {
if( 0 == nr) if( 0 == nr)
{ {
double mid_time = time_per_round_calc(); unsigned long mid_time = time_per_round_calc();
if( mid_time != 0) if( mid_time != 0)
{ {
HallData[nr].value = HallData[nr].unit_factor / (mid_time/HALL_NR_TURN); HallData[nr].value = HallData[nr].unit_factor / mid_time;
} }
} }
else{ else{
@ -414,6 +388,7 @@ void get_serial_cmd()
Serial.printf("Set kp to %f\n", kp); Serial.printf("Set kp to %f\n", kp);
} }
else if(command.startsWith("ki=")){ else if(command.startsWith("ki=")){
integ = 0.0;
ki = command.substring(command.indexOf("=")+1).toDouble(); ki = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set ki to %f\n", ki); Serial.printf("Set ki to %f\n", ki);
} }
@ -422,6 +397,7 @@ void get_serial_cmd()
Serial.printf("Set speed to %f\n", ser_speed); Serial.printf("Set speed to %f\n", ser_speed);
} }
else if(command.startsWith("kd=")){ else if(command.startsWith("kd=")){
derivative = 0.0;
kd = command.substring(command.indexOf("=")+1).toDouble(); kd = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set kd to %f\n", kd); Serial.printf("Set kd to %f\n", kd);
} }
@ -457,6 +433,7 @@ void get_serial_cmd()
err = 0.0; err = 0.0;
last_error = 0.0; last_error = 0.0;
integ = 0.0; integ = 0.0;
derivative = 0.0;
Serial.printf("Reset PID variables.\n"); Serial.printf("Reset PID variables.\n");
} }
@ -469,6 +446,7 @@ void get_serial_cmd()
} }
} }
#ifndef DUMMY_DATA
void IRAM_ATTR ISR_HALL1() void IRAM_ATTR ISR_HALL1()
{ {
const uint8_t hallnr = 0; const uint8_t hallnr = 0;
@ -545,6 +523,7 @@ void IRAM_ATTR ISR_HALL8()
HallData[hallnr].timestamp = run_time; HallData[hallnr].timestamp = run_time;
HallData[hallnr].act_update_ms = millis(); HallData[hallnr].act_update_ms = millis();
} }
#endif
//this code is just for generating dummy data - in normal use not needed //this code is just for generating dummy data - in normal use not needed
void data_generate() void data_generate()
@ -552,7 +531,9 @@ void data_generate()
#ifdef DUMMY_DATA #ifdef DUMMY_DATA
static unsigned long _last_updated_ms = 0; static unsigned long _last_updated_ms = 0;
speed = 1000 + random(-250,150);
speed = map(esc_output, MIN_ESC_SPEED, MAX_ESC_SPEED, MIN_SPEED, MAX_SPEED) + random(-50,50);
unsigned long _wait_ms = map( speed, MIN_SPEED, MAX_SPEED, 60000/MIN_SPEED/4, 60000/MAX_SPEED/4); unsigned long _wait_ms = map( speed, MIN_SPEED, MAX_SPEED, 60000/MIN_SPEED/4, 60000/MAX_SPEED/4);
//Serial.printf("Speed is : %lu and _wait_ms is %lu\n", speed, _wait_ms); //Serial.printf("Speed is : %lu and _wait_ms is %lu\n", speed, _wait_ms);
@ -571,17 +552,31 @@ void data_generate()
{ {
HallData[i].ticks++; HallData[i].ticks++;
HallData[i].period_ticks++; HallData[i].period_ticks++;
HallData[i].last_update_ms = millis();
HallData[i].timestamp = run_time; HallData[i].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[i].period_ticks >= (HALL_NR_TURN * HALL_TICKS_PER_TURN))
{
unsigned long time_ms = millis();
HallData[i].period_ticks = 0;
time_per_round_ms[time_per_round_pointer] = time_ms - HallData[i].act_update_ms;
Serial.printf(">tick_time:%d\n", time_per_round_ms[time_per_round_pointer]);
HallData[i].act_update_ms = time_ms;
time_per_round_pointer++;
if(time_per_round_pointer > TIME_PER_ROUND_VALS)
{
time_per_round_pointer = 0;
}
}
} }
else else
{ {
//Serial.printf("i=%d HallData[i-1].ticks=%lu %4 = %d\n" , i, HallData[i-1].ticks , HallData[i-1].ticks % 4); //Serial.printf("i=%d HallData[i-1].ticks=%lu %4 = %d\n" , i, HallData[i-1].ticks , HallData[i-1].ticks % 4);
if (HallData[i].last_update_ms != HallData[i-1].last_update_ms && HallData[i-1].ticks % 4 == 0) if (HallData[i].act_update_ms != HallData[i-1].act_update_ms && HallData[i-1].ticks % 4 == 0)
{ {
HallData[i].ticks++; HallData[i].ticks++;
HallData[i].period_ticks++; HallData[i].act_update_ms = HallData[i-1].act_update_ms;
HallData[i].last_update_ms = HallData[i-1].last_update_ms;
HallData[i].timestamp = run_time; HallData[i].timestamp = run_time;
} }
} }