Diff: Motion.cpp

Revision:

12:75d0291a9785

Parent:

11:2960fc540616

Child:

13:845e49f20426

--- a/Motion.cpp	Mon Apr 30 18:27:23 2018 +0000
+++ b/Motion.cpp	Wed May 02 14:58:48 2018 +0000
@@ -9,7 +9,9 @@
 const float Motion::KP = 2.5;
 const float Motion::KD = 0.0f;
 const int Motion::MOVE_DIST = 1605;
-const float Motion::MOVE_SPEED = 60.0f;
+const float Motion::MOVE_SPEED = 100.0f;
+const float Motion::ROTATE_SPEED = 20.0f;
+const float Motion::ACCEL_SCALE = 0.5f;
 
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
@@ -44,10 +46,17 @@
             countsL = counterLeft.read();
             countsR = counterRight.read();
             distanceC = irSensorC.readC();
+            distanceL = irSensorL.readL();
+            distanceR = irSensorR.readR();
             
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
             
-                control();
+            control();
+            /*
+            accel();
+            controller.setDesiredSpeedLeft(actSpeed);
+            controller.setDesiredSpeedRight(-actSpeed);
+            */
             
             if ((distanceC) < 40.0f) {
                 countsLOld = countsL;
@@ -58,10 +67,19 @@
                 }
                 stop();
                 break;    
-            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 150.0f) && (distanceC > 40.0f)) {
+            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 130.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
-            }
+            }/* else if ( distanceL < 80.0f || distanceR < 80.0f ) {
+                countsLOld = countsL;
+                countsROld = countsR;
+                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) {
+                    countsL = counterLeft.read();
+                    countsR = counterRight.read(); 
+                }
+                stop();
+                break;
+            }*/
             
         }    
 }
@@ -120,6 +138,8 @@
             countsL = counterLeft.read();
             countsR = counterRight.read();
             distanceC = irSensorC.readC();
