From 89a61801813328acef354ded81eeb20a96caffdf Mon Sep 17 00:00:00 2001 From: jenoack Date: Thu, 23 Mar 2023 15:14:01 +0100 Subject: [PATCH] adpated paramters and PID --- .../Unendlichkeitsmaschine/include/main.hpp | 8 +- vscode/Unendlichkeitsmaschine/src/main.cpp | 139 +++++++++--------- 2 files changed, 71 insertions(+), 76 deletions(-) diff --git a/vscode/Unendlichkeitsmaschine/include/main.hpp b/vscode/Unendlichkeitsmaschine/include/main.hpp index 2cec932..614df2d 100644 --- a/vscode/Unendlichkeitsmaschine/include/main.hpp +++ b/vscode/Unendlichkeitsmaschine/include/main.hpp @@ -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; diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp index 9e338e3..f6c17be 100644 --- a/vscode/Unendlichkeitsmaschine/src/main.cpp +++ b/vscode/Unendlichkeitsmaschine/src/main.cpp @@ -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 = ""; @@ -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; } }