Introduced Serial cmd

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;
+double kp = 0.005;
-const double ki = 0.0008;
+double ki = 0.0008;
-const double kd = 0.0001;
+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,7 +168,8 @@ void speed_get()
   static uint8_t _count_meas = 0;
   static int _Drehregler = 0;
   static unsigned long _last_read_ms = 0;
-  
+  if(ser_speed == -1)
+  {
     if( millis() - _last_read_ms > 25)
     {
       _last_read_ms = millis();
@@ -201,6 +205,12 @@ void speed_get()
       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