First PID version

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"
@@ -17,6 +17,7 @@
 #include "eeprom.hpp"
 #include "oled.hpp"
 #include "capportal.hpp"
+#include <esp32-hal-timer.h>
 
 #define HAS_MQTT
 #ifdef HAS_MQTT
@@ -98,6 +99,8 @@ const uint8_t HALL6_PIN = 26;
 const uint8_t HALL7_PIN = 27;
 const uint8_t HALL8_PIN = 14;
 
+const uint16_t MIN_SPEED = 600;
+const uint16_t MAX_SPEED = 1200;
 const uint8_t MAX_ESC_SPEED = 60;
 const uint8_t MIN_ESC_SPEED = 35;
 const uint16_t MAX_POTI_VALUE = 4095;

vscode/Unendlichkeitsmaschine/include/mqtt.hpp

@@ -46,7 +46,7 @@ emyPxgcYxn/eR44/KJ4EBs+lVDR3veyJm+kXQ99b21/+jh5Xos1AnX5iItreGCc=
 #define MQTT_TRY_TO_CONNECT_MS 2000
 #define MQTT_CHANNEL "infinitymachine"
 #define MQTT_BUFFER_SIZE 1024
-#define SEND_MQTT_EVERY_MS 1000
+#define SEND_MQTT_EVERY_MS 10000
 
 class Mymqtt
 {

vscode/Unendlichkeitsmaschine/src/main.cpp

@@ -6,6 +6,20 @@ int last_speed;
 
 time_t run_time = 0;
 time_t start_time = 0;
+hw_timer_t * timer = NULL;
+
+volatile double integ = 0.0;
+volatile double derivative = 0.0;
+volatile double esc_output = 0.0;
+volatile double last_error = 0.0;
+int esc_value = 0;
+volatile double goal_speed = 1000; //1.0 * speed;
+volatile double current_speed = 0;
+volatile double err = 0;
+volatile double output = 0.0;
+const double kp = 0.01;
+const double ki = 0.001;
+const double kd = 0.0005;
 
 void ISR_HALL1();
 void ISR_HALL2();
@@ -15,6 +29,7 @@ void ISR_HALL5();
 void ISR_HALL6();
 void ISR_HALL7();
 void ISR_HALL8();
+void ISR_onTimer();
 
 void data_generate();
 void data_store();
@@ -56,6 +71,12 @@ void setup()
     ESC.write(0);
   }
 
+  // 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, true); // Setze den Alarm auf 1/4 Sekunde und aktiviere ihn
+  timerAlarmEnable(timer); // Aktiviere den Alarm
+
   pinMode(39, INPUT_PULLDOWN);
   pinMode(HALL1_PIN, INPUT);
   pinMode(HALL2_PIN, INPUT);
@@ -66,7 +87,6 @@ void setup()
   pinMode(HALL7_PIN, INPUT);
   pinMode(HALL8_PIN, INPUT);
 
-
   //Serial.println("Init light sensor interrupt ...");
   //attachInterrupt(LS1_PIN, ISR_LS1, RISING);
 
@@ -102,6 +122,8 @@ void loop()
   
   display.show_values(speed, MIN_ESC_SPEED, MAX_ESC_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP());
 
+ // 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);
+
 }
 
 void count_secs(time_t* run_time)
@@ -153,31 +175,12 @@ void speed_set()
     }
     sumDrehregler = sumDrehregler - maxDrehregler - minDrehregler;
     sumDrehregler = sumDrehregler/4;
-    speed = map(sumDrehregler, 0, MAX_POTI_VALUE, MIN_ESC_SPEED, MAX_ESC_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.
+    #ifdef DUMMY_DATA
-    /*
+    speed = 1000;
-    if(abs(Drehregler - _Drehregler) < ((_Drehregler * JITTER_POTI_PERCENT)/100) )
+    #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.
-      _count_meas++;
+    #endif
   }
-    else
-    {
-      _count_meas = 0;
-    }
-    _Drehregler = Drehregler;
-   
-    if(_count_meas >= 4)
-    {
-      _count_meas >= 0;
-      speed = map(Drehregler, 0, MAX_POTI_VALUE, MIN_ESC_SPEED, MAX_ESC_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.
-    }
-    */
-  }
-
-  if(MIN_ESC_SPEED < speed)
-    ESC.write(speed);    // Der endgültige Wert für den ESC wird an den ESC gesendet. Der ESC nimmt das Signal an dieser Stelle auf und steuert den Motor entsprechend der gesendeten Werte an.
-  else
-    ESC.write(0); 
-
 }
 
 void data_init()
@@ -217,9 +220,9 @@ void data_init()
 
 void data_check()
 {
-  static unsigned long speedchanged_ms ;
   unsigned long current_millis = millis();
-  
+  static unsigned long speedchanged_ms = current_millis ;
+  /*
   Serial.printf("H1(pin %d): %d, H2(pin %d): %d, H3(pin %d): %d, H4(pin %d): %d, H5(pin %d) : %d, H6(pin %d) : %d, H7(pin %d) : %d, H8(pin %d) : %d.\n",
                 HALL1_PIN, digitalRead(HALL1_PIN),
                 HALL2_PIN, digitalRead(HALL2_PIN),
@@ -231,22 +234,17 @@ void data_check()
                 HALL8_PIN, digitalRead(HALL8_PIN)                
                 );
   
+  */
+  if(current_millis - speedchanged_ms >= UPDATE_TURN_VALUES_EVERY_MS)
+  {
+
 
-  if(speed != last_speed)
-  {
-    last_speed = speed;
-    speedchanged_ms = current_millis;
-    HallData[0].period_ticks = 0;
-  }
-  else
-  {
-    if(current_millis - speedchanged_ms > UPDATE_TURN_VALUES_EVERY_MS)
-    {
-      speedchanged_ms = current_millis;
     HallData[0].value = (HallData[0].period_ticks * HallData[0].unit_factor);
     HallData[0].period_ticks = 0;
+    Serial.printf("current_millis: %lu goal speed: %lu calc speed: %f (%lu ticks in %lu ms) \n", current_millis, speed, HallData[0].value, current_millis - speedchanged_ms);
+    speedchanged_ms = current_millis;
   }
-  }
+
 
   for(uint8_t nr = 1; nr < HALL_SENSORS_COUNT; nr++)
   {
@@ -346,13 +344,37 @@ void IRAM_ATTR ISR_HALL8()
   HallData[hallnr].last_update_ms = millis();
 }
 
+
+
+
+void IRAM_ATTR ISR_onTimer()
+{
+  current_speed = HallData[0].value;
+  err = goal_speed - current_speed;
+  integ = integ + err;
+  derivative = (err - last_error);
+  output = kp * err + ki * integ + kd * derivative;
+
+  //Beschränkung der Ausgabe
+  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.3%f) output=%06.3f \n", goal_speed, current_speed, err, integ, derivative, output);
+
+  //Aktualisierung der letzten Fehler- und Zeitwerte
+  last_error = err;
+
+  //Ansteuerung des Outputs
+  
+  ESC.write(esc_output); 
+}
+
 //this code is just for generating dummy data - in normal use not needed
 void data_generate()
 {
   #ifdef DUMMY_DATA
 
   static unsigned long _last_updated_ms = 0; 
-  unsigned long _wait_ms = map(speed, MIN_ESC_SPEED, MAX_ESC_SPEED, 60000/500/4, 60000/1500/4);
+  speed = 1000 + random(-250,150);
+  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);
 
   if(_last_updated_ms == 0)