Diff: Turn.cpp

Revision:

9:ab19796bf14a

Parent:

8:1c8a747c49c8

--- a/Turn.cpp	Wed May 16 12:15:23 2018 +0000
+++ b/Turn.cpp	Wed May 16 16:41:44 2018 +0000
@@ -14,8 +14,9 @@
     wallFront(wallFront),
     wallLeft(wallLeft),
     dontStop(dontStop),
-    path(path),
-    modeStart(modeStart)
+    modeStart(modeStart),
+    path(path)
+    
 {}
 
 Turn::~Turn() {}
@@ -28,79 +29,51 @@
     int countsLeft = counterLeft.read();
     int countsLeft0 = countsLeft;
     
-    //vor dem abbiegen halten
+    //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 ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
-    
-    //printf("Links ist frei\n");
-    
     while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
         controller.setDesiredSpeedRight(TURNINGSPEED);
         controller.setDesiredSpeedLeft(TURNINGSPEED);
         countsRight = counterRight.read();
         countsLeft = counterLeft.read();
-        //printf("%d\n\r", countsRight);
-        //printf("%d\n\r", countsLeft);
     }
+    
     controller.setDesiredSpeedRight(0.5f);
     controller.setDesiredSpeedLeft(-0.5f);
     
-    
-    
     dontStop = 1;
     
     }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
     
-        //printf("Vorne ist frei\n");
-        
         dontStop = 2;
         
-        
-        
         }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
             
-            //printf("Rechts ist frei\n");
-            
             while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
                 controller.setDesiredSpeedRight(-TURNINGSPEED);
                 controller.setDesiredSpeedLeft(-TURNINGSPEED);
                 countsRight = counterRight.read();
                 countsLeft = counterLeft.read();
-                //printf("%d\n", countsRight);
-                //printf("%d\n", countsLeft);
             }
             controller.setDesiredSpeedRight(0.5f);
             controller.setDesiredSpeedLeft(-0.5f);
-            
-            
-            
             dontStop = 3;
             
             
             }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
-            
-                //printf("Alles versperrt...zurueck\n");
-            
                 while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
                     controller.setDesiredSpeedRight(TURNINGSPEED);
                     controller.setDesiredSpeedLeft(TURNINGSPEED);
                     countsRight = counterRight.read();
                     countsLeft = counterLeft.read();
-                    //printf("%d\n", countsRight);
-                    //printf("%d\n", countsLeft);
                 }
                 controller.setDesiredSpeedRight(0.5f);  //0.0f
                 controller.setDesiredSpeedLeft(-0.5f);   //0.0f
                 
-                
-                
                 dontStop = 4;
             }
 }