experimental code for calibration

vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp

@@ -509,6 +509,30 @@ unsigned int CodeRacerMKII::show_right_stepcounter(){
 	return right_step_counter;
 }
 
+unsigned long CodeRacerMKII::show_left_time_of_last_tick(){
+	return timeafterleft;
+}
+
+unsigned long CodeRacerMKII::show_right_time_of_last_tick(){
+	return timeafterright;
+}
+
+unsigned long CodeRacerMKII::show_left_start_time(){
+	return timebeforeleft;
+}
+
+unsigned long CodeRacerMKII::show_right_start_time(){
+	return timebeforeright;
+}
+
+void CodeRacerMKII::set_left_start_time(){
+	timebeforeleft=0;
+}
+
+void CodeRacerMKII::set_right_start_time(){
+	timebeforeright=0;
+}
+
 /**	@} */ // end of group higherlevelgetters
 /**	@} */ // end of group higherlevel
 
@@ -1490,6 +1514,8 @@ void global_stop_driving(){
 *	@return	nothing
 */
 void IRAM_ATTR CodeRacerMKII::_count_steps_left() {
+	timeafterleft= millis();
+	timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft; 
 	left_step_counter++;
     if (left_step_counter==left_step_stopcount)
     {
@@ -1501,6 +1527,8 @@ void IRAM_ATTR CodeRacerMKII::_count_steps_left() {
 *	@return	nothing
 */
 void IRAM_ATTR CodeRacerMKII::_count_steps_right() {
+	timeafterright= millis();
+	timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright;
 	right_step_counter++;
     if (right_step_counter==right_step_stopcount)
     {

vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h

@@ -89,6 +89,10 @@ static volatile unsigned int right_step_counter=0;
 static volatile unsigned int right_step_stopcount=0;
 static volatile bool syncstop= 0;
 static volatile bool _drive;
+static volatile float timebeforeleft= 0;
+static volatile float timebeforeright= 0;
+static volatile float timeafterleft= 0;
+static volatile float timeafterright= 0;
 static volatile bool obstacle_stop= false;
 static volatile bool obstacle_left_side= false;
 static volatile bool obstacle_right_side= false;
@@ -233,6 +237,12 @@ class CodeRacerMKII {
 		unsigned int show_distance_mm();
 		unsigned int show_left_stepcounter();
 		unsigned int show_right_stepcounter();
+		unsigned long show_left_time_of_last_tick();
+		unsigned long show_right_time_of_last_tick();
+		unsigned long show_left_start_time();
+		unsigned long show_right_start_time();
+		void set_left_start_time();
+		void set_right_start_time();
 		
 
 		// higher level {code}racer services

vsode/coderacer_mkII/src/coderacer_main.cpp

@@ -18,8 +18,15 @@ CodeRacerMKII coderacer;
 int aabstand[160];
 void filladistance();
 int adegree[160];
-int ispeed=1;
+int ispeed=0;
+float fZeit= 0;
+float fSpeedminleft= 0;
+float fSpeedminright=0;
+float fSpeedmaxleft=0;
+int iTicks=31;
 float callibration_drive(unsigned int tickstogo, float calfactor);
+int Emit_vmin(bool state);
+float calculate_min_veliocity(bool side);
 //---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
 void setup() {
     // Monitor
@@ -38,6 +45,7 @@ void setup() {
     drehung = links;
     coderacer.set_syncstop(true);
     coderacer.set_obstacle_stop(false);
+    
 }
 
 //---- Hier startet unsere endlose Schleife - die immer wieder von vorn angefangen wird, wenn wir am Ende angekommen sind. Da ist unser "Fahr"Code drin, der den CodeRacer steuert
@@ -51,20 +59,78 @@ void loop()
       started = true;
       delay(1000);
     }
+    float vminright= calculate_min_veliocity(1);
+    Serial.println(vminright);
+    
+    delay(5000);
    
-  float calf = callibration_drive(1000, 0.04);
-  Serial.println(calf);
-  float compare_calf = callibration_drive(1000, calf);
-  Serial.println(compare_calf);
   }
-
-    
-
- 
-
-
-  
 }
+
+float calculate_min_veliocity(bool side)
+    {
+      ispeed=Emit_vmin(side);
+      coderacer.speed_settings(ispeed, ispeed);
+      coderacer.set_left_start_time();
+      coderacer.set_right_start_time();
+      int iStepsbefore=coderacer.show_left_stepcounter();
+      Serial.printf("Schritte: %i\n", iStepsbefore);
+      coderacer.drive_ticks(iTicks, iTicks);
+      while(coderacer.is_driving()){};
+      if(1==side)
+      {
+        fZeit= (coderacer.show_left_time_of_last_tick()- coderacer.show_left_start_time())/1000.0;
+        int iDifferenz= coderacer.show_left_stepcounter()-iStepsbefore-1;
+        Serial.printf("Zeit: %f timebefore: %lu timeafter: %lu Ticks: %i differenz: %i\n", fZeit, coderacer.show_left_start_time(), coderacer.show_left_time_of_last_tick(), coderacer.show_left_stepcounter(), iDifferenz);
+        
+        fSpeedminleft= (float)iDifferenz/fZeit;
+      }
+      else
+      {
+        fZeit= (coderacer.show_right_time_of_last_tick()- coderacer.show_right_start_time())/1000.0;
+        int iDifferenz= coderacer.show_right_stepcounter()-iStepsbefore-1;
+        fSpeedminright= (float)iDifferenz/fZeit;      
+      }
+      if(1==side)
+      {
+        return fSpeedminleft;
+      }
+      else return fSpeedminright;
+    }
+
+int Emit_vmin(bool state){
+  int iSteps=0;
+  if(1== state)
+  {
+    iSteps= coderacer.show_left_stepcounter();
+  }
+  else {iSteps= coderacer.show_right_stepcounter();}
+  for(int i=0;i<=150; i++)
+  {
+    if(1== state)
+    {
+      if(iSteps<coderacer.show_left_stepcounter())
+      {
+        return i*1.1;
+        break;
+      }
+      coderacer.drive_forward(i, 0);
+      Serial.println(i);
+    }
+    else
+    {
+     if(iSteps<coderacer.show_right_stepcounter())
+      {
+        return i*1.1;
+        break;
+      }
+      coderacer.drive_forward(0, i);
+      Serial.println(i);
+    }
+  }
+  return 0;
+}
+
 //Serial.printf("links %i\n", coderacer.show_left_stepcounter());
 //Serial.printf("rechts %i\n", coderacer.show_right_stepcounter());
 //Serial.printf("%i\n", coderacer.show_distance_mm());
@@ -76,6 +142,7 @@ float callibration_drive(unsigned int tickstogo, float calfactor)
     float GesamtSummeR = 0; 
     float GesamtSummeL=0; 
     unsigned int runcounter = 0;
+    int runs= 4;
     float MittelSummeR; 
     float MittelSummeL;
     unsigned int tickcheck= tickstogo*1.02;
@@ -89,7 +156,7 @@ float callibration_drive(unsigned int tickstogo, float calfactor)
       Serial.println(vr);
       int SummeR = 0;
       int SummeL=0;
-      for (int i=1; i<=4; i++) 
+      for (int i=1; i<=runs; i++) 
       {
         stepsprevl = coderacer.show_left_stepcounter();
         stepsprevr = coderacer.show_right_stepcounter();