Motion.cpp

00001 
00002 #include <cmath>
00003 #include "Motion.h"
00004 
00005 using namespace std;
00006 
00007 const float Motion::LEFT_MID_VAL = 40.73f; //44.73
00008 const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
00009 const float Motion::KP = 2.0;
00010 const float Motion::KD = 0.0f;
00011 const int Motion::MOVE_DIST = 1620;
00012 const float Motion::MOVE_SPEED = 50.0f;
00013 const float Motion::RUN_SPEED = 70.0f;
00014 const float Motion::ROTATE_SPEED = 80.0f;
00015 const float Motion::ACCEL_CONST = 4.0f; //2.212f
00016 
00017 
00018 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
00019                 EncoderCounter& counterRight, IRSensor& irSensorL,
00020                 IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
00021                 DigitalOut& enableMotorDriver) :
00022                 controller(controller), counterLeft(counterLeft),
00023                 counterRight(counterRight), irSensorL(irSensorL),
00024                 irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
00025                 enableMotorDriver(enableMotorDriver) {}
00026                
00027 Motion::~Motion() {}
00028 
00029 /**
00030  * Carry out move for one field
00031  */
00032 void Motion::move() {
00033         
00034         countsLOld = counterLeft.read();
00035         countsROld = counterRight.read();
00036         countsL = counterLeft.read();
00037         countsR = counterRight.read();
00038         
00039         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00040             
00041         t.start();
00042         
00043         while ((countsL - countsLOld)  < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
00044             
00045             countsL = counterLeft.read();
00046             countsR = counterRight.read();
00047             distanceC = irSensorC.readC();
00048             distanceL = irSensorL.readL();
00049             distanceR = irSensorR.readR();
00050             
00051             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00052                 
00053             avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00054         
00055             if (speedRun == true) accel(RUN_SPEED);
00056             else accel(MOVE_SPEED);
00057             control();
00058             
00059             //Stop at certainn distance before wall
00060             if ((distanceC) < 40.0f && speedRun == false) {
00061                 countsLOld = countsL;
00062                 countsROld = countsR;
00063                 
00064                 while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
00065                     countsL = counterLeft.read();
00066                     countsR = counterRight.read();
00067                     distanceC = irSensorC.readC(); 
00068                     
00069                     if (speedRun == true) accel(RUN_SPEED);
00070                     else accel(MOVE_SPEED);
00071                     control();
00072                     
00073                     if (distanceC > 40.0f) {
00074                         stop();
00075                         break;
00076                     }
00077                 }
00078                 stop();
00079                 break;    
00080             
00081             //Stop at certain distance in speed run
00082           /*  }else if (distanceC < 140.0f && speedRun == true && lastMove == true) { //Ursprünglich: 180.0f 
00083                 stop();
00084                 break;   
00085             */
00086             //Correct distance from wall
00087             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
00088                 countsLOld += 1000;
00089                 countsROld -= 1000; 
00090                   
00091             //Stop after certain distance if side wall and front wall missing
00092             }else if ( deceleration == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 180.0f ) { //Urspünglich 200.0f
00093 
00094                 controller.counterReset();
00095                 countsLOld = counterLeft.read();
00096                 countsROld = counterRight.read();
00097                 countsL = counterLeft.read();
00098                 countsR = counterRight.read();
00099                 
00100                 while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 400.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 400.0f) {
00101                     countsL = counterLeft.read();
00102                     countsR = counterRight.read(); 
00103                     
00104                     deceleration = true;
00105                     acceleration = false;
00106                     
00107                     accel(MOVE_SPEED);
00108                     control();
00109                 }
00110                 stop();
00111                 break;
00112             }
00113             
00114         }    
00115         
00116         t.stop();
00117         t.reset();
00118         lastMove = false;
00119         acceleration = false;
00120         deceleration = false;
00121 }
00122 
00123 /**
00124  * Carry out move for a half field
00125  */
00126 void Motion::moveHalf() {
00127         
00128         countsLOld = counterLeft.read();
00129         countsROld = counterRight.read();
00130         countsL = counterLeft.read();
00131         countsR = counterRight.read();
00132         
00133         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00134             
00135         t.start();
00136         
00137         while ((countsL - countsLOld)  < 824 || (countsR - countsROld) > -824) {
00138             
00139             countsL = counterLeft.read();
00140             countsR = counterRight.read();
00141             distanceC = irSensorC.readC();
00142             distanceL = irSensorL.readL();
00143             distanceR = irSensorR.readR();
00144             
00145             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00146                 
00147             avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00148         
00149             accel(RUN_SPEED);
00150             control();
00151             
00152             //Stop ceratin distance before wall
00153             if (distanceC < 100.0f && lastMove == false) {
00154                 break;    
00155             
00156             }else if ( ((countsL - countsLOld)  >= 824 || (countsR - countsROld) <= -824) && (distanceC >= 100.0f) && (distanceC < 120.0f) )  {
00157                 countsLOld += 100;
00158                 countsROld -= 100; 
00159                        
00160             //Stop after certain distance if side wall missing
00161             }else if ( (distanceL > 80.0f || distanceR > 80.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) {
00162 
00163                 controller.counterReset();
00164                 countsLOld = counterLeft.read();
00165                 countsROld = counterRight.read();
00166                 countsL = counterLeft.read();
00167                 countsR = counterRight.read();
00168                 
00169                 while ((countsL - countsLOld)  <  280.0f || (countsR - countsROld) > -280.0f) { //Ursprünglich 250.0f
00170                     countsL = counterLeft.read();
00171                     countsR = counterRight.read(); 
00172                     accel(RUN_SPEED); 
00173                     control();
00174                 }
00175                 stop();
00176                 break; 
00177                 
00178             //Stop before wall at target field
00179             }else if (distanceC < 40.0f && lastMove == true) {
00180                 stop();
00181                 break;
00182             }
00183             
00184         }    
00185         
00186         t.stop();
00187         t.reset();
00188         lastMove = false;
00189         acceleration = false;
00190         deceleration = false;
00191 }
00192 
00193 /**
00194  * Carry out move for one field with finish line detection
00195  */
00196 void Motion::scanMove() {
00197         
00198         acceleration = false;
00199         deceleration = false;
00200         longMove = false;
00201         
00202         countsLOld = counterLeft.read();
00203         countsROld = counterRight.read();
00204         countsL = counterLeft.read();
00205         countsR = counterRight.read();
00206         
00207         bool sideWalls = false;
00208         
00209         t.start();
00210         
00211         while ((countsL - countsLOld)  <  MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
00212             
00213             countsL = counterLeft.read();
00214             countsR = counterRight.read();
00215             distanceC = irSensorC.readC();
00216             distanceL = irSensorL.readL();
00217             distanceR = irSensorR.readR();
00218             
00219             //for distance correcture with side wall
00220             if (distanceL < 80.0f && distanceR < 80.0f) sideWalls = true;
00221             
00222             if (enableMotorDriver == 0) {
00223                 enableMotorDriver = 1;
00224             }
00225             
00226             if (lineSensor.read() > 0.85f) {
00227                 line = 1;  
00228             }
00229             
00230             
00231             accel(MOVE_SPEED);
00232             control();
00233             
00234             if ((distanceC) < 40.0f) {
00235                 countsLOld = countsL;
00236                 countsROld = countsR;
00237                 while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
00238                     countsL = counterLeft.read();
00239                     countsR = counterRight.read(); 
00240                     if (distanceC > 40.0f) {
00241                         stop();
00242                         break;
00243                     }
00244                 }
00245                 
00246                 stop();
00247                 break;   
00248                  
00249             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
00250                 countsLOld += 500;
00251                 countsROld += 500;   
00252             
00253             //Stop after certain distance if side wall and front wall missing
00254             }else if (sideWalls == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 200.0f ) {
00255                 countsLOld = counterLeft.read();
00256                 countsROld = counterRight.read();
00257                 
00258                 while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 320.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 320.0f) {
00259                     countsL = counterLeft.read();
00260                     countsR = counterRight.read(); 
00261                     
00262                     if (speedRun == true) accel(RUN_SPEED);
00263                     else accel(MOVE_SPEED);
00264                     control();
00265                 }
00266                 break;
00267             }
00268         } 
00269          
00270         t.stop();
00271         t.reset();
00272         lastMove = false;
00273 }
00274 /**
00275  * 90° Rotation left
00276  */
00277 void Motion::rotateL() {
00278     
00279         stop();
00280         controller.counterReset();
00281         countsLOld = counterLeft.read();
00282         countsROld = counterRight.read();
00283         countsL = counterLeft.read();
00284         countsR = counterRight.read();
00285         
00286         controller.setDesiredSpeedLeft(-ROTATE_SPEED);
00287         controller.setDesiredSpeedRight(-ROTATE_SPEED);
00288         
00289         while ((countsL - countsLOld)  > -870 || (countsR - countsROld) > -870) {
00290             
00291             //accel();
00292             
00293             countsL = counterLeft.read();
00294             countsR = counterRight.read();
00295             
00296             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00297             
00298         }    
00299         
00300         stop();
00301 }
00302     
00303 /**
00304  * 90° Rotation right
00305  */
00306 void Motion::rotateR() {
00307     
00308         stop();
00309         controller.counterReset();
00310         countsLOld = counterLeft.read();
00311         countsROld = counterRight.read();
00312         countsL = counterLeft.read();
00313         countsR = counterRight.read();
00314         
00315         controller.setDesiredSpeedLeft(ROTATE_SPEED);
00316         controller.setDesiredSpeedRight(ROTATE_SPEED);
00317         
00318         while ((countsL - countsLOld)  < 870 || (countsR - countsROld) < 870) {
00319             
00320             //accel();
00321             
00322             countsL = counterLeft.read();
00323             countsR = counterRight.read();
00324         
00325             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00326             
00327         }    
00328         
00329         stop();
00330 }
00331 /**
00332  * Turn left
00333  */
00334 void Motion::turnL() {     
00335         
00336         controller.counterReset();
00337         countsLOld = counterLeft.read();
00338         countsROld = counterRight.read();
00339         countsL = counterLeft.read();
00340         countsR = counterRight.read();
00341         
00342         controller.setDesiredSpeedLeft(24.15f);
00343         controller.setDesiredSpeedRight(-115.85f);  
00344         
00345         /*  Velocity Settings:
00346             50rpm   ->  17.25   :   82.75
00347             60rpm   ->  20.7    :   99.3
00348             70rpm   ->  24.15   :   115.85
00349             80rpm   ->  27.6    :   132.4
00350             90rpm   ->  31.05   :   148.95
00351             100rpm  ->  34.5    :   165.5
00352         */
00353         
00354         while ((countsL - countsLOld)  < 446 || (countsR - countsROld) > -2141) {
00355             
00356             countsL = counterLeft.read();
00357             countsR = counterRight.read();
00358         
00359             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00360         }         
00361 }
00362 /**
00363  * Turn right
00364  */
00365 void Motion::turnR() {    
00366         
00367         controller.counterReset();
00368         countsLOld = counterLeft.read();
00369         countsROld = counterRight.read();
00370         countsL = counterLeft.read();
00371         countsR = counterRight.read();
00372         
00373         controller.setDesiredSpeedLeft(115.85f);                                            
00374         controller.setDesiredSpeedRight(-24.15f);
00375         
00376         while ((countsL - countsLOld)  < 2141 || (countsR - countsROld) > -446) {
00377             
00378             countsL = counterLeft.read();
00379             countsR = counterRight.read();
00380         
00381             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00382         }        
00383 }
00384 /**
00385  * Motor Stop
00386  */
00387 void Motion::stop() {
00388         
00389         controller.setDesiredSpeedLeft(0.0f);
00390         controller.setDesiredSpeedRight(0.0f);
00391         actSpeed = 0.0f;
00392         
00393         float sL = controller.getSpeedLeft();
00394         float ticks = 0.08f*sL;
00395         
00396         waitStop = 0;
00397         while( waitStop < ticks) {
00398             controller.setDesiredSpeedLeft(0.0f);
00399             controller.setDesiredSpeedRight(0.0f);
00400             waitStop+= 1;
00401         }
00402 }
00403 /**
00404  * 180° Rotation
00405  */
00406 void Motion::rotate180() {
00407         //1723
00408         stop();
00409         controller.counterReset();
00410         countsLOld = counterLeft.read();
00411         countsROld = counterRight.read();
00412         countsL = counterLeft.read();
00413         countsR = counterRight.read();
00414         
00415         t.start();
00416         
00417         while ((countsL - countsLOld)  > -900 || (countsR - countsROld) > -900) {
00418             
00419             actSpeed = 3.5f * t.read()*60.0f; 
00420             
00421             controller.setDesiredSpeedLeft(-actSpeed);
00422             controller.setDesiredSpeedRight(-actSpeed);
00423             
00424             countsL = counterLeft.read();
00425             countsR = counterRight.read();
00426         
00427             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00428         }    
00429         
00430         t.reset();
00431 
00432         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00433         
00434         while ((countsL - countsLOld)  > -1720 || (countsR - countsROld) > -1720) {
00435             
00436             actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
00437             
00438             controller.setDesiredSpeedLeft(-actSpeed);
00439             controller.setDesiredSpeedRight(-actSpeed);
00440             
00441             countsL = counterLeft.read();
00442             countsR = counterRight.read();
00443         
00444             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
00445         }            
00446         t.stop();
00447         t.reset();
00448         stop();
00449 }
00450 
00451 
00452 void Motion::control() {
00453         
00454         float wallLeft = 47.0f; //48.73
00455         float wallRight = 44.0f; //51.03f;
00456         
00457         distanceL = irSensorL.readL();
00458         distanceR = irSensorR.readR();
00459         
00460         if (distanceL < wallLeft && distanceR > wallRight) {
00461                 
00462             errorP = distanceL - wallLeft;
00463             
00464         }else if (distanceL > wallLeft && distanceR < wallRight) {
00465                 
00466             errorP = wallRight - distanceR;
00467             
00468         }else if (distanceL < wallLeft+10.0f && distanceL > wallLeft && distanceR > wallRight) {
00469                 
00470             errorP = distanceL - wallLeft;
00471             
00472         }else if (distanceR < wallRight+10.0f && distanceL > wallLeft && distanceR > wallRight) {
00473                 
00474             errorP = wallRight - distanceR;
00475             
00476         }else{
00477             
00478             errorP = 0; 
00479             errorD = 0; 
00480         }
00481         
00482         errorD = errorP - oldErrorP;  
00483         
00484         oldErrorP = errorP;
00485         
00486         if (abs(errorP) < 80.0f) {
00487             totalError = KP*errorP + KD*errorD;
00488         }
00489         
00490         controller.setDesiredSpeedLeft(actSpeed - totalError);
00491         controller.setDesiredSpeedRight(-actSpeed - totalError);
00492 }
00493 
00494 void Motion::runTask(int path[],int task, bool reverse, int junction) {
00495         
00496         //reverse happens only in search run
00497         if (reverse == false) speedRun = true;
00498         else speedRun = false;
00499       
00500         switch(path[task]) {
00501             
00502             case 1:
00503             
00504                 //Acceleration
00505                 if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction && task-1 != junction) {     //if next move() and previous no move() and step no junction
00506                     
00507                     acceleration = true;
00508                     longMove = true;
00509                     deceleration = false;
00510                     
00511                 }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0 || path[task-1] != 4) ) {  //same as above, also if start field
00512                     
00513                     acceleration = true;
00514                     longMove = true;
00515                     deceleration = false;
00516                     
00517                 }else{
00518                     acceleration = false; 
00519                     avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00520                 }
00521                 
00522                 //Deceleration
00523                 if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {  //next step no move() or junction and speed above 120rpm
00524                     
00525                     deceleration = true;
00526                     acceleration = false;
00527                     lastMove = true;
00528                     longMove = false;
00529                     
00530                 }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() and no moveHalf() and speed above 120rpm
00531                     
00532                     deceleration = true;
00533                     acceleration = false;
00534                     lastMove = true;
00535                     longMove = false;
00536                     
00537                 }else if (reverse == false && path[task+1] != path[task] && path[task+1] == 4 && avgSpeed > 2.4f*MOVE_SPEED) {
00538                     
00539                     lastMove = true;
00540                     
00541                 }else{
00542                     deceleration = false;
00543                 }
00544                 
00545                 //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration);
00546                 //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed);
00547                 
00548                 move();
00549                 break;
00550             case 2:
00551                 rotateL();
00552                 break;
00553             case 3:
00554                 rotateR();
00555                 break;
00556             case 4:
00557             
00558                 if (reverse == false && path[task] == 4 && path[task+1] == 1) {     //accelerate if next step is move()
00559                     
00560                     acceleration = true;
00561                     longMove = true;
00562                     deceleration = false;
00563                     
00564                 }else{
00565                     acceleration = false; 
00566                     avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00567                 }
00568                 
00569                 if (reverse == false && path[task-1] == 1 && path[task] == 4 && path[task+1] != 0) {    //decelerate if previous step was move()
00570                     
00571                     deceleration = true;
00572                     acceleration = false;
00573                     longMove = false;
00574                     
00575                 }else{
00576                     deceleration = false;
00577                 }
00578                 
00579                 if (reverse == false && path[task+1] == 0) {
00580                     
00581                     lastMove = true;
00582                 }
00583                 
00584                 moveHalf();
00585                 break; 
00586             case 5:
00587                 turnL();
00588                 break;
00589             case 6:
00590                 turnR();
00591                 break;
00592             case 7:
00593                 break;   
00594         }
00595 }
00596 
00597 int Motion::finish() {
00598         
00599         return line;   
00600 }
00601 
00602 
00603 void Motion::accel(float targetSpeed) { 
00604 
00605         float fastSpeed;
00606         
00607         //Acclerated Target Speed
00608         if (speedRun == true) fastSpeed = targetSpeed*2.0f;
00609         else fastSpeed = targetSpeed*2.8f;
00610         
00611         avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
00612         
00613         //Acceleration logic
00614         
00615         //Short distance
00616         if (avgSpeed < targetSpeed && deceleration == false && acceleration == false && longMove == false) {
00617                 
00618             actSpeed = ACCEL_CONST * t.read()*60.0f;                //Acceleration equation
00619             
00620         }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
00621             
00622             actSpeed =  targetSpeed+5.0f;                           //Keep Speed steady in case of overshooting
00623             
00624         //Long distance
00625         }else if (avgSpeed < fastSpeed && acceleration == true && deceleration == false) {
00626             
00627             actSpeed = ACCEL_CONST * t.read()*60.0f;    
00628                
00629         }else if (avgSpeed > targetSpeed+5.0f && acceleration == false && deceleration == true) {
00630         
00631             actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f;    //Deceleration equation
00632             
00633         }else if (avgSpeed < targetSpeed && acceleration == false && deceleration == true) {
00634             
00635             actSpeed = targetSpeed+5.0f;                            //Limit deceleration in case of overshooting
00636             
00637         }    
00638 }
00639