Diff: Classes/Motion.cpp

Revision:

1:6ef5bc60e69c

Parent:

0:b886f13e4ac6

Child:

2:efa9a78591da

--- a/Classes/Motion.cpp	Sun Apr 22 16:14:54 2018 +0000
+++ b/Classes/Motion.cpp	Mon Apr 23 16:18:41 2018 +0000
@@ -2,89 +2,164 @@
 #include "Controller.h"
 #include "EncoderCounter.h"
 #include "Spurhaltung.h"
+#include "IRSensorGF.h"
+#include "IRSensorG.h"
+#include "IRSensorK.h"
+#include "IRSensorZ.h"
 
 
 using namespace std;
 
-Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung):
-    counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung)
+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() {}
 
+void Motion::gerade()
+{
+
+    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);
+}
+
+void Motion::drehenl90()
+{
+    controller.resetCounter();
+    int lastCountRight = counterRight.read();
+
+    while(counterRight.read()-lastCountRight>(-1.15*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);
+    
+}
+
+void Motion::drehenr90()
+{
+    controller.resetCounter();
+    int lastCountLeft = counterLeft.read();
+
+    while(counterLeft.read()-lastCountLeft<(1.15*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);
+}
+
+void Motion::drehen180()
+{
+    controller.resetCounter();
+    int lastCountRight = counterRight.read();
+
+    while(counterRight.read()-lastCountRight>(-2.3*1260)) {
+        controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(150.0f);
+    }
+
+    controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
+    controller.setDesiredSpeedRight(0.0f);
+}
 
 void Motion::switching(int D)
 {
     switch(D) {
         case 1:
-            //Left and forward
-            controller.resetCounter();
-            int lastCountRight = counterRight.read();
-            while(counterRight.read()-lastCountRight>(-1.15*1260)) {
-                controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
-                controller.setDesiredSpeedRight(150.0f);
-                
-            }
-            controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(0.0f);
+            // Left
+            drehenl90();
             // Forward
-            controller.resetCounter();
-            int lastCountLeft = counterLeft.read();
-            while(counterLeft.read()-lastCountLeft<(2.094*1260)) {
-                controller.setDesiredSpeedLeft(spurhaltung.speedr()); //Drehzahl in [rpm]
-                controller.setDesiredSpeedRight(-spurhaltung.speedl());
-            }
-            controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(0.0f);
+            //gerade();
             break;
 
-
         case 2:
-            //Forward
-            controller.setDesiredSpeedLeft(spurhaltung.speedr()); //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(-spurhaltung.speedl());
+            // Forward
+            gerade();
             break;
 
-
         case 3:
-            //Right and drive
-            controller.resetCounter();
-            lastCountLeft = counterLeft.read();
-            while(counterLeft.read()-lastCountLeft<(1.15*1260)) {
-                controller.setDesiredSpeedLeft(-150.0f); //Drehzahl in [rpm]
-                controller.setDesiredSpeedRight(-150.0f);
-            }
-            controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(0.0f);
+            // Right
+            drehenr90();
             // Forward
-            controller.resetCounter();
-            lastCountLeft = counterLeft.read();
-            while(counterLeft.read()-lastCountLeft<(2.094*1260)) {
-                controller.setDesiredSpeedLeft(spurhaltung.speedr()); //Drehzahl in [rpm]
-                controller.setDesiredSpeedRight(-spurhaltung.speedl());
-            }
-            controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(0.0f);
+            gerade();
             break;
 
+        case 4:
+            // 180 Drehen
+            drehen180();
+            // Forward
+            gerade();
+            break;
 
-        case 4:
-            // 180 -> right
-            controller.resetCounter();
-            lastCountRight = counterRight.read();
-            while(counterRight.read()-lastCountRight>(-2.3*1260)) {
-                controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
-                controller.setDesiredSpeedRight(150.0f);
-            }
-            controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(0.0f);
-            break;
-            
         default:
             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();
+
+    }*/  
 
 }