Diff: Classes/Motion.cpp

Revision:

0:b886f13e4ac6

Child:

1:6ef5bc60e69c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Classes/Motion.cpp	Sun Apr 22 16:14:54 2018 +0000
@@ -0,0 +1,90 @@
+#include "Motion.h"
+#include "Controller.h"
+#include "EncoderCounter.h"
+#include "Spurhaltung.h"
+
+
+using namespace std;
+
+Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung):
+    counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung)
+{
+
+}
+
+Motion::~Motion() {}
+
+
+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);
+            // 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);
+            break;
+
+
+        case 2:
+            //Forward
+            controller.setDesiredSpeedLeft(spurhaltung.speedr()); //Drehzahl in [rpm]
+            controller.setDesiredSpeedRight(-spurhaltung.speedl());
+            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);
+            // 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);
+            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);
+    }
+
+}