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

updated PID

Browse source
This commit is contained in:
jenoack 2023-03-24 09:40:31 +01:00
parent eac6b29825
commit 1f9524bc27
2 changed files with 98 additions and 32 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -19,7 +19,7 @@
#include "capportal.hpp"
#include <esp32-hal-timer.h>
#define HAS_MQTT
//#define HAS_MQTT
#ifdef HAS_MQTT
#include "mqtt.hpp"
#include "json.hpp"
@ -54,6 +54,7 @@ Cap capportal;
Servo ESC; // Der ESC-Controller (Electronic Speed Controller bzw. elektronischer Fahrtregler) wird als Objekt mit dem Namen "ESC" festgelegt.
Oled display;
const unsigned long MAX_TIME_PER_TURN_MS = 600;
const unsigned long UPDATE_TURN_VALUES_EVERY_MS = 1000;
const unsigned long UPDATE_ESC_EVERY_MS = 500;

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

@ -4,23 +4,24 @@ int Drehregler; // Ausgabewert des Drehreglers
int speed; // Das Wort "Geschwindigkeit" steht als Variable für den Ansteuerungswert am ESC.
int last_speed;
volatile uint8_t time_per_round_pointer = 0;
volatile uint8_t time_per_round_last_updated_ms = 0;
volatile double time_per_round_ms[TIME_PER_ROUND_VALS];
time_t run_time = 0;
time_t start_time = 0;
hw_timer_t * timer = NULL;
volatile double integ = 0.0;
volatile double derivative = 0.0;
volatile double esc_output = 0.0;
double integ = 0.0;
double derivative = 0.0;
double esc_output = 0.0;
int ser_esc_output = -1;
double ser_speed = 1000;
volatile double last_error = 0.0;
double last_error = 0.0;
int esc_value = 0;
volatile double goal_speed = 0;
volatile double current_speed = 0;
volatile double err = 0;
volatile double output = 0.0;
double goal_speed = 0;
double current_speed = 0;
double err = 0;
double output = 0.0;
double kp = 0.03;
double ki = 0.01;
double kd = 0.002;
@ -44,6 +45,7 @@ void data_check();
void data_init();
void speed_get();
void speed_set();
void PID();
unsigned long time_per_round_calc();
void count_secs(time_t* run_time);
@ -63,6 +65,7 @@ void setup()
randomSeed(micros());
display.begin();
display.progressBar(0,"Lets start ...", true);
capportal.begin();
html_content = "<!DOCTYPE html><html><head><meta http-equiv=\"refresh\" content=\"0; url='/_ac'\" /></head><body></body></html>";
@ -75,20 +78,20 @@ void setup()
Serial.println("Init ETC ...");
ESC.attach(ESC_PIN,1000,2000);
display.progressBar(0,"Init controller");
display.progressBar(0,"Init controller", true);
ESC.write(180);
delay(1000);
display.progressBar(1,"Connect ETC now!");
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.", "That's OK! ;-)");
display.progressBar(2,"Calibrating ETC", "You may hear some beeps.", "If not - check ETC connection.", true);
ESC.write(0);
for(uint8_t i=3;i<=100;i=i+10)
{
display.progressBar(i);
display.progressBar(i,false);
delay(1000);
}
ESC.write(0);
display.progressBar(100,"ETC init done!");
display.progressBar(100,"ETC init done!", true);
delay(1000);
data_init();
@ -168,8 +171,48 @@ void PID()
}
err = goal_speed - current_speed;
integ = ki>0?integ + err:0;
derivative = kd>0?(err - last_error):0;
double int_integ = integ + err;
double int_derivative = err - last_error;
if(ki != 0)
{
if(ki*int_integ > MAX_ESC_SPEED)
{
integ = MAX_ESC_SPEED/ki;
}
else if(ki*int_integ < -1*MAX_ESC_SPEED)
{
integ = -1*MAX_ESC_SPEED/ki;
}
else
{
integ = int_integ;
}
}
else
{
integ = 0;
}
if(kd != 0)
{
if(kd*int_derivative > MAX_ESC_SPEED)
{
derivative = MAX_ESC_SPEED/kd;
}
else if(kd*int_derivative < -1*MAX_ESC_SPEED)
{
derivative = -1*MAX_ESC_SPEED/kd;
}
else
{
derivative = int_derivative;
}
}
else
{
derivative = 0;
}
output = kp * err + ki * integ + kd * derivative;
esc_output = constrain(output, MIN_ESC_SPEED, MAX_ESC_SPEED);
last_error = err;
@ -181,26 +224,37 @@ unsigned long time_per_round_calc()
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(millis() - time_per_round_last_updated_ms <= (HALL_NR_TURN*MAX_TIME_PER_TURN_MS))
{
unsigned long time_round_ms = time_per_round_ms[i]/HALL_NR_TURN;
if(time_round_ms > 0)
for(uint8_t i = 0; i < TIME_PER_ROUND_VALS; i++)
{
if(time_round_ms < min_time || min_time == 0)
unsigned long time_round_ms = time_per_round_ms[i]/HALL_NR_TURN;
if(time_round_ms > 0)
{
min_time = time_round_ms;
if(time_round_ms < min_time || min_time == 0)
{
min_time = time_round_ms;
}
if(time_round_ms > max_time)
{
max_time = time_round_ms;
}
nr_times++;
mid_time = mid_time + time_round_ms;
}
if(time_round_ms > max_time)
{
max_time = time_round_ms;
}
nr_times++;
mid_time = mid_time + time_round_ms;
}
if(nr_times >= 3)
{
mid_time = (mid_time - max_time - min_time)/(nr_times - 2);
}
}
if(nr_times >= 3)
else
{
mid_time = (mid_time - max_time - min_time)/(nr_times - 2);
for (uint8_t i= 0; i < TIME_PER_ROUND_VALS; i++)
{
time_per_round_ms[i] = HALL_NR_TURN*MAX_TIME_PER_TURN_MS;
}
mid_time = MAX_TIME_PER_TURN_MS;
}
static unsigned long last_mid_time = millis();
if(millis() - last_mid_time > 500)
@ -217,7 +271,10 @@ void speed_set()
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);
//Serial.printf(">periodticks0:%d\n>ticks0turntime_ms:%d\n>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",
// HallData[0].period_ticks, time_per_round_ms[time_per_round_pointer], goal_speed, current_speed, err, integ, derivative, output, esc_output);
Serial.printf(">period_ticks:%d\n>time_per_round_last_updated_ms:%d\n>goal_speed:%06.3f\n>current:%06.3f\n>err:%06.3f\n",
HallData[0].period_ticks, time_per_round_last_updated_ms, goal_speed, current_speed, err);
if(-1 == ser_esc_output )
{
ESC.write(esc_output);
@ -299,7 +356,7 @@ void data_init()
speed = 0;
for (uint8_t i= 0; i < TIME_PER_ROUND_VALS; i++)
{
time_per_round_ms[i] = 0;
time_per_round_ms[i] = HALL_NR_TURN*MAX_TIME_PER_TURN_MS;
}
unsigned long current_millis = millis();
for (uint8_t i = 0; i < ALL_DATA_COUNT; i ++)
@ -349,7 +406,14 @@ void data_check()
unsigned long mid_time = time_per_round_calc();
if( mid_time != 0)
{
HallData[nr].value = HallData[nr].unit_factor / mid_time;
if( mid_time >= MAX_TIME_PER_TURN_MS)
{
HallData[nr].value = 0;
}
else
{
HallData[nr].value = HallData[nr].unit_factor / mid_time;
}
}
}
else{
@ -461,6 +525,7 @@ void IRAM_ATTR ISR_HALL1()
time_per_round_ms[time_per_round_pointer] = time_ms - HallData[hallnr].act_update_ms;
HallData[hallnr].act_update_ms = time_ms;
time_per_round_pointer++;
time_per_round_last_updated_ms = time_ms;
if(time_per_round_pointer > TIME_PER_ROUND_VALS)
{
time_per_round_pointer = 0;