Diff: Motion.cpp

Revision:

28:b7ce1e3bf08b

Parent:

27:f111ba194412

Child:

29:0419f4873807

Child:

30:bdb8c92434a0

--- a/Motion.cpp	Tue May 15 18:34:39 2018 +0000
+++ b/Motion.cpp	Thu May 17 16:39:03 2018 +0000
@@ -56,7 +56,7 @@
             else accel(MOVE_SPEED);
             control();
             
-            if ((distanceC) < 40.0f) {
+            if ((distanceC) < 40.0f && speedRun == false) {
                 countsLOld = countsL;
                 countsROld = countsR;
                 
@@ -72,6 +72,9 @@
                 stop();
                 break;    
                 
+            }else if (distanceC < 101.0f && speedRun == true) {
+                stop();
+                break;   
             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500; 
@@ -127,7 +130,6 @@
             control();
             
             if (distanceC < 101.0f && lastMove == false) {
-                stop();
                 break;    
             }else if (distanceC < 40.0f && lastMove == true) {
                 stop();
@@ -273,7 +275,16 @@
         controller.setDesiredSpeedLeft(24.15f);
         controller.setDesiredSpeedRight(-115.85f);  
         
-        while ((countsL - countsLOld)  < 440 || (countsR - countsROld) > -2148) {
+        /*  Velocity Settings:
+            50rpm   ->  17.25   :   82.75
+            60rpm   ->  20.7    :   99.3
+            70rpm   ->  24.15   :   115.85
+            80rpm   ->  27.6    :   132.4
+            90rpm   ->  31.05   :   148.95
+            100rpm  ->  34.5    :   165.5
+        */
+        
+        while ((countsL - countsLOld)  < 446 || (countsR - countsROld) > -2141) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -292,10 +303,10 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(115.85f);
+        controller.setDesiredSpeedLeft(115.85f);                                            
         controller.setDesiredSpeedRight(-24.15f);
         
-        while ((countsL - countsLOld)  < 2148 || (countsR - countsROld) > -440) {
+        while ((countsL - countsLOld)  < 2141 || (countsR - countsROld) > -446) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -433,8 +444,8 @@
 
 void Motion::runTask(int path[],int task, bool reverse, int junction) {
       
-        if (reverse == true) speedRun = 1;
-        else speedRun = 0;
+        if (reverse == false) speedRun = true;
+        else speedRun = false;
       
         switch(path[task]) {
             
@@ -467,7 +478,7 @@
                     lastMove = true;
                     longMove = false;
                     
-                }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
+                }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) {
                     
                     deceleration = true;
                     acceleration = false;
@@ -517,7 +528,6 @@
                 if (reverse == false && path[task+1] == 0) {
                     
                     lastMove = true;
-                       
                 }
                 
             
@@ -544,30 +554,35 @@
 
         float fastSpeed;
         
+        //Acclerated Target Speed
         if (speedRun == true) fastSpeed = targetSpeed*2.0f;
         else fastSpeed = targetSpeed*2.8f;
         
         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
-
-        if (avgSpeed < targetSpeed && deceleration == false && acceleration == false) {
+        
+        //Acceleration logic
+        
+        //Short distance
+        if (avgSpeed < targetSpeed && deceleration == false && acceleration == false && longMove == false) {
                 
-            actSpeed = ACCEL_CONST * t.read()*60.0f; 
+            actSpeed = ACCEL_CONST * t.read()*60.0f;                //Acceleration equation
             
         }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
             
-            actSpeed =  55.0f;
+            actSpeed =  targetSpeed+5.0f;                           //Keep Speed steady in case of overshooting
             
-        }else if (avgSpeed < fastSpeed && acceleration == true) {
+        //Long distance
+        }else if (avgSpeed < fastSpeed && acceleration == true && deceleration == false) {
             
-            actSpeed = ACCEL_CONST * t.read()*60.0f; 
+            actSpeed = ACCEL_CONST * t.read()*60.0f;    
                
-        }else if (avgSpeed > targetSpeed+5.0f && deceleration == true) {
+        }else if (avgSpeed > targetSpeed+5.0f && acceleration == false && deceleration == true) {
         
-            actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f;
+            actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f;    //Deceleration equation
             
-        }else if (avgSpeed < targetSpeed && deceleration == true) {
+        }else if (avgSpeed < targetSpeed && acceleration == false && deceleration == true) {
             
-            actSpeed = 51.0f;
+            actSpeed = targetSpeed+5.0f;                            //Limit deceleration in case of overshooting
             
         }    
 }