Diff: Classes/Motion.cpp

Revision:

3:f44ef28cfb2d

Parent:

2:efa9a78591da

Child:

4:3c6d2c035243

--- a/Classes/Motion.cpp	Wed Apr 25 11:16:16 2018 +0000
+++ b/Classes/Motion.cpp	Sun Apr 29 11:29:00 2018 +0000
@@ -7,7 +7,7 @@
 #include "IRSensorK.h"
 #include "IRSensorZ.h"
 
-// Left + = Rückwärts Left - = Vorwärts 
+// Left + = Rückwärts Left - = Vorwärts
 // Right + = Vorwärts Right - = Rückwärts
 
 using namespace std;
@@ -15,7 +15,7 @@
 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)
 {
-    
+
 }
 
 Motion::~Motion() {}
@@ -27,30 +27,16 @@
     controller.resetCounter();
     int lastCountRight = counterRight.read();
 
-    while(counterRight.read()-lastCountRight>(-1.35*1260)) {
+    while(counterRight.read()-lastCountRight>(-1.08*1260)) {
         controller.setDesiredSpeedLeft(150.0f);             //Drehzahl in [rpm]
         controller.setDesiredSpeedRight(150.0f);
-        
-        
+
+
     }
 
     controller.setDesiredSpeedLeft(0.0f);                   //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
-    
-    controller.resetCounter();
-    int lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft>(-2.094*1260)) {
-        controller.setDesiredSpeedLeft(-spurhaltung.speedr());   //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(spurhaltung.speedl());
-        spurhaltung.speedl();
-        spurhaltung.speedr();
-
-    }
-
-    controller.setDesiredSpeedLeft(0.0f);                       //Drehzahl in [rpm]
-    controller.setDesiredSpeedRight(0.0f);
-    
 }
 
 //------------------------------------------------------------------------------
@@ -60,14 +46,15 @@
 
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
-
-    while(counterLeft.read()-lastCountLeft>(-2.094*1260)) {
-        controller.setDesiredSpeedLeft(-spurhaltung.speedr());   //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(spurhaltung.speedl());
-        spurhaltung.speedl();
-        spurhaltung.speedr();
+    
+    if (Sensor1.read() == 0) {
+        while(counterLeft.read()-lastCountLeft>(-2.0*1260) || Sensor1.read() == 0) {
+            spurhaltung.speedl();
+            spurhaltung.speedr();
+            controller.setDesiredSpeedLeft(-spurhaltung.speedr());   //Drehzahl in [rpm]
+            controller.setDesiredSpeedRight(spurhaltung.speedl());
+        }
     }
-
     controller.setDesiredSpeedLeft(0.0f);                       //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
 }
@@ -79,28 +66,15 @@
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft<(1.35*1260)) {
+    while(counterLeft.read()-lastCountLeft<(1.08*1260)) {
         controller.setDesiredSpeedLeft(-150.0f);            //Drehzahl in [rpm]
         controller.setDesiredSpeedRight(-150.0f);
-        
+
     }
 
     controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
     controller.setDesiredSpeedRight(0.0f);
-    
-        controller.resetCounter();
-    lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft<(-2.094*1260)) {
-        controller.setDesiredSpeedLeft(-spurhaltung.speedr());   //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(spurhaltung.speedl());
-        spurhaltung.speedl();
-        spurhaltung.speedr();
-        
-    }
-
-    controller.setDesiredSpeedLeft(0.0f);                       //Drehzahl in [rpm]
-    controller.setDesiredSpeedRight(0.0f);
 }
 
 //------------------------------------------------------------------------------
@@ -110,11 +84,11 @@
     controller.resetCounter();
     int lastCountRight = counterRight.read();
 
-    while(counterRight.read()-lastCountRight>(-2.7*1260)) {
+    while(counterRight.read()-lastCountRight>(-2.16*1260)) {
         controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
         controller.setDesiredSpeedRight(150.0f);
-        
-        
+
+
     }
 
     controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
@@ -130,7 +104,7 @@
             // Left
             drehenl90();
             // Forward
-            //gerade();
+            gerade();
             break;
 
         case 2:
@@ -154,30 +128,30 @@
             controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
             controller.setDesiredSpeedRight(0.0f);
     }
-    
+
 //------------------------------------------------------------------------------
-    
-/*void Motion::switching()
-{
-     if(Sensor4.read() == 0) {
-            // Left
-            drehenl90();
-            // Forward
-            gerade();
-    } else if (Sensor1.read() == 0) {
-            // Forward
-            gerade();
-    } else if (Sensor5.read() == 0) {
-            // Right
-            drehenr90();
-            // Forward
-            gerade();
-    } else {
-            // 180 Drehen
-            drehen180();
-            // Forward
-            gerade();
 
-    }*/  
+    /*void Motion::switching()
+    {
+         if(Sensor4.read() == 0) {
+                // Left
+                drehenl90();
+                // Forward
+                gerade();
+        } else if (Sensor1.read() == 0) {
+                // Forward
+                gerade();
+        } else if (Sensor5.read() == 0) {
+                // Right
+                drehenr90();
+                // Forward
+                gerade();
+        } else {
+                // 180 Drehen
+                drehen180();
+                // Forward
+                gerade();
+
+        }*/
 
 }