From 6bee40cf5af00a8e1e46f869205e92bed606c935 Mon Sep 17 00:00:00 2001
From: jenoack <jenoack@gmail.com>
Date: Fri, 24 Mar 2023 10:27:37 +0100
Subject: [PATCH] modified speed calculation

---
 vscode/Unendlichkeitsmaschine/src/main.cpp | 43 +++++++++++-----------
 1 file changed, 22 insertions(+), 21 deletions(-)

diff --git a/vscode/Unendlichkeitsmaschine/src/main.cpp b/vscode/Unendlichkeitsmaschine/src/main.cpp
index e5cb0dc..10f8031 100644
--- a/vscode/Unendlichkeitsmaschine/src/main.cpp
+++ b/vscode/Unendlichkeitsmaschine/src/main.cpp
@@ -4,7 +4,6 @@ int Drehregler;  // Ausgabewert des Drehreglers
 int speed;  // Das Wort "Geschwindigkeit" steht als Variable für den Ansteuerungswert am ESC.
 int last_speed;
 volatile uint8_t time_per_round_pointer = 0;
-volatile uint8_t time_per_round_last_updated_ms = 0;
 volatile double time_per_round_ms[TIME_PER_ROUND_VALS];
 
 time_t run_time = 0;
@@ -120,7 +119,8 @@ void setup()
   #endif
 
   Serial.println(" Auf gehts ... ");
- 
+  ESC.write(20);
+  delay(1000); 
 }
 
 void loop() 
@@ -214,7 +214,14 @@ void PID()
   }
 
   output = kp * err + ki * integ + kd * derivative;
-  esc_output = constrain(output, MIN_ESC_SPEED, MAX_ESC_SPEED);
+  if(-1 == ser_esc_output ) 
+  {
+    esc_output = constrain(output, MIN_ESC_SPEED, MAX_ESC_SPEED);
+  }
+  else
+  {
+    esc_output = ser_esc_output;
+  }
   last_error = err;
 }
 
@@ -224,8 +231,10 @@ unsigned long time_per_round_calc()
   unsigned long min_time = 0;
   unsigned long max_time = 0;
   uint8_t nr_times = 0;
-  if(millis() - time_per_round_last_updated_ms <= (HALL_NR_TURN*MAX_TIME_PER_TURN_MS))
+  bool real_values = false;
+  if(millis() - HallData[0].act_update_ms <= (HALL_NR_TURN*MAX_TIME_PER_TURN_MS))
   {
+    real_values = true;
     for(uint8_t i = 0; i < TIME_PER_ROUND_VALS; i++)
     {
       unsigned long time_round_ms = time_per_round_ms[i]/HALL_NR_TURN;
@@ -247,19 +256,23 @@ unsigned long time_per_round_calc()
     {
       mid_time = (mid_time - max_time  - min_time)/(nr_times - 2);
     }
+    else if (nr_times > 0)
+    {
+      mid_time = mid_time/nr_times;
+    }
   }
   else
   {
     for (uint8_t i= 0; i < TIME_PER_ROUND_VALS; i++)
     {
-      time_per_round_ms[i] = HALL_NR_TURN*MAX_TIME_PER_TURN_MS;
+      time_per_round_ms[i] = TIME_PER_ROUND_VALS;
     }     
     mid_time = MAX_TIME_PER_TURN_MS;
   }
   static unsigned long last_mid_time = millis();
   if(millis() - last_mid_time > 500)
   {
-    Serial.printf(">turn_time_ms:%d\n", mid_time);
+    Serial.printf(">turn_time_ms:%d\n>real_values:%d\n", mid_time, real_values);
     last_mid_time = millis();
   }
   return(mid_time);
@@ -274,16 +287,8 @@ void speed_set()
     //Serial.printf(">periodticks0:%d\n>ticks0turntime_ms:%d\n>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",
     //               HallData[0].period_ticks, time_per_round_ms[time_per_round_pointer], goal_speed, current_speed, err, integ, derivative, output, esc_output);
     Serial.printf(">period_ticks:%d\n>time_per_round_last_updated_ms:%d\n>goal_speed:%06.3f\n>current:%06.3f\n>err:%06.3f\n",
-                   HallData[0].period_ticks, time_per_round_last_updated_ms, goal_speed, current_speed, err);    
-    if(-1 == ser_esc_output )
-    {
-      ESC.write(esc_output);
-    }
-    else
-    {
-      ESC.write(ser_esc_output);
-      ser_esc_output = -1;
-    }
+                   HallData[0].period_ticks,HallData[0].act_updated_ms, goal_speed, current_speed, err);    
+    ESC.write(esc_output);
     last_esc_write_ms = millis();
   }
 }
@@ -525,11 +530,7 @@ void IRAM_ATTR ISR_HALL1()
     time_per_round_ms[time_per_round_pointer] = time_ms - HallData[hallnr].act_update_ms;
     HallData[hallnr].act_update_ms = time_ms;
     time_per_round_pointer++;
-    time_per_round_last_updated_ms = time_ms;
-    if(time_per_round_pointer > TIME_PER_ROUND_VALS)
-    {
-      time_per_round_pointer = 0;
-    }
+    time_per_round_pointer = time_per_round_pointer>TIME_PER_ROUND_VALS?0:time_per_round_pointer;
   }
 }