Diff: Motion.cpp

Revision:

19:6cd6cc5c8b4c

Parent:

18:3309329d5f42

Child:

20:20573f55a5fd

diff -r 3309329d5f42 -r 6cd6cc5c8b4c Motion.cpp
--- a/Motion.cpp	Mon May 07 19:52:54 2018 +0000
+++ b/Motion.cpp	Thu May 10 18:10:15 2018 +0000
@@ -4,8 +4,8 @@
 
 using namespace std;
 
-const float Motion::LEFT_MID_VAL = 41.73f; //44.73
-const float Motion::RIGHT_MID_VAL = 44.03f; //47.03
+const float Motion::LEFT_MID_VAL = 40.73f; //44.73
+const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
 const float Motion::KP = 2.5;
 const float Motion::KD = 0.0f;
 const int Motion::MOVE_DIST = 1605;
@@ -36,18 +36,7 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        speedLeft = MOVE_SPEED;
-        speedRight = -MOVE_SPEED;
-        
-        //controller.setDesiredSpeedLeft(actSpeed);
-        //controller.setDesiredSpeedRight(-actSpeed);
-        
         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
-        
-        /*int partytime;
-        
-        if (avgSpeed == 0.0f) partytime = 1;
-        else partytime = 0;*/
             
         t.start();
         
@@ -63,75 +52,50 @@
                 
             avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
         
-           // if (partytime == 1) 
             accel(MOVE_SPEED);
             control();
-            /*
-            accel(MOVE_SPEED);
-            controller.setDesiredSpeedLeft(actSpeed);
-            controller.setDesiredSpeedRight(-actSpeed);
-            */
             
             if ((distanceC) < 40.0f) {
                 countsLOld = countsL;
                 countsROld = countsR;
+                
                 while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
                     countsL = counterLeft.read();
-                    countsR = counterRight.read(); 
+                    countsR = counterRight.read();
+                    distanceC = irSensorC.readC(); 
+                    if (distanceC > 40.0f) {
+                        stop();
+                        break;
+                    }
                 }
                 stop();
                 break;    
-            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 130.0f) && (distanceC > 40.0f)) {
+                
+            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
-                countsROld += 500;   
-            }/* else if ( distanceL < 80.0f || distanceR < 80.0f ) {
-                countsLOld = countsL;
-                countsROld = countsR;
-                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) {
+                countsROld += 500; 
+                  
+            }else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) {
+                countsLOld = counterLeft.read();
+                countsROld = counterRight.read();
+                
+                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) {
                     countsL = counterLeft.read();
                     countsR = counterRight.read(); 
+                    accel(MOVE_SPEED); 
+                    control();
                 }
                 stop();
                 break;
-            }*/
+            }
             
         }    
         
         t.stop();
         t.reset();
+        lastMove = false;
 }
-/**
- * Eine Feldstrecke mit höherer Geschwindigkeit fahren
- */
-void Motion::moveFast() {
-        
-        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();
-            distanceC = irSensorC.readC();
-            
-            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-            
-            control();
-            
-            if (distanceC < 40.0f) {
-                stop();
-                break;    
-            }
-        }    
-}
+
 /**
  * Eine Feldstrecke mit überprüfung der Ziellinie fahren
  */
@@ -145,13 +109,6 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        speedLeft = SCAN_SPEED;
-        speedRight = -SCAN_SPEED;
-        
-        //controller.setDesiredSpeedLeft(actSpeed);
-        //controller.setDesiredSpeedRight(-actSpeed);
-        
-        distanceC = irSensorC.readC();
         t.start();
         
         while ((countsL - countsLOld)  <  MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
@@ -166,7 +123,7 @@
                 enableMotorDriver = 1;
             }
             
-            if (lineSensor.read() == 1.0f) {
+            if (lineSensor.read() > 0.85f) {
                 line = 1;  
             }
             
@@ -183,18 +140,24 @@
                 while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
                     countsL = counterLeft.read();
                     countsR = counterRight.read(); 
+                    if (distanceC > 40.0f) {
+                        stop();
+                        break;
+                    }
                 }
                 stop();
                 break;    
             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
-            }/*else if ( distanceL > 80.0f || distanceR > 80.0f ) {
-                countsLOld = countsL;
-                countsROld = countsR;
-                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 330.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 330.0f) {
+            }/*else if ( ( distanceL > 80.0f || distanceR > 80.0f ) ) {
+                countsLOld = counterRight.read();
+                countsROld = counterRight.read();
+                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 404.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 404.0f) {
                     countsL = counterLeft.read();
-                    countsR = counterRight.read(); 
+                    countsR = counterRight.read();
+                    accel(SCAN_SPEED);
+                    control();
                 }
                 stop();
                 break;
@@ -202,6 +165,7 @@
         }  
         t.stop();
         t.reset();
+        lastMove = false;
 }
 /**
  * 90° Rotation nach Links
@@ -436,9 +400,11 @@
                 if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {
                     deceleration = true;
                     acceleration = false;
+                    lastMove = true;
                 }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
                     deceleration = true;
                     acceleration = false;
+                    lastMove = true;
                 }else{
                     deceleration = false;
                 }
@@ -457,11 +423,8 @@
                 rotateR();
                 break;
             case 4:
-                moveFast();
-                break;  
-            case 5:
                 stop();
-                break;  
+                break;    
         }
 }
 
@@ -474,14 +437,12 @@
 void Motion::accel(float targetSpeed) { 
 
         
-
-        avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
 
-        if ( avgSpeed < targetSpeed && deceleration == false) {
+        if ( avgSpeed < targetSpeed && deceleration == false && acceleration == false) {
                 
             actSpeed = ACCEL_CONST * t.read()*60.0f; 
-              
+            
         }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) {
             
             actSpeed = 2.5f * t.read()*60.0f;