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 inline void LOCOMOTION::setMotors12(float x,float y)
00025 {
00026     _m1f=x;
00027     _m1b=x;
00028     _m2f=y;
00029     _m2b=y;
00030 }
00031 
00032 inline float LOCOMOTION::compG(float speed, bool dir, int angle)
00033 {
00034     if(dir)
00035         return (sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
00036     else
00037         return -1*(sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
00038 }
00039 
00040 inline int LOCOMOTION::wrap(int num)
00041 {
00042     return num%360;
00043 }
00044 
00045 void LOCOMOTION::eStop(void)
00046 {
00047     //Stop Motors
00048     setMotors(0);
00049     //Disable Motor Drivers
00050     _en=0;
00051 }
00052 
00053 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
00054 {
00055     /*if(abs(current-target)<=error)
00056         s=SPEED_X_MIN;
00057     else
00058         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;*/
00059     if(abs(current-target)<30)
00060         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
00061     else
00062         s=SPEED_X_MAX;
00063     if(current>target+error) {
00064         if(angle==0) {
00065             _m1dir=1;
00066             _m2dir=1;
00067         } else {
00068             _m1dir=0;
00069             _m2dir=0;
00070         }
00071         setMotors12(s,s);
00072     } else if(current<target-error) {
00073         if(angle==0) {
00074             _m1dir=0;
00075             _m2dir=0;
00076         } else {
00077             _m1dir=1;
00078             _m2dir=1;
00079         }
00080         setMotors12(s,s);
00081     } else {
00082         setMotors(0);
00083         return true;
00084     }
00085     return false;
00086 }
00087 
00088 bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
00089 {
00090     if(abs(current-target)<=error)
00091         s=Y_BIAS_MIN;
00092     else
00093         s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_HE)+Y_BIAS_MIN;
00094     if(current>target+error) {
00095         //_m1dir=1;
00096         //_m2dir=1;
00097         if(angle==0) {
00098             _m1f=_m1f*(1+s);
00099             _m1b=_m1b*(1+s);
00100         } else {
00101             _m2f=_m2f*(1+s);
00102             _m2b=_m2b*(1+s);
00103         }
00104     } else if(current<target-error) {
00105         //_m1dir=0;
00106         //_m2dir=0;
00107         if(angle==0) {
00108             _m2f=_m2f*(1+s);
00109             _m2b=_m2b*(1+s);
00110         } else {
00111             _m1f=_m1f*(1+s);
00112             _m1b=_m1b*(1+s);
00113         }
00114     } else {
00115         s=0;
00116         return true;
00117     }
00118     return false;
00119 }
00120 
00121 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
00122 {
00123     float a = 0;
00124     int diff = 0;
00125     diff = 180-wrap(target);
00126     if(abs(wrap(current+diff)-180)<=error)
00127         a=SPEED_TURN_MIN;
00128     else
00129         a=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
00130     switch(mode) {
00131         case ANGLE_TURN:
00132             if(wrap(current+diff)>180+error) {
00133                 _m1dir=0;
00134                 _m2dir=1;
00135                 setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current));
00136                 //_m2dir=0;
00137             } else if(wrap(current+diff)<180-error) {
00138                 _m1dir=1;
00139                 _m2dir=0;
00140                 setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current));
00141                 //_m2dir=1;
00142             } else {
00143                 setMotors(0);
00144                 return true;
00145             }
00146             break;
00147         case ANGLE_BIAS:
00148             if(wrap(current+diff)>180+error) {
00149                 if(_m1dir==0)
00150                     setMotors12(s*1.4,s*0.6);
00151                 else
00152                     setMotors12(s*0.6,s*1.4);
00153                 //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
00154                 //_m2dir=0;
00155             } else if(wrap(current+diff)<180-error) {
00156                 //_m1dir=1;
00157                 //_m2dir=0;
00158                 if(_m1dir==0)
00159                     setMotors12(s*0.6,s*1.4);
00160                 else
00161                     setMotors12(s*1.4,s*0.6);
00162                 //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
00163                 //_m2dir=1;
00164             } else {
00165                 //setMotors(0);
00166                 return true;
00167             }         
00168             break;
00169     }
00170     return false;
00171 }
00172 
00173 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget)
00174 {
00175     if (yTarget>=FRAME_HE-ROB_SIZE/2 && xGood) {
00176         *xCurrState=X_STOP;
00177     }
00178     if (*xCurrState==X_INCREASE) {
00179         if(angleGood && xGood) {
00180             *xCurrState=X_DECREASE;
00181             wait(0.5);
00182         }
00183     }
00184     else if (*xCurrState==X_DECREASE) {
00185         if(angleGood && xGood) {
00186             *xCurrState=X_INCREASE;
00187             wait(0.5);
00188         }
00189 
00190     }
00191 
00192     switch(*xCurrState) {
00193 
00194         case X_INCREASE:
00195             *angleGoal=0+TILTZZ;
00196             _m1dir=0;
00197             _m2dir=0;
00198             *xTarget=X_FAR_GOAL;
00199             break;
00200 
00201         case X_DECREASE:
00202             *angleGoal=0-TILTZZ;
00203             _m1dir=1;
00204             _m2dir=1;
00205             *xTarget=X_NEAR_GOAL;
00206             break;
00207 
00208     }
00209 }
00210 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
00211 {
00212     if (xGood||yGood) {
00213         *angleTol=2;
00214     } else {
00215         *angleTol=10;
00216     }
00217 }
00218 
00219 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
00220 {
00221     if (yGood && xGood) {
00222         *yTarget+=Y_INCREMENT;
00223     }
00224     if (*yTarget>(FRAME_HE-ROB_SIZE/2)) {
00225         *yTarget=FRAME_HE-ROB_SIZE/2;
00226     }
00227 }
00228 
00229 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)
00230 {
00231     *xGood=abs(xya.x-xGoal)<xErr;
00232     *yGood=abs(xya.y-yGoal)<yErr;
00233     *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
00234 }
00235 
00236 void LOCOMOTION::mStop(void)
00237 {
00238     setMotors(0);
00239 }
00240 void LOCOMOTION::moveF(int angle)
00241 {
00242     _m1dir=1;
00243     _m2dir=1;
00244     setMotors12(compG(0.2,_m1dir,angle),compG(0.2,_m2dir,angle));
00245 }
00246 
00247 void LOCOMOTION::moveB(void)
00248 {
00249     _m1dir=1;
00250     _m2dir=1;
00251     setMotors(0.15) ;
00252 }
00253