diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp index 021269e..ccb3f68 100644 --- a/vscode/Unendlichkeitsmaschine/src/main.cpp +++ b/vscode/Unendlichkeitsmaschine/src/main.cpp @@ -11,15 +11,17 @@ hw_timer_t * timer = NULL; volatile double integ = 0.0; volatile double derivative = 0.0; volatile double esc_output = 0.0; +int ser_esc_output = -1; +double ser_speed = 1000; volatile double last_error = 0.0; int esc_value = 0; -volatile double goal_speed = 1000; //1.0 * speed; +volatile double goal_speed = 0; volatile double current_speed = 0; volatile double err = 0; volatile double output = 0.0; -const double kp = 0.005; -const double ki = 0.0008; -const double kd = 0.0001; +double kp = 0.005; +double ki = 0.0008; +double kd = 0.0001; void ISR_HALL1(); void ISR_HALL2(); @@ -39,6 +41,8 @@ void speed_get(); void speed_set(); void count_secs(time_t* run_time); +void get_serial_cmd(); + #ifdef HAS_MQTT void json_init(); #endif @@ -110,9 +114,8 @@ void loop() { capportal.handle(); - + get_serial_cmd(); data_generate(); - count_secs(&run_time); speed_get(); speed_set(); @@ -145,18 +148,18 @@ void count_secs(time_t* run_time) void speed_set() { static unsigned long last_esc_write_ms = 0; - static double last_esc_output = 0; if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS) { 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); - if(esc_output != last_esc_output) + if(-1 == ser_esc_output ) { - //Serial.printf("PID ... (goal>:%06.3f, current>:%06.3f, err>:%06.3f, integ>:%06.3f, derive>:%06.3f, output>:%06.3f), esc_output>:%06.3f \n", goal_speed, current_speed, err, integ, derivative, output, esc_output); - ESC.write(esc_output); - last_esc_output = esc_output; - last_esc_write_ms = millis(); } + else + { + ESC.write(ser_esc_output); + } + last_esc_write_ms = millis(); } } @@ -165,42 +168,49 @@ void speed_get() static uint8_t _count_meas = 0; static int _Drehregler = 0; static unsigned long _last_read_ms = 0; - - if( millis() - _last_read_ms > 25) + if(ser_speed == -1) { - _last_read_ms = millis(); - int maxDrehregler = 0; - int minDrehregler = 0; - int sumDrehregler = 0; - for(uint8_t i = 0; i<6;i++) + if( millis() - _last_read_ms > 25) { - Drehregler = analogRead(POTI_PIN); // Dieser Befehl liest den Wert des Potentiometers am analogen Pin A0 aus und speichert ihn unter der Variablen "Drehregler". Der Wert liegt zwischen 0 und 1023. - if(i==0) + _last_read_ms = millis(); + int maxDrehregler = 0; + int minDrehregler = 0; + int sumDrehregler = 0; + for(uint8_t i = 0; i<6;i++) { - maxDrehregler = Drehregler; - minDrehregler = Drehregler; - } - else - { - if(Drehregler < minDrehregler) - { - minDrehregler = Drehregler; - } - if(Drehregler > maxDrehregler) + Drehregler = analogRead(POTI_PIN); // Dieser Befehl liest den Wert des Potentiometers am analogen Pin A0 aus und speichert ihn unter der Variablen "Drehregler". Der Wert liegt zwischen 0 und 1023. + if(i==0) { maxDrehregler = Drehregler; + minDrehregler = Drehregler; } + else + { + if(Drehregler < minDrehregler) + { + minDrehregler = Drehregler; + } + if(Drehregler > maxDrehregler) + { + maxDrehregler = Drehregler; + } + } + sumDrehregler = sumDrehregler + Drehregler; } - sumDrehregler = sumDrehregler + Drehregler; + sumDrehregler = sumDrehregler - maxDrehregler - minDrehregler; + sumDrehregler = sumDrehregler/4; + #ifdef DUMMY_DATA + speed = 1000; + #else + speed = map(sumDrehregler, 0, MAX_POTI_VALUE, MIN_SPEED, MAX_SPEED); // Der "MAP-" Befehl wandelt den Messwert aus der Variablen "Drehregler" um, damit er am ESC verarbeitet werden kann. Der Zahlenbereich 0 bis 1023 wird dabei in einen Zahlenwert zwischen 0 und 180 umgewandelt. + #endif } - sumDrehregler = sumDrehregler - maxDrehregler - minDrehregler; - sumDrehregler = sumDrehregler/4; - #ifdef DUMMY_DATA - speed = 1000; - #else - speed = map(sumDrehregler, 0, MAX_POTI_VALUE, MIN_SPEED, MAX_SPEED); // Der "MAP-" Befehl wandelt den Messwert aus der Variablen "Drehregler" um, damit er am ESC verarbeitet werden kann. Der Zahlenbereich 0 bis 1023 wird dabei in einen Zahlenwert zwischen 0 und 180 umgewandelt. - #endif } + else{ + speed = ser_speed; + } + goal_speed = 1.0 * speed; + } void data_init() @@ -297,6 +307,60 @@ void data_store() } } +void get_serial_cmd() +{ + if(Serial.available()){ + String command = Serial.readStringUntil('\n'); + Serial.printf("Received command '%s'", command.c_str()); + if(command.startsWith("kp=")){ + kp = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set kp to %f\n", kp); + } + else if(command.startsWith("ki=")){ + ki = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set ki to %f\n", ki); + } + + else if(command.startsWith("speed=")){ + ser_speed = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set speed to %f\n", speed); + } + else if(command.startsWith("kd=")){ + kd = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set kd to %f\n", kd); + } + else if(command.equals("params")){ + Serial.printf("Actual values: kp=%f ki=%f kd=%f \n", kp, ki, kd); + } + else if(command.equals("control")){ + ser_esc_output = -1; + Serial.printf("Set to drive PID control mode.\n"); + } + else if(command.equals("max")){ + ser_esc_output = MAX_ESC_SPEED; + Serial.printf("Set to maximal speed.\n"); + } + else if(command.equals("min")){ + ser_esc_output = MIN_ESC_SPEED; + Serial.printf("Set to minimal speed.\n"); + } + else if(command.equals("start")){ + ser_esc_output = -1; + Serial.printf("Start drive PID control mode.\n"); + } + else if(command.equals("stop")){ + ser_esc_output = 0; + Serial.printf("Stop drive.\n"); + } + else if(command.equals("help")){ + Serial.printf("Commands: kp,ki,kd=x.xxxx | params | control | max | min | start | stop | speed=x | help \n"); + } + else{ + Serial.printf("Invalid command."); + } + } +} + void IRAM_ATTR ISR_HALL1() { const uint8_t hallnr = 0; @@ -371,6 +435,9 @@ void IRAM_ATTR ISR_HALL8() void IRAM_ATTR ISR_onTimer() { current_speed = HallData[0].value; + + //current_speed = map(HallData[0].value, MIN_SPEED, MAX_SPEED, MIN_ESC_SPEED, MAX_ESC_SPEED); + err = goal_speed - current_speed; integ = integ + err; derivative = (err - last_error); @@ -378,6 +445,7 @@ void IRAM_ATTR ISR_onTimer() //Beschränkung der Ausgabe esc_output = MIN_ESC_SPEED + constrain(output, 0, MAX_ESC_SPEED - MIN_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