Diff: Motion.cpp

Revision:

1:2b5f79285a3e

Child:

2:f898adf2d817

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.cpp	Thu Apr 19 06:19:43 2018 +0000
@@ -0,0 +1,268 @@
+
+#include <cmath>
+#include "Motion.h"
+
+using namespace std;
+
+const float Motion::LEFT_MID_VAL = 45.0f;
+const float Motion::RIGHT_MID_VAL = 45.0f;
+const float Motion::KP = 2.5f;
+const float Motion::KD = 0.00f;
+
+
+Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
+                EncoderCounter& counterRight, IRSensor& irSensorL,
+                IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
+                DigitalOut& enableMotorDriver) :
+                controller(controller), counterLeft(counterLeft),
+                counterRight(counterRight), irSensorL(irSensorL),
+                irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
+                enableMotorDriver(enableMotorDriver) {}
+               
+Motion::~Motion() {}
+
+/**
+ * Eine Feldstrecke druchführen
+ */
+void Motion::move() {
+        
+        controller.counterReset();
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        speedLeft = 50.0f;
+        speedRight = -50.0f;
+        
+        controller.setDesiredSpeedLeft(speedLeft);
+        controller.setDesiredSpeedRight(speedRight);
+        
+        while ((countsL - countsLOld)  < 1647 || (countsR - countsROld) > -1647) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+            
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+            
+            control();
+            
+        }    
+}
+/**
+ * Eine Feldstrecke mit höherer Geschwindigkeit fahren
+ */
+void Motion::moveFast() {
+        
+        controller.counterReset();
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        speedLeft = 100.0f;
+        speedRight = -100.0f;
+        
+        controller.setDesiredSpeedLeft(speedLeft);
+        controller.setDesiredSpeedRight(speedRight);
+        
+        while ((countsL - countsLOld)  < 1647 || (countsR - countsROld) > -1647) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+            
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+            
+            control();
+            
+        }    
+}
+/**
+ * Eine Feldstrecke mit überprüfung der Ziellinie fahren
+ */
+void Motion::scanMove() {
+        
+        controller.counterReset();
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        speedLeft = 50.0f;
+        speedRight = -50.0f;
+        
+        controller.setDesiredSpeedLeft(speedLeft);
+        controller.setDesiredSpeedRight(speedRight);
+        
+        while ((countsL - countsLOld)  <  1647 || (countsR - countsROld) > -1647) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+            
+            if (enableMotorDriver == 0) {
+                enableMotorDriver = 1;
+            }
+            
+            if (lineSensor.read() > 0.9f) {
+                line = 1;  
+            }else{
+                line = 0;
+            }
+            
+            control();
+            
+        }    
+        
+}
+/**
+ * 90° Rotation nach Links
+ */
+void Motion::rotateL() {
+    
+        stop();
+        controller.counterReset();
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        controller.setDesiredSpeedLeft(-20.0f);
+        controller.setDesiredSpeedRight(-20.0f);
+        
+        while ((countsL - countsLOld)  > -862 || (countsR - countsROld) > -862) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+            
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+        }    
+        
+        stop();
+}
+    
+/**
+ * 90° Rotation nach Rechts
+ */
+void Motion::rotateR() {
+    
+        stop();
+        controller.counterReset();
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        controller.setDesiredSpeedLeft(20.0f);
+        controller.setDesiredSpeedRight(20.0f);
+        
+        while ((countsL - countsLOld)  < 862 || (countsR - countsROld) < 862) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+        }    
+        
+        stop();
+}
+
+/**
+ * Motor Stop
+ */
+void Motion::stop() {
+        
+        controller.setDesiredSpeedLeft(0.0f);
+        controller.setDesiredSpeedRight(0.0f);
+        
+        float sL = controller.getSpeedLeft();
+        float ticks = 0.08f*sL;
+        
+        waitStop = 0;
+        while( waitStop < ticks) {
+            waitStop+= 1;
+        }
+}
+/**
+ * 180° Rotation
+ */
+void Motion::rotate180() {
+    
+        stop();
+        controller.counterReset();
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        controller.setDesiredSpeedLeft(-50.0f);
+        controller.setDesiredSpeedRight(-50.0f);
+        
+        while ((countsL - countsLOld)  > -1723 || (countsR - countsROld) > -1723) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+        }    
+        
+        stop();
+}
+
+
+void Motion::control() {
+        
+    //float wallLeft = 70.0f;
+    //float wallRight = 70.0f;
+    
+    distanceL = irSensorL.readL();
+    distanceR = irSensorR.readR();
+    
+    if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+        
+        errorP = LEFT_MID_VAL - distanceL;
+           
+    }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
+        
+        errorP = distanceR - RIGHT_MID_VAL;
+         
+    }else{
+        
+        errorP = 0;   
+    }
+    
+    errorD = errorP - oldErrorP;  
+    
+    oldErrorP = errorP;
+    
+    totalError = KP*errorP + KD*errorD;
+    
+    controller.setDesiredSpeedLeft(speedLeft + totalError);
+    controller.setDesiredSpeedRight(speedRight + totalError);
+}
+
+void Motion::runTask(int task) {
+      
+    switch(task) {
+        
+        case 1:
+            move();
+            break;
+        case 2:
+            rotateL();
+            break;
+        case 3:
+            rotateR();
+            break;
+        case 4:
+            moveFast();
+            break;  
+        case 5:
+            stop();
+            break;  
+    }
+}
+
+int Motion::finish() {
+    
+    return line;   
+}