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 MACHINE_RESTART
//#define DUMMY_DATA
#define DUMMY_DATA
#define EEPROM_NAME_TICK "hall_tick"
#define EEPROM_NAME_TIMESTAMP "timestamp"
@ -103,10 +103,10 @@ const uint8_t HALL8_PIN = 14;
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 uint8_t MAX_ESC_SPEED = 100;
const uint8_t MIN_ESC_SPEED = 10;
const uint8_t MAX_ESC_SPEED = 180;
const uint8_t MIN_ESC_SPEED = 0;
const uint16_t MAX_POTI_VALUE = 4095;
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 err = 0;
volatile double output = 0.0;
double kp = 0.005;
double ki = 0.0008;
double kd = 0.0001;
double kp = 0.03;
double ki = 0.01;
double kd = 0.002;
//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_set();
void PID();
double time_per_round_calc();
unsigned long time_per_round_calc();
void count_secs(time_t* run_time);
void get_serial_cmd();
@ -63,16 +64,6 @@ void setup()
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();
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);
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
*/
#ifndef DUMMY_DATA
pinMode(39, INPUT_PULLDOWN);
pinMode(HALL1_PIN, INPUT);
@ -129,6 +114,8 @@ void setup()
attachInterrupt(digitalPinToInterrupt(HALL7_PIN), ISR_HALL7, FALLING);
attachInterrupt(digitalPinToInterrupt(HALL8_PIN), ISR_HALL8, FALLING);
#endif
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()
{
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;
double int_integ = ki>0?integ + int_err:0;
double int_derivative = kd>0?(int_err - last_error):0;
double int_output = kp * int_err + ki * int_integ + kd * int_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;
}
err = goal_speed - current_speed;
integ = ki>0?integ + err:0;
derivative = kd>0?(err - last_error):0;
output = kp * err + ki * integ + kd * derivative;
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;
}
double time_per_round_calc()
unsigned long time_per_round_calc()
{
double mid_time = 0;
double min_time = -1;
double max_time = 0;
unsigned long mid_time = 0;
unsigned long min_time = 0;
unsigned long max_time = 0;
uint8_t nr_times = 0;
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++;
mid_time = mid_time + time_per_round_ms[i];
mid_time = mid_time + time_round_ms;
}
}
if(nr_times >= 3)
{
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);
}
@ -252,6 +225,7 @@ void speed_set()
else
{
ESC.write(ser_esc_output);
ser_esc_output = -1;
}
last_esc_write_ms = millis();
}
@ -325,7 +299,7 @@ void data_init()
speed = 0;
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();
for (uint8_t i = 0; i < ALL_DATA_COUNT; i ++)
@ -372,10 +346,10 @@ void data_check()
{
if( 0 == nr)
{
double mid_time = time_per_round_calc();
unsigned long mid_time = time_per_round_calc();
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{
@ -414,6 +388,7 @@ void get_serial_cmd()
Serial.printf("Set kp to %f\n", kp);
}
else if(command.startsWith("ki=")){
integ = 0.0;
ki = command.substring(command.indexOf("=")+1).toDouble();
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);
}
else if(command.startsWith("kd=")){
derivative = 0.0;
kd = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set kd to %f\n", kd);
}
@ -457,6 +433,7 @@ void get_serial_cmd()
err = 0.0;
last_error = 0.0;
integ = 0.0;
derivative = 0.0;
Serial.printf("Reset PID variables.\n");
}
@ -469,6 +446,7 @@ void get_serial_cmd()
}
}
#ifndef DUMMY_DATA
void IRAM_ATTR ISR_HALL1()
{
const uint8_t hallnr = 0;
@ -545,6 +523,7 @@ void IRAM_ATTR ISR_HALL8()
HallData[hallnr].timestamp = run_time;
HallData[hallnr].act_update_ms = millis();
}
#endif
//this code is just for generating dummy data - in normal use not needed
void data_generate()
@ -552,7 +531,9 @@ void data_generate()
#ifdef DUMMY_DATA
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);
//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].period_ticks++;
HallData[i].last_update_ms = millis();
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
{
//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].period_ticks++;
HallData[i].last_update_ms = HallData[i-1].last_update_ms;
HallData[i].act_update_ms = HallData[i-1].act_update_ms;
HallData[i].timestamp = run_time;
}
}