Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 25:f3bf8351bbd4
- Parent:
- 24:fb1f083ebd62
- Child:
- 26:0ea6a550a99d
--- a/LOCOMOTION.cpp Sun Apr 03 07:25:07 2016 +0000 +++ b/LOCOMOTION.cpp Tue Apr 05 02:30:40 2016 +0000 @@ -4,7 +4,7 @@ _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4) { s=0; - stopMotors(); + setMotors(0); _m1dir=0; _m2dir=0; _en=1; @@ -13,12 +13,12 @@ _led1=0; } -inline void LOCOMOTION::stopMotors(void) +inline void LOCOMOTION::setMotors(float x) { - _m1f=0; - _m1b=0; - _m2f=0; - _m2b=0; + _m1f=x; + _m1b=x; + _m2f=x; + _m2b=x; } @@ -30,18 +30,17 @@ void LOCOMOTION::eStop(void) { //Stop Motors - stopMotors(); + setMotors(0); //Disable Motor Drivers _en=0; } bool LOCOMOTION::setXPos(int target, int current, int error, int angle) { - //s = 0; if(abs(current-target)<=error) - s=0.07; + s=SPEED_X_MIN; else - s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; + s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN; if(current>target+error) { if(angle==0) { _m1dir=1; @@ -50,10 +49,7 @@ _m1dir=0; _m2dir=0; } - _m1f=s; - _m1b=s; - _m2f=s; - _m2b=s; + setMotors(s); } else if(current<target-error) { if(angle==0) { _m1dir=0; @@ -62,24 +58,20 @@ _m1dir=1; _m2dir=1; } - _m1f=s; - _m1b=s; - _m2f=s; - _m2b=s; + setMotors(s); } else { - stopMotors(); + setMotors(0); return true; } return false; } -bool LOCOMOTION::setYPos(int target, int current, int error, int angle) +bool LOCOMOTION::setYBias(int target, int current, int error, int angle) { - //float s = 0; if(abs(current-target)<=error) - s=0.50; + s=Y_BIAS_MIN; else - s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; + s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN; if(current>target+error) { //_m1dir=1; //_m2dir=1; @@ -102,7 +94,6 @@ } } else { s=0; - return true; } return false; @@ -118,29 +109,89 @@ 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; - _m1f=s; - _m1b=s; - _m2f=s; - _m2b=s; + setMotors(s); } else if(wrap(current+diff)<180-error) { _m1dir=0; _m2dir=1; - _m1f=s; - _m1b=s; - _m2f=s; - _m2b=s; + setMotors(s); } else { - stopMotors(); + setMotors(0); return true; } break; - case ANGLE_BIAS: + } + return false; +} + +void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal) +{ + if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) { + if(angleGood && xGood) { + *xCurrState=X_BACKWARDS; + + } + } + if(*xCurrState==X_BACKWARDS) { + if(xGood && angleGood) { + if(*angleGoal==0) { + *xCurrState=X_INCREASE; + } else { + *xCurrState=X_DECREASE; + } + } + } + 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; } - return false; +} + +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 (xGood && angleGood && 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); } \ No newline at end of file