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

Mqtt fixed

Browse source
This commit is contained in:
Jens Noack 2023-03-27 17:08:46 +02:00
parent 57580f4436
commit cbabfc3132
4 changed files with 103 additions and 71 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
vscode/Unendlichkeitsmaschine/include/json.hpp
View file

@ -5,6 +5,22 @@
#include <map> #include <map>
#include <string> #include <string>
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 typedef struct
{ {
const char * name; const char * name;
@ -23,7 +39,7 @@ class Myjson{
private: private:
public: 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 #endif //JSON_HPP

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

@ -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. Servo ESC; // Der ESC-Controller (Electronic Speed Controller bzw. elektronischer Fahrtregler) wird als Objekt mit dem Namen "ESC" festgelegt.
Oled display; Oled display;
const unsigned long MAX_TIME_PER_TURN_MS = 600; const unsigned long MAX_TIME_PER_TURN_MS = 200;
const unsigned long UPDATE_TURN_VALUES_EVERY_MS = 1000; const unsigned long UPDATE_ESC_EVERY_MS = 100;
const unsigned long UPDATE_ESC_EVERY_MS = 200;
const uint8_t HALL_SENSORS_COUNT = 8; // input, sec, min, hour, day, week, month, year const uint8_t HALL_SENSORS_COUNT = 8; // input, sec, min, hour, day, week, month, year
const uint8_t ALL_DATA_COUNT = HALL_SENSORS_COUNT; 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 AllData[ALL_DATA_COUNT];
volatile DataStruct *HallData = AllData; volatile DataStruct *HallData = AllData;
PIDStruct Pid_params;
const char *Data_names[ALL_DATA_COUNT] = 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 HALL7_PIN = 27;
const uint8_t HALL8_PIN = 14; 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 MIN_SPEED = 500;
const uint16_t MAX_SPEED = 5000; const uint16_t MAX_SPEED = 5000;
const uint8_t MAX_ESC_SPEED = 180; const uint8_t MAX_ESC_SPEED = 100;
const uint8_t MIN_ESC_SPEED = 7; const uint8_t MIN_ESC_SPEED = 6;
#endif //MAIN_HPP #endif //MAIN_HPP

26
vscode/Unendlichkeitsmaschine/src/json.cpp
View file

@ -1,18 +1,34 @@
#include "json.hpp" #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; static unsigned long last_send_ms = 0;
unsigned int _sections = nr_dataobjects; //number of different values 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 _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 = ""; String json_str = "";
int _capacity = JSON_ARRAY_SIZE(_sections) + _sec_objects*JSON_OBJECT_SIZE(_obj_members); int _capacity = JSON_ARRAY_SIZE(_sections) + _sec_objects*JSON_OBJECT_SIZE(_obj_members);
DynamicJsonDocument _doc(_capacity + 100); DynamicJsonDocument _doc(_capacity + 100);
_doc["runtime_s"] = runtime_sec; _doc["runtime_s"] = runtime_s;
JsonObject _doc_object[_sections];
for(unsigned int section_nr = 0; section_nr < _sections; section_nr++) 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] = _doc.createNestedObject(data_struct[section_nr].name);
_doc_object[section_nr]["ticks"] = data_struct[section_nr].ticks; _doc_object[section_nr]["ticks"] = data_struct[section_nr].ticks;

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

@ -6,23 +6,8 @@ volatile double time_per_round_ms[TIME_PER_ROUND_VALS];
time_t run_time = 0; time_t run_time = 0;
time_t start_time = 0; time_t start_time = 0;
hw_timer_t * timer = NULL; hw_timer_t * timer = NULL;
double integ = 0.0;
double derivative = 0.0;
double esc_output = 0.0;
int ser_esc_output = -1; int ser_esc_output = -1;
double speed = 1000.0;
double last_error = 0.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_HALL1();
void ISR_HALL2(); void ISR_HALL2();
@ -52,6 +37,7 @@ void json_init();
void setup() void setup()
{ {
delay(1000); delay(1000);
Serial.begin(115200); Serial.begin(115200);
Serial.println("Init capture portal ..."); Serial.println("Init capture portal ...");
@ -126,10 +112,10 @@ void loop()
data_store(); data_store();
#ifdef HAS_MQTT #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 #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() 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); Pid_params.err = constrain(Pid_params.speed - Pid_params.current_speed, -100, 100);
double int_integ = integ + err; double int_integ = Pid_params.integ + Pid_params.err;
double int_derivative = err - last_error; double int_derivative = Pid_params.err - last_error;
if(ki != 0) 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 else
{ {
integ = int_integ; Pid_params.integ = int_integ;
} }
} }
else 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 else
{ {
derivative = int_derivative; Pid_params.derivative = int_derivative;
} }
} }
else 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 ) 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 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() 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) if(0 == last_esc_write_ms || millis() - last_esc_write_ms > UPDATE_ESC_EVERY_MS)
{ {
PID(); PID();
Serial.printf(">speed:%06.3f\n>current:%06.3f\n>esc:%06.3f\n",speed, current_speed, 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(esc_output); ESC.write(Pid_params.esc_output);
last_esc_write_ms = millis(); last_esc_write_ms = millis();
Pid_params.timestamp = last_esc_write_ms;
} }
} }
void data_init() void data_init()
{ {
Serial.println("Init internal data ..."); 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 #ifdef MACHINE_RESTART
run_time = 0; run_time = 0;
start_time = capportal.epochTime(); start_time = capportal.epochTime();
@ -390,22 +390,22 @@ void get_serial_cmd()
String command = Serial.readStringUntil('\n'); String command = Serial.readStringUntil('\n');
Serial.printf("Received command '%s'", command.c_str()); Serial.printf("Received command '%s'", command.c_str());
if(command.startsWith("kp=")){ if(command.startsWith("kp=")){
kp = command.substring(command.indexOf("=")+1).toDouble(); Pid_params.kp = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set kp to %f\n", kp); Serial.printf("Set kp to %f\n", Pid_params.kp);
} }
else if(command.startsWith("ki=")){ else if(command.startsWith("ki=")){
integ = 0.0; Pid_params.integ = 0.0;
ki = command.substring(command.indexOf("=")+1).toDouble(); Pid_params.ki = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set ki to %f\n", ki); Serial.printf("Set ki to %f\n", Pid_params.ki);
} }
else if(command.startsWith("speed=")){ else if(command.startsWith("speed=")){
speed = command.substring(command.indexOf("=")+1).toDouble(); Pid_params.speed = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set speed to %f\n", speed); Serial.printf("Set speed to %f\n", Pid_params.speed);
} }
else if(command.startsWith("kd=")){ else if(command.startsWith("kd=")){
derivative = 0.0; Pid_params.derivative = 0.0;
kd = command.substring(command.indexOf("=")+1).toDouble(); Pid_params.kd = command.substring(command.indexOf("=")+1).toDouble();
Serial.printf("Set kd to %f\n", kd); Serial.printf("Set kd to %f\n", Pid_params.kd);
} }
else if(command.startsWith("esc=")){ else if(command.startsWith("esc=")){
ser_esc_output = command.substring(command.indexOf("=")+1).toDouble(); ser_esc_output = command.substring(command.indexOf("=")+1).toDouble();
@ -413,7 +413,7 @@ void get_serial_cmd()
} }
else if(command.equals("params")){ 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")){ else if(command.equals("control")){
ser_esc_output = -1; ser_esc_output = -1;
@ -436,10 +436,10 @@ void get_serial_cmd()
Serial.printf("Stop drive.\n"); Serial.printf("Stop drive.\n");
} }
else if(command.equals("pidreset")){ else if(command.equals("pidreset")){
err = 0.0; Pid_params.err = 0.0;
last_error = 0.0; last_error = 0.0;
integ = 0.0; Pid_params.integ = 0.0;
derivative = 0.0; Pid_params.derivative = 0.0;
Serial.printf("Reset PID variables.\n"); Serial.printf("Reset PID variables.\n");
} }