Diff: MotorDriver.cpp

Revision:

11:68ee67d17320

Parent:

8:73c8188916dc

Child:

13:060d2a90d1c5

--- a/MotorDriver.cpp	Fri May 11 14:21:29 2018 +0000
+++ b/MotorDriver.cpp	Sat May 12 12:19:26 2018 +0000
@@ -60,14 +60,25 @@
 
     //Fährt bis Anzahl umdrehungen erreicht sind
     while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 990*NumbField) {
+        distance_front = readSensorValue(2);    //Distanz zur vorderen Wand wird gemessen
+        if(readSensorValue(1) < 15.0f)
+        {
         distance_side= readSensorValue(1);      //Distanz zur linken Wand wird gemessen
-        distance_front = readSensorValue(2);    //Distanz zur vorderen Wand wird gemessen
         AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
         distance_side = distance_side - 6.9f;         //Distanz zwischen Position und Ideallinie wird berechnet
-        CorrectFactor = distance_side*1.0f;      //P Faktor des P-Reglers
+        CorrectFactor = distance_side*2.0f;      //P Faktor des P-Reglers
+        }
+        else
+        {
+        distance_side= readSensorValue(3);      //Distanz zur linken Wand wird gemessen
+        AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
+        distance_side = distance_side - 6.9f;         //Distanz zwischen Position und Ideallinie wird berechnet
+        CorrectFactor = distance_side*-2.0f;      //P Faktor des P-Reglers
+        }
+
 
         //printf("%f\n",CorrectFactor);
-        if(abs(CorrectFactor) <= 5.0f) {    //erkennt Wand oder Lücke auf linker Seite
+        if(abs(CorrectFactor) <= 16.0f) {    //erkennt Wand oder Lücke auf linker Seite
             controller.setDesiredSpeedLeft((Speed) - CorrectFactor); //Bei Wand wird korrigiert
             controller.setDesiredSpeedRight((Speed * (-1.0f)) - CorrectFactor);
         } else {
@@ -90,7 +101,7 @@
 {
     controller.setDesiredSpeedLeft(40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
     controller.setDesiredSpeedRight(40.0f);
-    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
         //do nothing
     }
     stop();
@@ -106,7 +117,7 @@
 {
     controller.setDesiredSpeedRight(-40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
     controller.setDesiredSpeedLeft(-40.0f);
-    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
         //do nothing
     }
     stop();