Diff: Turn.cpp

Revision:

1:d9e840c48b1e

Child:

2:592f01278db4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Turn.cpp	Sat Mar 31 16:45:57 2018 +0000
@@ -0,0 +1,80 @@
+#include <cmath>
+#include "Turn.h"
+
+using namespace std;
+
+const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit
+const int Turn::TURNINGCOUNTS = 2000;  //Entspricht Drehung um 90Grad
+
+Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
+    counterLeft(counterLeft), 
+    counterRight(counterRight),
+    controller(controller),
+    wallRight(wallRight),
+    wallFront(wallFront),
+    wallLeft(wallLeft)
+{}
+
+Turn::~Turn() {}
+
+
+void Turn::turning()
+{
+    int countsRight = counterRight.read();  //EncoderCounts auslesen  
+    int countsRight0 = countsRight;         //ReferenzCounts setzten
+    int countsLeft = counterLeft.read();
+    int countsLeft0 = countsLeft;
+    
+    //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
+    
+    if (wallLeft == 0){ //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", countsRight);
+        printf("%d\n", countsLeft);
+    }
+    controller.setDesiredSpeedRight(0.0f);
+    controller.setDesiredSpeedLeft(0.0f);
+    
+    }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
+    
+        printf("Vorne ist frei\n");
+        
+        }else if (wallRight == 0) { //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.0f);
+            controller.setDesiredSpeedLeft(0.0f);
+            
+            
+            }else{ //Alle Wege versperrt-> Wenden
+            
+                printf("Alles versperrt...zurueck\n");
+            
+                while((countsRight >= countsRight0 - 2*TURNINGCOUNTS) && (countsLeft >= countsLeft0 - 2*TURNINGCOUNTS)){
+                    controller.setDesiredSpeedRight(TURNINGSPEED);
+                    controller.setDesiredSpeedLeft(TURNINGSPEED);
+                    countsRight = counterRight.read();
+                    countsLeft = counterLeft.read();
+                    printf("%d\n", countsRight);
+                    printf("%d\n", countsLeft);
+                }
+                controller.setDesiredSpeedRight(0.0f);
+                controller.setDesiredSpeedLeft(0.0f);
+            }
+}