Diff: Motion.cpp

Revision:

5:47262622a9bf

Parent:

4:e74c06e43485

Child:

8:862bf9225953

--- a/Motion.cpp	Wed Apr 11 15:26:03 2018 +0000
+++ b/Motion.cpp	Mon Apr 16 12:44:48 2018 +0000
@@ -10,71 +10,137 @@
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
                 EncoderCounter& counterRight, IRSensor& irSensorL,
-                IRSensor& irSensorC, IRSensor& irSensorR) :
+                IRSensor& irSensorC, IRSensor& irSensorR, 
+                DigitalOut& enableMotorDriver) :
                 controller(controller), counterLeft(counterLeft),
                 counterRight(counterRight), irSensorL(irSensorL),
-                irSensorC(irSensorC), irSensorR(irSensorR) {
+                irSensorC(irSensorC), irSensorR(irSensorR),
+                enableMotorDriver(enableMotorDriver) {
                     
-                    countsLeft = 0;
-                    countsRight = 0;
+                    countsL = 0;
+                    countsR = 0;
+                    countsLOld = 0;
+                    countsROld = 0 ;
 }
                
 Motion::~Motion() {}
 
+/**
+ * Eine Feldbewegung druchführen
+ */
 void Motion::move() {
     
-        distanceL = irSensorL.read();
-        distanceC = irSensorC.read();
-        distanceR = irSensorR.read();
+        //distanceL = irSensorL.read();
+        //distanceC = irSensorC.read();
+        //distanceR = irSensorR.read();
+        
+        while ((countsL - countsLOld)  < 1630 || (countsR - countsROld) > -1630) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
         
-    while(countsLeft  < 200 || countsRight < 200) { 
-        //Counts für eine Feld Bewegung
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
-    
-        controller.setDesiredSpeedLeft(50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
-    
-    }
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+        }    
+        //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL + 150;
+        countsROld = countsR - 150;
 }
-    
+
+/**
+ * 90° Rotation nach Links
+ */
 void Motion::rotateL() {
     
-    while(countsLeft  < 200 || countsRight > -200) { 
-        //Count Grenze durch Rechnung bestimmen für 90° Umfang/4
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
-    
-        controller.setDesiredSpeedLeft(50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
-    
-    }
+        while ((countsL - countsLOld)  > -760 || (countsR - countsROld) > -760) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(-50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+        }    
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL - 100;
+        countsROld = countsR - 100;
 }
     
+/**
+ * 90° Rotation nach Rechts
+ */
 void Motion::rotateR() {
     
-    while(countsLeft  > -200 || countsRight < 200) { 
-        //Count Grenze durch Rechnung bestimmen für 90° Umfang/4
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
-    
-        controller.setDesiredSpeedLeft(-50.0f);
-        controller.setDesiredSpeedRight(50.0f);
-    
-    }
+        while ((countsL - countsLOld)  < 760 || (countsR - countsROld) < 760) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(50.0f);
+            enableMotorDriver = 1;
+        }    
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL + 100;
+        countsROld = countsR + 100;
 }
 
-//180° rotation
-void Motion::reverse() {
+/**
+ * Motor Stop
+ */
+void Motion::stop() {
+       
+        controller.setDesiredSpeedLeft(0.0f);
+        controller.setDesiredSpeedRight(0.0f);
+}
+/**
+ * 180° Rotation
+ */
+void Motion::rotate180() {
     
-    while(countsLeft  < 200 || countsRight > -200) { 
-        //Count Grenze durch Rechnung bestimmen für 180° Umfang/2
-        countsLeft = counterLeft.read();
-        countsRight = counterRight.read();
+        while ((countsL - countsLOld)  > -1560 || (countsR - countsROld) > -1560) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(-50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+            printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
+        }    
+        
+        stop();
+        enableMotorDriver = 0;
+        countsLOld = countsL - 150;
+        countsROld = countsR - 150;
+        
+}
+
+void Motion::test() {
+        
+        while (countsL > -1560 || countsR > - 1560) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            controller.setDesiredSpeedLeft(-50.0f);
+            controller.setDesiredSpeedRight(-50.0f);
+            enableMotorDriver = 1;
+            printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
+        }
+        
+        counterLeft.reset();
+        counterRight.reset();
+        stop();
+        enableMotorDriver = 0;
+}
+
     
-        controller.setDesiredSpeedLeft(50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
-    
-    }
-}
-