Diff: Motion.cpp

Revision:

17:8a8758bfe3c5

Parent:

16:c5b864804632

Child:

18:3309329d5f42

--- a/Motion.cpp	Mon May 07 14:03:36 2018 +0000
+++ b/Motion.cpp	Mon May 07 18:28:34 2018 +0000
@@ -12,7 +12,7 @@
 const float Motion::MOVE_SPEED = 50.0f;
 const float Motion::SCAN_SPEED = 50.0f;
 const float Motion::ROTATE_SPEED = 80.0f;
-const float Motion::ACCEL_CONST = 2.212f; //
+const float Motion::ACCEL_CONST = 2.5f; //2.212f
 
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
@@ -138,6 +138,7 @@
 void Motion::scanMove() {
         
         acceleration = false;
+        deceleration = false;
         
         countsLOld = counterLeft.read();
         countsROld = counterRight.read();
@@ -420,33 +421,33 @@
         switch(path[task]) {
             
             case 1:
-                if (reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) {
+                if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) {
+                    acceleration = true;
+                    deceleration = false;
+                }else if (reverse == false && 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] && path[task-1] != path[task]) {
-                    acceleration = true;
+                    acceleration = false; 
                     deceleration = false;
-                }else{
-                    acceleration = false;  
+                    avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
                 }
             
-                if (reverse == true && path[task-1] != path[task]) {
+                if (reverse == true && path[task-1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
+                    deceleration = true;
+                    acceleration = false;
+                }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
                     deceleration = true;
                     acceleration = false;
                 }else{
                     deceleration = false;
                 }
                 
-                if (reverse == false && path[task+1] != path[task]) {
-                    deceleration = true;
-                    acceleration = false;
-                }else{
-                    deceleration = false;
-                }
+                //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration);
+                //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed);
+                
+                
+                
                 move();
                 break;
             case 2:
@@ -477,7 +478,7 @@
         avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
 
-        if ( avgSpeed < targetSpeed) {
+        if ( avgSpeed < targetSpeed && deceleration == false) {
                 
             actSpeed = ACCEL_CONST * t.read()*60.0f;