diff --git a/vscode/Unendlichkeitsmaschine/include/main.hpp b/vscode/Unendlichkeitsmaschine/include/main.hpp index 78e2b36..6c975b4 100644 --- a/vscode/Unendlichkeitsmaschine/include/main.hpp +++ b/vscode/Unendlichkeitsmaschine/include/main.hpp @@ -19,7 +19,7 @@ #include "capportal.hpp" #include -#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; diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp index f6c17be..e5cb0dc 100644 --- a/vscode/Unendlichkeitsmaschine/src/main.cpp +++ b/vscode/Unendlichkeitsmaschine/src/main.cpp @@ -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 = ""; @@ -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;