Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
LOCOMOTION.cpp
- Committer:
- 12104404
- Date:
- 2016-04-13
- Revision:
- 28:65daccc10f31
- Parent:
- 27:fb1298d35bd1
- Child:
- 29:e8ef4a2e628d
File content as of revision 28:65daccc10f31:
#include "LOCOMOTION.h" LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4): _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4) { s=0; setMotors(0); _m1dir=0; _m2dir=0; _en=1; _led1=1; wait(0.01); _led1=0; } inline void LOCOMOTION::setMotors(float x) { _m1f=x; _m1b=x; _m2f=x; _m2b=x; } inline void LOCOMOTION::setMotors12(float x,float y) { _m1f=x; _m1b=x; _m2f=y; _m2b=y; } inline float LOCOMOTION::compG(float speed, bool dir, int angle) { if(dir) return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; else return -1*(cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; } inline int LOCOMOTION::wrap(int num) { return num%360; } void LOCOMOTION::eStop(void) { //Stop Motors setMotors(0); //Disable Motor Drivers _en=0; } bool LOCOMOTION::setXPos(int target, int current, int error, int angle) { if(abs(current-target)<=error) s=SPEED_X_MIN; else s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN; if(current>target+error) { if(angle==0) { _m1dir=1; _m2dir=1; } else { _m1dir=0; _m2dir=0; } setMotors(s); } else if(current<target-error) { if(angle==0) { _m1dir=0; _m2dir=0; } else { _m1dir=1; _m2dir=1; } setMotors(s); } else { setMotors(0); return true; } return false; } bool LOCOMOTION::setYBias(int target, int current, int error, int angle) { if(abs(current-target)<=error) s=Y_BIAS_MIN; else s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN; if(current>target+error) { //_m1dir=1; //_m2dir=1; if(angle==0) { _m1f=_m1f*(1+s); _m1b=_m1b*(1+s); } else { _m2f=_m2f*(1+s); _m2b=_m2b*(1+s); } } else if(current<target-error) { //_m1dir=0; //_m2dir=0; if(angle==0) { _m2f=_m2f*(1+s); _m2b=_m2b*(1+s); } else { _m1f=_m1f*(1+s); _m1b=_m1b*(1+s); } } else { s=0; return true; } return false; } bool LOCOMOTION::setAngle(int target, int current, int error, int mode) { s = 0; int diff = 0; diff = 180-wrap(target); if(abs(wrap(current+diff)-180)<=error) s=SPEED_TURN_MIN; else s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; switch(mode) { default: case ANGLE_TURN: if(wrap(current+diff)>180+error) { _m1dir=1; _m2dir=0; setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); } else if(wrap(current+diff)<180-error) { _m1dir=0; _m2dir=1; setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); } else { setMotors(0); return true; } break; } return false; } void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol) { if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) { if(angleGood && xGood) { *xCurrState=X_BACKWARDS; wait(0.5); } } else if(xGood && angleGood && *xCurrState==X_BACKWARDS) { if(*angleGoal==0) { *xCurrState=ROTATE_1; wait(0.5); } else { *xCurrState=ROTATE_2; wait(0.5); } } else if (xGood && angleGood) { if (*xCurrState==ROTATE_1) { *xCurrState=X_INCREASE; wait(0.5); } else { *xCurrState=X_DECREASE; wait(0.5); } } switch(*xCurrState) { case X_INCREASE: *angleGoal=180; *xTarget=X_FAR_GOAL; _m1dir=1; _m2dir=1; break; case X_DECREASE: *angleGoal=0; *xTarget=X_NEAR_GOAL; //_m1dir=1; //_m2dir=1; break; case X_BACKWARDS: if (*angleGoal==0) { *xTarget=X_NEAR_GOAL+BACKOFF; } else { *xTarget=X_FAR_GOAL-BACKOFF; } //_m1dir=0; //_m2dir=0; break; case ROTATE_1: *xTarget=*xTarget; *angleTol=2; *angleGoal=180; break; case ROTATE_2: *xTarget=*xTarget; *angleTol=2; *angleGoal=0; break; } } void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood) { if (xGood||yGood) { *angleTol=2; } else { *angleTol=10; } } void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget) { if (yGood) { *yTarget+=Y_INCREMENT; } } 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) { *xGood=abs(xya.x-xGoal)<xErr; *yGood=abs(xya.y-yGoal)<yErr; *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN); } void LOCOMOTION::mStop(void) { setMotors(0); } void LOCOMOTION::moveF(int angle) { _m1dir=1; _m2dir=1; setMotors12(compG(0.5,_m1dir,angle),compG(0.5,_m2dir,angle)); } void LOCOMOTION::moveB(void) { _m1dir=1; _m2dir=1; setMotors(0.15) ; }