Diff: Motion.cpp

Revision:

3:076dd7ec7eb4

Parent:

2:f898adf2d817

Child:

4:932eb2d29206

--- a/Motion.cpp	Thu Apr 19 11:26:51 2018 +0000
+++ b/Motion.cpp	Fri Apr 20 13:37:21 2018 +0000
@@ -4,9 +4,9 @@
 
 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::LEFT_MID_VAL = 44.73f;
+const float Motion::RIGHT_MID_VAL = 47.03f;
+const float Motion::KP = 3.0f;
 const float Motion::KD = 0.00f;
 
 
@@ -98,12 +98,13 @@
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
+            distanceC = irSensorC.readC();
             
             if (enableMotorDriver == 0) {
                 enableMotorDriver = 1;
             }
             
-            if (lineSensor.read() > 0.9f) {
+            if (lineSensor.read() == 1.0f) {
                 line = 1;  
             }else{
                 line = 0;
@@ -111,6 +112,11 @@
             
             control();
             
+            if (distanceC < 40.0f) {
+                stop();
+                break;    
+            }
+            
         }    
         
 }
@@ -168,25 +174,7 @@
 /**
  * Links abbiegen
  */
-void Motion::turnL() {
-    
-        
-        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)  < 824 || (countsR - countsROld) > -824) {
-            
-            countsL = counterLeft.read();
-            countsR = counterRight.read();
-        
-            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-        }      
+void Motion::turnL() {     
         
         controller.counterReset();
         countsLOld = counterLeft.read();
@@ -203,47 +191,12 @@
             countsR = counterRight.read();
         
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-        } 
-        
-        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)  < 824 || (countsR - countsROld) > -824) {
-            
-            countsL = counterLeft.read();
-            countsR = counterRight.read();
-        
-            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
         }         
 }
 /**
  * Rechts abbiegen
  */
-void Motion::turnR() {
-    
-        
-        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)  < 824 || (countsR - countsROld) > -824) {
-            
-            countsL = counterLeft.read();
-            countsR = counterRight.read();
-        
-            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-        }      
+void Motion::turnR() {    
         
         controller.counterReset();
         countsLOld = counterLeft.read();
@@ -260,24 +213,7 @@
             countsR = counterRight.read();
         
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-        }    
-        
-        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)  < 824 || (countsR - countsROld) > -824) {
-            
-            countsL = counterLeft.read();
-            countsR = counterRight.read();
-        
-            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-        }      
+        }        
 }
 /**
  * Motor Stop