From cbabfc3132ac933095f9fbe5563588eaed3042c8 Mon Sep 17 00:00:00 2001 From: jenoack Date: Mon, 27 Mar 2023 17:08:46 +0200 Subject: [PATCH] Mqtt fixed --- .../Unendlichkeitsmaschine/include/json.hpp | 18 ++- .../Unendlichkeitsmaschine/include/main.hpp | 12 +- vscode/Unendlichkeitsmaschine/src/json.cpp | 26 +++- vscode/Unendlichkeitsmaschine/src/main.cpp | 118 +++++++++--------- 4 files changed, 103 insertions(+), 71 deletions(-) diff --git a/vscode/Unendlichkeitsmaschine/include/json.hpp b/vscode/Unendlichkeitsmaschine/include/json.hpp index dcae2e0..9197675 100644 --- a/vscode/Unendlichkeitsmaschine/include/json.hpp +++ b/vscode/Unendlichkeitsmaschine/include/json.hpp @@ -5,6 +5,22 @@ #include #include +typedef struct { + unsigned long timestamp; + double integ; + double derivative; + double esc_output; + double speed; + double current_speed; + double err; + double output; + double kp; + double ki; + double kd; + int max_speed; + int min_speed; +}PIDStruct; + typedef struct { const char * name; @@ -23,7 +39,7 @@ class Myjson{ private: public: - String create_json_string(volatile DataStruct *data_struct, size_t nr_dataobjects, time_t runtime_s); + String create_json_string(volatile DataStruct *data_struct, size_t nr_dataobjects, PIDStruct *pid_struct, time_t runtime_s); }; #endif //JSON_HPP \ No newline at end of file diff --git a/vscode/Unendlichkeitsmaschine/include/main.hpp b/vscode/Unendlichkeitsmaschine/include/main.hpp index 52da6c8..53513c6 100644 --- a/vscode/Unendlichkeitsmaschine/include/main.hpp +++ b/vscode/Unendlichkeitsmaschine/include/main.hpp @@ -54,9 +54,8 @@ 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 = 200; +const unsigned long MAX_TIME_PER_TURN_MS = 200; +const unsigned long UPDATE_ESC_EVERY_MS = 100; const uint8_t HALL_SENSORS_COUNT = 8; // input, sec, min, hour, day, week, month, year const uint8_t ALL_DATA_COUNT = HALL_SENSORS_COUNT; @@ -67,6 +66,7 @@ const uint8_t HALL_MIN_PULSE_MS = 50; volatile DataStruct AllData[ALL_DATA_COUNT]; volatile DataStruct *HallData = AllData; +PIDStruct Pid_params; const char *Data_names[ALL_DATA_COUNT] = { @@ -100,11 +100,11 @@ const uint8_t HALL6_PIN = 26; const uint8_t HALL7_PIN = 27; const uint8_t HALL8_PIN = 14; -const uint8_t TIME_PER_ROUND_VALS = 20; +const uint8_t TIME_PER_ROUND_VALS = 10; const uint16_t MIN_SPEED = 500; const uint16_t MAX_SPEED = 5000; -const uint8_t MAX_ESC_SPEED = 180; -const uint8_t MIN_ESC_SPEED = 7; +const uint8_t MAX_ESC_SPEED = 100; +const uint8_t MIN_ESC_SPEED = 6; #endif //MAIN_HPP \ No newline at end of file diff --git a/vscode/Unendlichkeitsmaschine/src/json.cpp b/vscode/Unendlichkeitsmaschine/src/json.cpp index 82c0881..2571040 100644 --- a/vscode/Unendlichkeitsmaschine/src/json.cpp +++ b/vscode/Unendlichkeitsmaschine/src/json.cpp @@ -1,18 +1,34 @@ #include "json.hpp" -String Myjson::create_json_string(volatile DataStruct *data_struct, size_t nr_dataobjects, time_t runtime_sec) +String Myjson::create_json_string(volatile DataStruct *data_struct, size_t nr_dataobjects, PIDStruct *pid_struct, time_t runtime_s) { static unsigned long last_send_ms = 0; unsigned int _sections = nr_dataobjects; //number of different values const unsigned int _sec_objects = NR_JSON_SECTION_OBJECTS; //number of objects per value - const unsigned int _obj_members = 5; //number of members per object + const unsigned int _obj_members = 5 + 14; //number of members per object String json_str = ""; int _capacity = JSON_ARRAY_SIZE(_sections) + _sec_objects*JSON_OBJECT_SIZE(_obj_members); DynamicJsonDocument _doc(_capacity + 100); - _doc["runtime_s"] = runtime_sec; - JsonObject _doc_object[_sections]; - for(unsigned int section_nr = 0; section_nr < _sections; section_nr++) + _doc["runtime_s"] = runtime_s; + + JsonObject _doc_object[_sections + 1]; + _doc_object[0] = _doc.createNestedObject("PIDparams"); + _doc_object[0]["goal_speed"]=pid_struct->speed; + _doc_object[0]["current_speed"]=pid_struct->current_speed; + _doc_object[0]["err"]=pid_struct->err; + _doc_object[0]["integrative"]=pid_struct->integ; + _doc_object[0]["derivative"]=pid_struct->derivative; + _doc_object[0]["output"]=pid_struct->output; + _doc_object[0]["esc_output"]=pid_struct->esc_output; + _doc_object[0]["weigth_proportional"]=pid_struct->kp; + _doc_object[0]["weigth_integrational"]=pid_struct->ki; + _doc_object[0]["weigth_differential"]=pid_struct->kd; + _doc_object[0]["min_speed"]=pid_struct->min_speed; + _doc_object[0]["max_speed"]=pid_struct->max_speed; + _doc_object[0]["timestamp"]=pid_struct->timestamp; + + for(unsigned int section_nr = 1; section_nr < _sections; section_nr++) { _doc_object[section_nr] = _doc.createNestedObject(data_struct[section_nr].name); _doc_object[section_nr]["ticks"] = data_struct[section_nr].ticks; diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp index 10e256b..771b88e 100644 --- a/vscode/Unendlichkeitsmaschine/src/main.cpp +++ b/vscode/Unendlichkeitsmaschine/src/main.cpp @@ -6,23 +6,8 @@ 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; - -double integ = 0.0; -double derivative = 0.0; -double esc_output = 0.0; int ser_esc_output = -1; -double speed = 1000.0; double last_error = 0.0; -int esc_value = 0; -double current_speed = 0; -double err = 0; -double output = 0.0; -double kp = 0.03; -double ki = 0.008; -double kd = 0.002; - - -//bei 2000 als speed: values: kp=0.020000 ki=0.005000 kd=0.000100 void ISR_HALL1(); void ISR_HALL2(); @@ -52,6 +37,7 @@ void json_init(); void setup() { + delay(1000); Serial.begin(115200); Serial.println("Init capture portal ..."); @@ -126,10 +112,10 @@ void loop() data_store(); #ifdef HAS_MQTT - mqtt.send_and_loop("infinity/data", json.create_json_string(AllData, ALL_DATA_COUNT, run_time)); + mqtt.send_and_loop("infinity/data", json.create_json_string(AllData, ALL_DATA_COUNT, &Pid_params, run_time)); #endif - display.show_values(speed, MIN_SPEED, MAX_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP()); + display.show_values(Pid_params.speed, MIN_SPEED, MAX_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP()); } @@ -149,70 +135,70 @@ void count_secs(time_t* run_time) void PID() { - current_speed = HallData[0].value; + Pid_params.current_speed = HallData[0].value; - if(ki==0) + if(Pid_params.ki==0) { - integ = 0; + Pid_params.integ = 0; } - if(kd==0) + if(Pid_params.kd==0) { - derivative = 0; + Pid_params.derivative = 0; } - err = constrain(speed - current_speed, -100, 100); - double int_integ = integ + err; - double int_derivative = err - last_error; - if(ki != 0) + Pid_params.err = constrain(Pid_params.speed - Pid_params.current_speed, -100, 100); + double int_integ = Pid_params.integ + Pid_params.err; + double int_derivative = Pid_params.err - last_error; + if(Pid_params.ki != 0) { - if(ki*int_integ > MAX_ESC_SPEED) + if(Pid_params.ki*int_integ > MAX_ESC_SPEED) { - integ = MAX_ESC_SPEED/ki; + Pid_params.integ = MAX_ESC_SPEED/Pid_params.ki; } - else if(ki*int_integ < -1*MAX_ESC_SPEED) + else if(Pid_params.ki*int_integ < -1*MAX_ESC_SPEED) { - integ = -1*MAX_ESC_SPEED/ki; + Pid_params.integ = -1*MAX_ESC_SPEED/Pid_params.ki; } else { - integ = int_integ; + Pid_params.integ = int_integ; } } else { - integ = 0; + Pid_params.integ = 0; } - if(kd != 0) + if(Pid_params.kd != 0) { - if(kd*int_derivative > MAX_ESC_SPEED) + if(Pid_params.kd*int_derivative > MAX_ESC_SPEED) { - derivative = MAX_ESC_SPEED/kd; + Pid_params.derivative = MAX_ESC_SPEED/Pid_params.kd; } - else if(kd*int_derivative < -1*MAX_ESC_SPEED) + else if(Pid_params.kd*int_derivative < -1*MAX_ESC_SPEED) { - derivative = -1*MAX_ESC_SPEED/kd; + Pid_params.derivative = -1*MAX_ESC_SPEED/Pid_params.kd; } else { - derivative = int_derivative; + Pid_params.derivative = int_derivative; } } else { - derivative = 0; + Pid_params.derivative = 0; } - output = kp * err + ki * integ + kd * derivative; + Pid_params.output = Pid_params.kp * Pid_params.err + Pid_params.ki * Pid_params.integ + Pid_params.kd * Pid_params.derivative; if(-1 == ser_esc_output ) { - esc_output = constrain(output, MIN_ESC_SPEED, MAX_ESC_SPEED); + Pid_params.esc_output = constrain(Pid_params.output, MIN_ESC_SPEED, MAX_ESC_SPEED); } else { - esc_output = ser_esc_output; + Pid_params.esc_output = ser_esc_output; } - last_error = err; + last_error = Pid_params.err; } unsigned long time_per_round_calc() @@ -276,15 +262,29 @@ void speed_set() if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS) { PID(); - Serial.printf(">speed:%06.3f\n>current:%06.3f\n>esc:%06.3f\n",speed, current_speed, esc_output); - ESC.write(esc_output); + Serial.printf(">speed:%06.3f\n>current:%06.3f\n>esc:%06.3f\n",Pid_params.speed, Pid_params.current_speed, Pid_params.esc_output); + ESC.write(Pid_params.esc_output); last_esc_write_ms = millis(); + Pid_params.timestamp = last_esc_write_ms; } } void data_init() { Serial.println("Init internal data ..."); + Pid_params.timestamp = millis(); + Pid_params.integ = 0.0; + Pid_params.derivative = 0.0; + Pid_params.esc_output = 0.0; + Pid_params.speed = 1000.0; + Pid_params.current_speed = 0.0; + Pid_params.err = 0; + Pid_params.output = 0.0; + Pid_params.kp = 0.02; + Pid_params.ki = 0.006; + Pid_params.kd = 0.001; + Pid_params.max_speed = MAX_SPEED; + Pid_params.min_speed = MIN_SPEED; #ifdef MACHINE_RESTART run_time = 0; start_time = capportal.epochTime(); @@ -390,22 +390,22 @@ void get_serial_cmd() 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); + Pid_params.kp = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set kp to %f\n", Pid_params.kp); } else if(command.startsWith("ki=")){ - integ = 0.0; - ki = command.substring(command.indexOf("=")+1).toDouble(); - Serial.printf("Set ki to %f\n", ki); + Pid_params.integ = 0.0; + Pid_params.ki = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set ki to %f\n", Pid_params.ki); } else if(command.startsWith("speed=")){ - speed = command.substring(command.indexOf("=")+1).toDouble(); - Serial.printf("Set speed to %f\n", speed); + Pid_params.speed = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set speed to %f\n", Pid_params.speed); } else if(command.startsWith("kd=")){ - derivative = 0.0; - kd = command.substring(command.indexOf("=")+1).toDouble(); - Serial.printf("Set kd to %f\n", kd); + Pid_params.derivative = 0.0; + Pid_params.kd = command.substring(command.indexOf("=")+1).toDouble(); + Serial.printf("Set kd to %f\n", Pid_params.kd); } else if(command.startsWith("esc=")){ ser_esc_output = command.substring(command.indexOf("=")+1).toDouble(); @@ -413,7 +413,7 @@ void get_serial_cmd() } else if(command.equals("params")){ - Serial.printf("Actual values: kp=%f ki=%f kd=%f \n", kp, ki, kd); + Serial.printf("Actual values: kp=%f ki=%f kd=%f \n", Pid_params.kp, Pid_params.ki, Pid_params.kd); } else if(command.equals("control")){ ser_esc_output = -1; @@ -436,10 +436,10 @@ void get_serial_cmd() Serial.printf("Stop drive.\n"); } else if(command.equals("pidreset")){ - err = 0.0; + Pid_params.err = 0.0; last_error = 0.0; - integ = 0.0; - derivative = 0.0; + Pid_params.integ = 0.0; + Pid_params.derivative = 0.0; Serial.printf("Reset PID variables.\n"); }