Diff: Classes/Motion.cpp

Revision:

5:93d3efe46493

Parent:

4:3c6d2c035243

diff -r 3c6d2c035243 -r 93d3efe46493 Classes/Motion.cpp
--- a/Classes/Motion.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Motion.cpp	Wed May 09 13:33:59 2018 +0000
@@ -7,11 +7,17 @@
 #include "IRSensorK.h"
 #include "IRSensorZ.h"
 
-// Left + = Rückwärts Left - = Vorwärts
-// Right + = Vorwärts Right - = Rückwärts
+// Left + = Vorwärts Left - = Vorwärts
+// Right + = Rückwärts Right - = Rückwärts
+
+
 
 using namespace std;
 
+const float Motion::SPEED = 150;
+const float Motion::DREHEN90 = 1607;
+const float Motion::GERADE = 3280;
+
 Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung, IRSensorGF& Sensor1, IRSensorG& Sensor2, IRSensorG& Sensor3, IRSensorK& Sensor4, IRSensorK& Sensor5, IRSensorZ& Sensor6):
     counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung), Sensor1(Sensor1), Sensor2(Sensor2), Sensor3(Sensor3), Sensor4(Sensor4), Sensor5(Sensor5), Sensor6(Sensor6)
 {
@@ -27,9 +33,9 @@
     controller.resetCounter();
     int lastCountRight = counterRight.read();
 
-    while(counterRight.read()-lastCountRight>(-1.19*1260)) {
-        controller.setDesiredSpeedLeft(150.0f);             //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(150.0f);
+    while(counterRight.read()-lastCountRight > -DREHEN90) {
+        controller.setDesiredSpeedLeft(-SPEED);             //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-SPEED);
     }
     controller.setDesiredSpeedLeft(0.0f);                   //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
@@ -43,14 +49,23 @@
 
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
-    
-    if (Sensor1.read() == 0) {
-        while(counterLeft.read()-lastCountLeft>(-2.45*1260)) {
-            spurhaltung.speedl();
-            spurhaltung.speedr();
-            controller.setDesiredSpeedLeft(-spurhaltung.speedr());   //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(spurhaltung.speedl());
+
+    while((counterLeft.read()-lastCountLeft < GERADE) && (Sensor1.read() > 120 )) { // 1.806
+        controller.setDesiredSpeedLeft(spurhaltung.speedl());   //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-spurhaltung.speedr());
+        printf("");
+        //printf("SpurhaltungLinks: %d\r\n", spurhaltung.speedl());
+        //printf("SpurhaltungRechts: %d\r\n", spurhaltung.speedr());
+        //printf("Sensor2:%d\r\n", Sensor2.read());
+        //printf("Sensor1:%d\r\n\r\n", Sensor1.read());
+    }
+    if((Sensor1.read() > 120) && (Sensor1.read() < 250)) {
+        while(Sensor1.read() > 120) {
+            controller.setDesiredSpeedLeft(spurhaltung.speedl());   //Drehzahl in [rpm]
+            controller.setDesiredSpeedRight(-spurhaltung.speedr());
+            printf("");
         }
+
     }
     controller.setDesiredSpeedLeft(0.0f);                       //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
@@ -63,9 +78,9 @@
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft<(1.19*1260)) {
-        controller.setDesiredSpeedLeft(-150.0f);            //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(-150.0f);
+    while(counterLeft.read()-lastCountLeft < DREHEN90) {
+        controller.setDesiredSpeedLeft(SPEED);            //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(SPEED);
     }
     controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
@@ -79,7 +94,7 @@
     controller.resetCounter();
     int lastCountRight = counterRight.read();
 
-    while(counterRight.read()-lastCountRight>(-2.38*1260)) {
+    while(counterRight.read()-lastCountRight < 2*1607 + 150) {
         controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
         controller.setDesiredSpeedRight(150.0f);
     }
@@ -95,25 +110,31 @@
         case 1:
             // Left
             drehenl90();
+            wait(0.05f);
             // Forward
             gerade();
+            wait(0.05f);
             break;
 
         case 2:
             // Forward
             gerade();
+            wait(0.05f);
             break;
 
         case 3:
             // Right
             drehenr90();
+            wait(0.05f);
             // Forward
             gerade();
+            wait(0.05f);
             break;
 
         case 4:
             // 180 Drehen
             drehen180();
+            wait(0.05f);
             break;
 
         default: