Diff: Motion.cpp

Revision:

13:845e49f20426

Parent:

12:75d0291a9785

Child:

14:2926932e26df

--- a/Motion.cpp	Wed May 02 14:58:48 2018 +0000
+++ b/Motion.cpp	Fri May 04 19:10:36 2018 +0000
@@ -9,9 +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 = 100.0f;
-const float Motion::ROTATE_SPEED = 20.0f;
-const float Motion::ACCEL_SCALE = 0.5f;
+const float Motion::MOVE_SPEED = 130.0f;
+const float Motion::SCAN_SPEED = 50.0f;
+const float Motion::ROTATE_SPEED = 80.0f;
+const float Motion::ACCEL_CONST = 2.212f;
 
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
@@ -41,6 +42,8 @@
         controller.setDesiredSpeedLeft(MOVE_SPEED);
         controller.setDesiredSpeedRight(-MOVE_SPEED);
         
+        t.start();
+        
         while ((countsL - countsLOld)  < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
             
             countsL = counterLeft.read();
@@ -53,7 +56,7 @@
             
             control();
             /*
-            accel();
+            accel(MOVE_SPEED);
             controller.setDesiredSpeedLeft(actSpeed);
             controller.setDesiredSpeedRight(-actSpeed);
             */
@@ -82,6 +85,9 @@
             }*/
             
         }    
+        
+        t.stop();
+        t.reset();
 }
 /**
  * Eine Feldstrecke mit höherer Geschwindigkeit fahren
@@ -125,13 +131,15 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        speedLeft = MOVE_SPEED;
-        speedRight = -MOVE_SPEED;
+        speedLeft = SCAN_SPEED;
+        speedRight = -SCAN_SPEED;
         
-        controller.setDesiredSpeedLeft(MOVE_SPEED);
-        controller.setDesiredSpeedRight(-MOVE_SPEED);
+        controller.setDesiredSpeedLeft(actSpeed);
+        controller.setDesiredSpeedRight(-actSpeed);
+        acceleration = true;
         
         distanceC = irSensorC.readC();
+        t.start();
         
         while ((countsL - countsLOld)  <  MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
             
@@ -149,7 +157,12 @@
                 line = 1;  
             }
             
-                control();
+            
+            control();
+            accel(SCAN_SPEED);
+            
+            //controller.setDesiredSpeedLeft(actSpeed);
+            //controller.setDesiredSpeedRight(-actSpeed);
             
             if ((distanceC) < 40.0f) {
                 countsLOld = countsL;
@@ -174,6 +187,9 @@
                 break;
             }*/
         }  
+        t.stop();
+        t.reset();
+        acceleration = false;
 }
 /**
  * 90° Rotation nach Links
@@ -190,7 +206,7 @@
         controller.setDesiredSpeedLeft(-ROTATE_SPEED);
         controller.setDesiredSpeedRight(-ROTATE_SPEED);
         
-        while ((countsL - countsLOld)  > -862 || (countsR - countsROld) > -862) {
+        while ((countsL - countsLOld)  > -870 || (countsR - countsROld) > -870) {
             
             //accel();
             
@@ -219,7 +235,7 @@
         controller.setDesiredSpeedLeft(ROTATE_SPEED);
         controller.setDesiredSpeedRight(ROTATE_SPEED);
         
-        while ((countsL - countsLOld)  < 862 || (countsR - countsROld) < 862) {
+        while ((countsL - countsLOld)  < 870 || (countsR - countsROld) < 870) {
             
             //accel();
             
@@ -296,7 +312,7 @@
  * 180° Rotation
  */
 void Motion::rotate180() {
-    
+        //1723
         stop();
         controller.counterReset();
         countsLOld = counterLeft.read();
@@ -304,12 +320,17 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(-ROTATE_SPEED);
-        controller.setDesiredSpeedRight(-ROTATE_SPEED);
+        //controller.setDesiredSpeedLeft(-ROTATE_SPEED);
+        //controller.setDesiredSpeedRight(-ROTATE_SPEED);
+        
+        t.start();
         
-        while ((countsL - countsLOld)  > -1723 || (countsR - countsROld) > -1723) {
+        while ((countsL - countsLOld)  > -900 || (countsR - countsROld) > -900) {
             
-            //accel();
+            actSpeed = 3.5f * t.read()*60.0f; 
+            
+            controller.setDesiredSpeedLeft(-actSpeed);
+            controller.setDesiredSpeedRight(-actSpeed);
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -317,6 +338,24 @@
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
         }    
         
+        t.reset();
+
+        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
+        
+        while ((countsL - countsLOld)  > -1720 || (countsR - countsROld) > -1720) {
+            
+            actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
+            
+            controller.setDesiredSpeedLeft(-actSpeed);
+            controller.setDesiredSpeedRight(-actSpeed);
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+        
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+        }            
+        t.stop();
+        t.reset();
         stop();
 }
 
@@ -358,8 +397,8 @@
             totalError = 0;
         }
         
-        controller.setDesiredSpeedLeft(speedLeft - totalError);
-        controller.setDesiredSpeedRight(speedRight - totalError);
+        controller.setDesiredSpeedLeft(actSpeed - totalError);
+        controller.setDesiredSpeedRight(-actSpeed - totalError);
 }
 
 void Motion::runTask(int path[],int task, bool reverse) {
@@ -413,19 +452,21 @@
 }
 
 
-void Motion::accel() { 
+void Motion::accel(float targetSpeed) { 
+
+        
 
         avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
 
-        if ( avgSpeed < MOVE_SPEED && acceleration == true) {
+        if ( avgSpeed < targetSpeed && acceleration == true) {
                 
-            actSpeed = avgCounts * ACCEL_SCALE;   
+            actSpeed = ACCEL_CONST * t.read()*60.0f;   
         }
             
-        if ( MOVE_DIST - avgCounts < MOVE_SPEED*2 && deceleration == true) {
+        if ( MOVE_DIST - avgCounts < targetSpeed*2 && deceleration == true) {
         
-            actSpeed = -ACCEL_SCALE * (avgCounts - (MOVE_DIST - MOVE_SPEED*2)) + MOVE_SPEED;
+            actSpeed = -ACCEL_CONST * (avgCounts - (MOVE_DIST - targetSpeed*2)) + targetSpeed;
         }