Diff: Motion.cpp

Revision:

27:f111ba194412

Parent:

26:f964408401fa

Child:

28:b7ce1e3bf08b

--- a/Motion.cpp	Tue May 15 17:57:59 2018 +0000
+++ b/Motion.cpp	Tue May 15 18:34:39 2018 +0000
@@ -10,7 +10,7 @@
 const float Motion::KD = 0.0f;
 const int Motion::MOVE_DIST = 1605;
 const float Motion::MOVE_SPEED = 50.0f;
-const float Motion::SCAN_SPEED = 50.0f;
+const float Motion::RUN_SPEED = 70.0f;
 const float Motion::ROTATE_SPEED = 80.0f;
 const float Motion::ACCEL_CONST = 3.5f; //2.212f
 
@@ -52,7 +52,8 @@
                 
             avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
         
-            accel(MOVE_SPEED);
+            if (speedRun == true) accel(RUN_SPEED);
+            else accel(MOVE_SPEED);
             control();
             
             if ((distanceC) < 40.0f) {
@@ -122,7 +123,7 @@
                 
             avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
         
-            accel(MOVE_SPEED);
+            accel(RUN_SPEED);
             control();
             
             if (distanceC < 101.0f && lastMove == false) {
@@ -173,7 +174,7 @@
             }
             
             
-            accel(SCAN_SPEED);
+            accel(MOVE_SPEED);
             control();
             
             if ((distanceC) < 40.0f) {
@@ -269,8 +270,8 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(17.0f);
-        controller.setDesiredSpeedRight(-83.0f);  
+        controller.setDesiredSpeedLeft(24.15f);
+        controller.setDesiredSpeedRight(-115.85f);  
         
         while ((countsL - countsLOld)  < 440 || (countsR - countsROld) > -2148) {
             
@@ -291,8 +292,8 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(83.0f);
-        controller.setDesiredSpeedRight(-17.0f);
+        controller.setDesiredSpeedLeft(115.85f);
+        controller.setDesiredSpeedRight(-24.15f);
         
         while ((countsL - countsLOld)  < 2148 || (countsR - countsROld) > -440) {
             
@@ -432,6 +433,9 @@
 
 void Motion::runTask(int path[],int task, bool reverse, int junction) {
       
+        if (reverse == true) speedRun = 1;
+        else speedRun = 0;
+      
         switch(path[task]) {
             
             case 1:
@@ -538,10 +542,14 @@
 
 void Motion::accel(float targetSpeed) { 
 
+        float fastSpeed;
+        
+        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) {
+        if (avgSpeed < targetSpeed && deceleration == false && acceleration == false) {
                 
             actSpeed = ACCEL_CONST * t.read()*60.0f; 
             
@@ -549,13 +557,13 @@
             
             actSpeed =  55.0f;
             
-        }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) {
+        }else if (avgSpeed < fastSpeed && acceleration == true) {
             
             actSpeed = ACCEL_CONST * t.read()*60.0f; 
                
         }else if (avgSpeed > targetSpeed+5.0f && deceleration == true) {
         
-            actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f;
+            actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f;
             
         }else if (avgSpeed < targetSpeed && deceleration == true) {