LOCOMOTION.cpp

00001 #include "LOCOMOTION.h"
00002 
00003 LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4):
00004     _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
00005 {
00006     s=0;
00007     setMotors(0);
00008     _m1dir=0;
00009     _m2dir=0;
00010     _en=1;
00011     _led1=1;
00012     wait(0.01);
00013     _led1=0;
00014 }
00015 
00016 inline void LOCOMOTION::setMotors(float x)
00017 {
00018     _m1f=x;
00019     _m1b=x;
00020     _m2f=x;
00021     _m2b=x;
00022 }
00023 
00024 
00025 inline int LOCOMOTION::wrap(int num)
00026 {
00027     return num%360;
00028 }
00029 
00030 void LOCOMOTION::eStop(void)
00031 {
00032     //Stop Motors
00033     setMotors(0);
00034     //Disable Motor Drivers
00035     _en=0;
00036 }
00037 
00038 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
00039 {
00040     if(abs(current-target)<=error)
00041         s=SPEED_X_MIN;
00042     else
00043         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
00044     if(current>target+error) {
00045         if(angle==0) {
00046             _m1dir=1;
00047             _m2dir=1;
00048         } else {
00049             _m1dir=0;
00050             _m2dir=0;
00051         }
00052         setMotors(s);
00053     } else if(current<target-error) {
00054         if(angle==0) {
00055             _m1dir=0;
00056             _m2dir=0;
00057         } else {
00058             _m1dir=1;
00059             _m2dir=1;
00060         }
00061         setMotors(s);
00062     } else {
00063         setMotors(0);
00064         return true;
00065     }
00066     return false;
00067 }
00068 
00069 bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
00070 {
00071     if(abs(current-target)<=error)
00072         s=Y_BIAS_MIN;
00073     else
00074         s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN;
00075     if(current>target+error) {
00076         //_m1dir=1;
00077         //_m2dir=1;
00078         if(angle==0) {
00079             _m1f=_m1f*(1+s);
00080             _m1b=_m1b*(1+s);
00081         } else {
00082             _m2f=_m2f*(1+s);
00083             _m2b=_m2b*(1+s);
00084         }
00085     } else if(current<target-error) {
00086         //_m1dir=0;
00087         //_m2dir=0;
00088         if(angle==0) {
00089             _m2f=_m2f*(1+s);
00090             _m2b=_m2b*(1+s);
00091         } else {
00092             _m1f=_m1f*(1+s);
00093             _m1b=_m1b*(1+s);
00094         }
00095     } else {
00096         s=0;
00097         return true;
00098     }
00099     return false;
00100 }
00101 
00102 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
00103 {
00104     s = 0;
00105     int diff = 0;
00106     diff = 180-wrap(target);
00107     if(abs(wrap(current+diff)-180)<=error)
00108         s=SPEED_TURN_MIN;
00109     else
00110         s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
00111     switch(mode) {
00112         default:
00113         case ANGLE_TURN:
00114             if(wrap(current+diff)>180+error) {
00115                 _m1dir=1;
00116                 _m2dir=0;
00117                 setMotors(s);
00118             } else if(wrap(current+diff)<180-error) {
00119                 _m1dir=0;
00120                 _m2dir=1;
00121                 setMotors(s);
00122             } else {
00123                 setMotors(0);
00124                 return true;
00125             }
00126             break;
00127     }
00128     return false;
00129 }
00130 
00131 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
00132 {
00133     if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
00134         if(angleGood && xGood) {
00135             *xCurrState=X_BACKWARDS;
00136             wait(0.5);
00137 
00138         }
00139     }
00140 
00141     else if(xGood && angleGood && *xCurrState==X_BACKWARDS) {
00142         if(*angleGoal==0) {
00143             *xCurrState=ROTATE_1;
00144             wait(0.5);
00145         } else {
00146             *xCurrState=ROTATE_2;
00147             wait(0.5);
00148         }
00149     }
00150 
00151     else if (xGood && angleGood) {
00152         if (*xCurrState==ROTATE_1) {
00153             *xCurrState=X_INCREASE;
00154             wait(0.5);
00155         } else {
00156             *xCurrState=X_DECREASE;
00157             wait(0.5);
00158         }
00159     }
00160 
00161     switch(*xCurrState) {
00162         case X_INCREASE:
00163             *angleGoal=180;
00164             *xTarget=X_FAR_GOAL;
00165             _m1dir=1;
00166             _m2dir=1;
00167             break;
00168 
00169         case X_DECREASE:
00170             *angleGoal=0;
00171             *xTarget=X_NEAR_GOAL;
00172             //_m1dir=1;
00173             //_m2dir=1;
00174             break;
00175 
00176         case X_BACKWARDS:
00177             if (*angleGoal==0) {
00178                 *xTarget=X_NEAR_GOAL+BACKOFF;
00179             } else {
00180                 *xTarget=X_FAR_GOAL-BACKOFF;
00181             }
00182             //_m1dir=0;
00183             //_m2dir=0;
00184             break;
00185 
00186         case ROTATE_1:
00187             *xTarget=*xTarget;
00188             *angleTol=2;
00189             *angleGoal=180;
00190             break;
00191 
00192         case ROTATE_2:
00193             *xTarget=*xTarget;
00194             *angleTol=2;
00195             *angleGoal=0;
00196             break;
00197 
00198     }
00199 }
00200 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
00201 {
00202     if (xGood||yGood) {
00203         *angleTol=2;
00204     } else {
00205         *angleTol=10;
00206     }
00207 }
00208 
00209 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
00210 {
00211     if (yGood) {
00212         *yTarget+=Y_INCREMENT;
00213     }
00214 }
00215 
00216 void LOCOMOTION::check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr)
00217 {
00218     *xGood=abs(xya.x-xGoal)<xErr;
00219     *yGood=abs(xya.y-yGoal)<yErr;
00220     *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
00221 }
00222 
00223 void LOCOMOTION::mStop(void)
00224 {
00225     setMotors(0);
00226 }