+            distanceL = irSensorL.readL();
+            distanceR = irSensorR.readR();
             
             if (enableMotorDriver == 0) {
                 enableMotorDriver = 1;
@@ -143,7 +163,16 @@
             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
-            }
+            }/*else if ( distanceL > 80.0f || distanceR > 80.0f ) {
+                countsLOld = countsL;
+                countsROld = countsR;
+                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 330.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 330.0f) {
+                    countsL = counterLeft.read();
+                    countsR = counterRight.read(); 
+                }
+                stop();
+                break;
+            }*/
         }  
 }
 /**
@@ -158,15 +187,18 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(-20.0f);
-        controller.setDesiredSpeedRight(-20.0f);
+        controller.setDesiredSpeedLeft(-ROTATE_SPEED);
+        controller.setDesiredSpeedRight(-ROTATE_SPEED);
         
         while ((countsL - countsLOld)  > -862 || (countsR - countsROld) > -862) {
             
+            //accel();
+            
             countsL = counterLeft.read();
             countsR = counterRight.read();
             
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+            
         }    
         
         stop();
@@ -184,15 +216,18 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(20.0f);
-        controller.setDesiredSpeedRight(20.0f);
+        controller.setDesiredSpeedLeft(ROTATE_SPEED);
+        controller.setDesiredSpeedRight(ROTATE_SPEED);
         
         while ((countsL - countsLOld)  < 862 || (countsR - countsROld) < 862) {
             
+            //accel();
+            
             countsL = counterLeft.read();
             countsR = counterRight.read();
         
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+            
         }    
         
         stop();
@@ -269,11 +304,13 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        controller.setDesiredSpeedLeft(-50.0f);
-        controller.setDesiredSpeedRight(-50.0f);
+        controller.setDesiredSpeedLeft(-ROTATE_SPEED);
+        controller.setDesiredSpeedRight(-ROTATE_SPEED);
         
         while ((countsL - countsLOld)  > -1723 || (countsR - countsROld) > -1723) {
             
+            //accel();
+            
             countsL = counterLeft.read();
             countsR = counterRight.read();
         
@@ -286,80 +323,111 @@
 
 void Motion::control() {
         
-    float wallLeft = 48.73f;
-    float wallRight = 51.03f;
-    
-    distanceL = irSensorL.readL();
-    distanceR = irSensorR.readR();
-    
-    
-    if (distanceL < wallLeft && distanceR < wallRight) {
+        float wallLeft = 48.73f;
+        float wallRight = 51.03f;
         
-        errorP = distanceL - distanceR + 2.30f;
-
-    }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
-            
-        errorP = distanceL - LEFT_MID_VAL;
-               
-    }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
-            
-        errorP = RIGHT_MID_VAL - distanceR;
+        distanceL = irSensorL.readL();
+        distanceR = irSensorR.readR();
+        
         
-    }else{
-        
-        errorP = 0; 
-        errorD = 0; 
-    }
-    
-    errorD = errorP - oldErrorP;  
+        if (distanceL < wallLeft && distanceR < wallRight) {
+            
+            errorP = distanceL - distanceR + 2.30f;
     
-    oldErrorP = errorP;
-    
-    if (abs(errorP) < 80.0f) {
-        totalError = KP*errorP + KD*errorD;
-    }else{
-        totalError = 0;
-    }
-    
-    controller.setDesiredSpeedLeft(speedLeft - totalError);
-    controller.setDesiredSpeedRight(speedRight - totalError);
+        }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+                
+            errorP = distanceL - LEFT_MID_VAL;
+                   
+        }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
+                
+            errorP = RIGHT_MID_VAL - distanceR;
+            
+        }else{
+            
+            errorP = 0; 
+            errorD = 0; 
+        }
+        
+        errorD = errorP - oldErrorP;  
+        
+        oldErrorP = errorP;
+        
+        if (abs(errorP) < 80.0f) {
+            totalError = KP*errorP + KD*errorD;
+        }else{
+            totalError = 0;
+        }
+        
+        controller.setDesiredSpeedLeft(speedLeft - totalError);
+        controller.setDesiredSpeedRight(speedRight - totalError);
 }
 
-void Motion::runTask(int task) {
+void Motion::runTask(int path[],int task, bool reverse) {
       
-    switch(task) {
-        
-        case 1:
-            move();
-            break;
-        case 2:
-            rotateL();
-            break;
-        case 3:
-            rotateR();
-            break;
-        case 4:
-            moveFast();
-            break;  
-        case 5:
-            stop();
-            break;  
-    }
+        switch(path[task]) {
+            
+            case 1:
+                if (reverse == true && path[task+1] == path[task]) {
+                    acceleration = true;
+                }else{
+                    acceleration = false;
+                }
+                
+                if (reverse == false && path[task-1] == path[task]) {
+                    acceleration = true;   
+                }else{
+                    acceleration = false;  
+                }
+            
+                if (reverse == true && path[task-1] != path[task]) {
+                    deceleration = true;
+                }else{
+                    deceleration = false;
+                }
+                
+                if (reverse == false && path[task+1] != path[task]) {
+                    deceleration = true;
+                }else{
+                    deceleration = false;
+                }
+                move();
+                break;
+            case 2:
+                rotateL();
+                break;
+            case 3:
+                rotateR();
+                break;
+            case 4:
+                moveFast();
+                break;  
+            case 5:
+                stop();
+                break;  
+        }
 }
 
 int Motion::finish() {
-    
-    return line;   
+        
+        return line;   
 }
 
-/*
-    if ( controller.getSpeedLeft() < MOVE_SPEED ) {
+
+void Motion::accel() { 
+
+        avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
+        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
+
+        if ( avgSpeed < MOVE_SPEED && acceleration == true) {
+                
+            actSpeed = avgCounts * ACCEL_SCALE;   
+        }
+            
+        if ( MOVE_DIST - avgCounts < MOVE_SPEED*2 && deceleration == true) {
         
-        speed = counterLeft.read()/2;   
-    }
-    
-    if ( MOVE_DIST - counterLeft.read() < MOVE_SPEED*2 ) {
-    
-        speed = -(MOVE_DIST - MOVE_SPEED*2) + MOVE_SPEED;
-    }
-*/
+            actSpeed = -ACCEL_SCALE * (avgCounts - (MOVE_DIST - MOVE_SPEED*2)) + MOVE_SPEED;
+        }
+            
+            
+}
+