Diff: Turn.cpp

Revision:

7:5ef09519a6e9

Parent:

6:a09d2ee3b82e

--- a/Turn.cpp	Wed Apr 25 12:49:48 2018 +0000
+++ b/Turn.cpp	Mon May 07 18:52:04 2018 +0000
@@ -3,16 +3,17 @@
 
 using namespace std;
 
-const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
-const int Turn::TURNINGCOUNTS = 868;  //Entspricht Drehung um 90Grad //DONT TOUCH
+const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
+const int Turn::TURNINGCOUNTS = 950;  //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070
 
-Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
+Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop):
     counterLeft(counterLeft), 
     counterRight(counterRight),
     controller(controller),
     wallRight(wallRight),
     wallFront(wallFront),
-    wallLeft(wallLeft)
+    wallLeft(wallLeft),
+    dontStop(dontStop)
 {}
 
 Turn::~Turn() {}
@@ -25,6 +26,13 @@
     int countsLeft = counterLeft.read();
     int countsLeft0 = countsLeft;
     
+    //vor dem abbiegen halten
+    controller.setDesiredSpeedRight(0.5f);
+    controller.setDesiredSpeedLeft(-0.5f);
+    //wait(0.1f);
+    
+    
+    
     //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
     
     if (wallLeft == 0){ //Nach Links Drehen
@@ -40,12 +48,16 @@
         //printf("%d\n\r", countsLeft);
     }
     controller.setDesiredSpeedRight(0.5f);
-    controller.setDesiredSpeedLeft(0.5f);
+    controller.setDesiredSpeedLeft(-0.5f);
+    
+    dontStop = 1;
     
     }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
     
         //printf("Vorne ist frei\n");
         
+        dontStop = 2;
+        
         }else if (wallRight == 0) { //Nach Rechts Drehen
             
             //printf("Rechts ist frei\n");
@@ -59,14 +71,16 @@
                 //printf("%d\n", countsLeft);
             }
             controller.setDesiredSpeedRight(0.5f);
-            controller.setDesiredSpeedLeft(0.5f);
+            controller.setDesiredSpeedLeft(-0.5f);
+            
+            dontStop = 3;
             
             
             }else{ //Alle Wege versperrt-> Wenden
             
                 //printf("Alles versperrt...zurueck\n");
             
-                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){
+                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
                     controller.setDesiredSpeedRight(TURNINGSPEED);
                     controller.setDesiredSpeedLeft(TURNINGSPEED);
                     countsRight = counterRight.read();
@@ -75,6 +89,8 @@
                     //printf("%d\n", countsLeft);
                 }
                 controller.setDesiredSpeedRight(0.5f);  //0.0f
-                controller.setDesiredSpeedLeft(0.5f);   //0.0f
+                controller.setDesiredSpeedLeft(-0.5f);   //0.0f
+                
+                dontStop = 4;
             }
 }