From 4feb8b3598daf721cabd6c907a7c850161b495e4 Mon Sep 17 00:00:00 2001
From: jglueck <jonathangluecki@gmail.com>
Date: Thu, 9 Apr 2020 16:35:40 +0200
Subject: [PATCH] New routine for callibration

---
 vsode/coderacer_mkII/src/coderacer_main.cpp | 70 +++++++++++++++++----
 1 file changed, 57 insertions(+), 13 deletions(-)

diff --git a/vsode/coderacer_mkII/src/coderacer_main.cpp b/vsode/coderacer_mkII/src/coderacer_main.cpp
index 699fa41..d5ada6e 100644
--- a/vsode/coderacer_mkII/src/coderacer_main.cpp
+++ b/vsode/coderacer_mkII/src/coderacer_main.cpp
@@ -19,6 +19,7 @@ int aabstand[160];
 void filladistance();
 int adegree[160];
 int ispeed=1;
+float callibration_drive(unsigned int tickstogo, float calfactor);
 //---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
 void setup() {
     // Monitor
@@ -37,7 +38,6 @@ void setup() {
     drehung = links;
     coderacer.set_syncstop(true);
     coderacer.set_obstacle_stop(false);
-    coderacer.speed_settings(135, 135);
 }
 
 //---- 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,24 +51,68 @@ void loop()
       started = true;
       delay(1000);
     }
+  
+  float calf = callibration_drive(1000, 0.04);
+  Serial.println(calf);
+  float compare_calf = callibration_drive(1000, calf);
+  Serial.println(compare_calf);
+  }
 
-    unsigned int stepsprevl, stepsprevr ;
-    for(unsigned int v= 80;v<=255;v=v+10)
-    {
-      coderacer.speed_settings(v, v);
-      stepsprevl = coderacer.show_left_stepcounter();
-      stepsprevr = coderacer.show_right_stepcounter();
-      coderacer.drive_ticks_left(2000, true);
-      while (1== coderacer.is_driving()){}
-      Serial.printf("v=%i  links*(2000)=%i rechts=%i\n",v,  coderacer.show_left_stepcounter() - stepsprevl , coderacer.show_right_stepcounter() - stepsprevr);
-    }
     
 
-  }
+ 
+
+
+  
 }
 //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());
 
-
+float callibration_drive(unsigned int tickstogo, float calfactor)
+{
+    unsigned int stepsprevl; 
+    unsigned int stepsprevr;
+    float GesamtSummeR = 0; 
+    float GesamtSummeL=0;
+    unsigned int runcounter = 0;
+    float MittelSummeR;
+    float MittelSummeL;
+    unsigned int tickcheck= tickstogo*1.02;
+    for(unsigned int v= 190;v<=200;v=v+10)
+    {
+      unsigned int vr = v + calfactor*255;
+      unsigned int vl = v - calfactor*255;
+      runcounter ++;
+      coderacer.speed_settings(vl, vr);
+      Serial.println(vl);
+      Serial.println(vr);
+      int SummeR = 0;
+      int SummeL=0;
+      for (int i=1; i<=4; i++) 
+      {
+        stepsprevl = coderacer.show_left_stepcounter();
+        stepsprevr = coderacer.show_right_stepcounter();
+        coderacer.drive_ticks_left(tickstogo, true);
+        while (1== coderacer.is_driving()){}
+        delay(1000);
+        SummeR = SummeR + (coderacer.show_right_stepcounter() - stepsprevr);
+        SummeL= SummeL +(coderacer.show_left_stepcounter()- stepsprevl);
+        if (tickcheck<(coderacer.show_left_stepcounter() - stepsprevl))
+        {
+          Serial.printf("ERROR: calibration failed");
+          Serial.printf("links=%i\n" ,coderacer.show_left_stepcounter()- stepsprevl);
+          return 0;
+        } 
+        Serial.printf("v=%i links=%i rechts=%i\n" ,v, coderacer.show_left_stepcounter()- stepsprevl, coderacer.show_right_stepcounter() - stepsprevr);
+        
+      }
+      GesamtSummeR = GesamtSummeR+ SummeR/4.0;
+      GesamtSummeL = GesamtSummeL+ SummeL/4.0;
+    }
+    MittelSummeR= GesamtSummeR/(float)runcounter;
+    MittelSummeL= GesamtSummeL/(float)runcounter;
+    float CalibrationFactor= MittelSummeL/ MittelSummeR;
+    return CalibrationFactor;
+}