Diff: Classes/Motion.cpp

Revision:

2:efa9a78591da

Parent:

1:6ef5bc60e69c

Child:

3:f44ef28cfb2d

--- a/Classes/Motion.cpp	Mon Apr 23 16:18:41 2018 +0000
+++ b/Classes/Motion.cpp	Wed Apr 25 11:16:16 2018 +0000
@@ -7,6 +7,8 @@
 #include "IRSensorK.h"
 #include "IRSensorZ.h"
 
+// Left + = Rückwärts Left - = Vorwärts 
+// Right + = Vorwärts Right - = Rückwärts
 
 using namespace std;
 
@@ -18,31 +20,18 @@
 
 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);
+    while(counterRight.read()-lastCountRight>(-1.35*1260)) {
+        controller.setDesiredSpeedLeft(150.0f);             //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(150.0f);
+        
+        
     }
 
     controller.setDesiredSpeedLeft(0.0f);                   //Drehzahl in [rpm]
@@ -51,11 +40,12 @@
     controller.resetCounter();
     int lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft<(2.094*1260)) {
-        controller.setDesiredSpeedLeft(spurhaltung.speedr());   //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(-spurhaltung.speedl());
+    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]
@@ -63,14 +53,36 @@
     
 }
 
+//------------------------------------------------------------------------------
+
+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::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);
+    while(counterLeft.read()-lastCountLeft<(1.35*1260)) {
+        controller.setDesiredSpeedLeft(-150.0f);            //Drehzahl in [rpm]
+        controller.setDesiredSpeedRight(-150.0f);
+        
     }
 
     controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
@@ -79,31 +91,38 @@
         controller.resetCounter();
     lastCountLeft = counterLeft.read();
 
-    while(counterLeft.read()-lastCountLeft<(2.094*1260)) {
-        controller.setDesiredSpeedLeft(spurhaltung.speedr());   //Drehzahl in [rpm]
-        controller.setDesiredSpeedRight(-spurhaltung.speedl());
+    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)) {
+    while(counterRight.read()-lastCountRight>(-2.7*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) {
@@ -129,8 +148,6 @@
         case 4:
             // 180 Drehen
             drehen180();
-            // Forward
-            gerade();
             break;
 
         default:
@@ -138,6 +155,7 @@
             controller.setDesiredSpeedRight(0.0f);
     }
     
+//------------------------------------------------------------------------------
     
 /*void Motion::switching()
 {