Diff: Motion.cpp

Revision:

16:c5b864804632

Parent:

15:0ae61d3d199f

Child:

17:8a8758bfe3c5

--- a/Motion.cpp	Fri May 04 20:36:38 2018 +0000
+++ b/Motion.cpp	Mon May 07 14:03:36 2018 +0000
@@ -42,6 +42,13 @@
         //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();
         
         while ((countsL - countsLOld)  < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
@@ -53,7 +60,10 @@
             distanceR = irSensorR.readR();
             
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
-            
+                
+            avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
+        
+           // if (partytime == 1) 
             accel(MOVE_SPEED);
             control();
             /*
@@ -127,6 +137,8 @@
  */
 void Motion::scanMove() {
         
+        acceleration = false;
+        
         countsLOld = counterLeft.read();
         countsROld = counterRight.read();
         countsL = counterLeft.read();
@@ -298,12 +310,15 @@
         
         controller.setDesiredSpeedLeft(0.0f);
         controller.setDesiredSpeedRight(0.0f);
+        actSpeed = 0.0f;
         
         float sL = controller.getSpeedLeft();
         float ticks = 0.08f*sL;
         
         waitStop = 0;
         while( waitStop < ticks) {
+            controller.setDesiredSpeedLeft(0.0f);
+            controller.setDesiredSpeedRight(0.0f);
             waitStop+= 1;
         }
 }
@@ -405,14 +420,14 @@
         switch(path[task]) {
             
             case 1:
-                if (reverse == true && path[task-1] == path[task]) {
+                if (reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) {
                     acceleration = true;
                     deceleration = false;
                 }else{
                     acceleration = false;
                 }
                 
-                if (reverse == false && path[task+1] == path[task]) {
+                if (reverse == false && path[task+1] == path[task] && path[task-1] != path[task]) {
                     acceleration = true;
                     deceleration = false;
                 }else{
@@ -464,15 +479,13 @@
 
         if ( avgSpeed < targetSpeed) {
                 
-            actSpeed = ACCEL_CONST * t.read()*60.0f;   
-        }
-        
-        if ( avgSpeed < targetSpeed*2.6f && acceleration == true) {
+            actSpeed = ACCEL_CONST * t.read()*60.0f; 
+              
+        }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) {
             
-            actSpeed = 3.0f * t.read()*60.0f;    
-        }
-            
-        if ( avgSpeed > targetSpeed && deceleration == true) {
+            actSpeed = 2.5f * t.read()*60.0f; 
+               
+        }else if ( avgSpeed > targetSpeed && deceleration == true) {
         
             actSpeed = targetSpeed*2.6f - ACCEL_CONST * t.read()*60.0f;
         }