Diff: Turn.cpp

Revision:

8:1c8a747c49c8

Parent:

7:5ef09519a6e9

Child:

9:ab19796bf14a

--- a/Turn.cpp	Mon May 07 18:52:04 2018 +0000
+++ b/Turn.cpp	Wed May 16 12:15:23 2018 +0000
@@ -1,19 +1,21 @@
 #include <cmath>
 #include "Turn.h"
-
+//ausr startx und starty kann aus turn.cpp/turn.h entfernt werden
 using namespace std;
 
 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, int& dontStop):
+Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop, int& modeStart,int& path):
     counterLeft(counterLeft), 
     counterRight(counterRight),
     controller(controller),
     wallRight(wallRight),
     wallFront(wallFront),
     wallLeft(wallLeft),
-    dontStop(dontStop)
+    dontStop(dontStop),
+    path(path),
+    modeStart(modeStart)
 {}
 
 Turn::~Turn() {}
@@ -35,7 +37,7 @@
     
     //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
     
-    if (wallLeft == 0){ //Nach Links Drehen
+    if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
     
     //printf("Links ist frei\n");
     
@@ -50,15 +52,19 @@
     controller.setDesiredSpeedRight(0.5f);
     controller.setDesiredSpeedLeft(-0.5f);
     
+    
+    
     dontStop = 1;
     
-    }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
+    }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
     
         //printf("Vorne ist frei\n");
         
         dontStop = 2;
         
-        }else if (wallRight == 0) { //Nach Rechts Drehen
+        
+        
+        }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
             
             //printf("Rechts ist frei\n");
             
@@ -73,10 +79,12 @@
             controller.setDesiredSpeedRight(0.5f);
             controller.setDesiredSpeedLeft(-0.5f);
             
+            
+            
             dontStop = 3;
             
             
-            }else{ //Alle Wege versperrt-> Wenden
+            }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
             
                 //printf("Alles versperrt...zurueck\n");
             
@@ -91,6 +99,8 @@
                 controller.setDesiredSpeedRight(0.5f);  //0.0f
                 controller.setDesiredSpeedLeft(-0.5f);   //0.0f
                 
+                
+                
                 dontStop = 4;
             }
 }