Diff: Motion.cpp

Revision:

15:0ae61d3d199f

Parent:

14:2926932e26df

Child:

16:c5b864804632

--- a/Motion.cpp	Fri May 04 19:56:05 2018 +0000
+++ b/Motion.cpp	Fri May 04 20:36:38 2018 +0000
@@ -9,10 +9,10 @@
 const float Motion::KP = 2.5;
 const float Motion::KD = 0.0f;
 const int Motion::MOVE_DIST = 1605;
-const float Motion::MOVE_SPEED = 130.0f;
+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.212f; //
 
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
@@ -39,8 +39,8 @@
         speedLeft = MOVE_SPEED;
         speedRight = -MOVE_SPEED;
         
-        controller.setDesiredSpeedLeft(actSpeed);
-        controller.setDesiredSpeedRight(-actSpeed);
+        //controller.setDesiredSpeedLeft(actSpeed);
+        //controller.setDesiredSpeedRight(-actSpeed);
         
         t.start();
         
@@ -54,8 +54,8 @@
             
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
             
+            accel(MOVE_SPEED);
             control();
-            accel(MOVE_SPEED);
             /*
             accel(MOVE_SPEED);
             controller.setDesiredSpeedLeft(actSpeed);
@@ -135,9 +135,8 @@
         speedLeft = SCAN_SPEED;
         speedRight = -SCAN_SPEED;
         
-        controller.setDesiredSpeedLeft(actSpeed);
-        controller.setDesiredSpeedRight(-actSpeed);
-        acceleration = true;
+        //controller.setDesiredSpeedLeft(actSpeed);
+        //controller.setDesiredSpeedRight(-actSpeed);
         
         distanceC = irSensorC.readC();
         t.start();
@@ -159,8 +158,8 @@
             }
             
             
+            accel(SCAN_SPEED);
             control();
-            accel(SCAN_SPEED);
             
             //controller.setDesiredSpeedLeft(actSpeed);
             //controller.setDesiredSpeedRight(-actSpeed);
@@ -190,7 +189,6 @@
         }  
         t.stop();
         t.reset();
-        acceleration = false;
 }
 /**
  * 90° Rotation nach Links
@@ -407,26 +405,30 @@
         switch(path[task]) {
             
             case 1:
-                if (reverse == true && path[task+1] == path[task]) {
+                if (reverse == true && path[task-1] == path[task]) {
                     acceleration = true;
+                    deceleration = false;
                 }else{
                     acceleration = false;
                 }
                 
-                if (reverse == false && path[task-1] == path[task]) {
-                    acceleration = true;   
+                if (reverse == false && path[task+1] == path[task]) {
+                    acceleration = true;
+                    deceleration = false;
                 }else{
                     acceleration = false;  
                 }
             
                 if (reverse == true && path[task-1] != path[task]) {
                     deceleration = true;
+                    acceleration = false;
                 }else{
                     deceleration = false;
                 }
                 
                 if (reverse == false && path[task+1] != path[task]) {
                     deceleration = true;
+                    acceleration = false;
                 }else{
                     deceleration = false;
                 }
@@ -460,14 +462,19 @@
         avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
 
-        if ( avgSpeed < targetSpeed && acceleration == true) {
+        if ( avgSpeed < targetSpeed) {
                 
             actSpeed = ACCEL_CONST * t.read()*60.0f;   
         }
+        
+        if ( avgSpeed < targetSpeed*2.6f && acceleration == true) {
             
-        if ( MOVE_DIST - avgCounts < targetSpeed*2 && deceleration == true) {
+            actSpeed = 3.0f * t.read()*60.0f;    
+        }
+            
+        if ( avgSpeed > targetSpeed && deceleration == true) {
         
-            actSpeed = -ACCEL_CONST * (avgCounts - (MOVE_DIST - targetSpeed*2)) + targetSpeed;
+            actSpeed = targetSpeed*2.6f - ACCEL_CONST * t.read()*60.0f;
         }