uhbduhbv
Dependencies: BNO055_fusion mbed
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 19:5832e34b7533
- Parent:
- 18:f9012e93edb8
--- a/LOCOMOTION.cpp Tue Mar 29 02:12:08 2016 +0000 +++ b/LOCOMOTION.cpp Tue Mar 29 21:58:26 2016 +0000 @@ -132,4 +132,73 @@ int LOCOMOTION::wrap(int num) { return num%360; +} +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=xFarGoal; + _m1dir=1; + _m2dir=1; + break; + + case x_decrease: + *angleGoal=0; + *xTarget=xNearGoal; + _m1dir=1; + _m2dir=1; + break; + + case x_backwards: + if (*angleGoal==0) { + *xTarget=xNearGoal+backoff; + } else { + *xTarget=xNearGoal-backoff; + } + _m1dir=0; + _m2dir=0; + break; + } +} + +void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood) +{ + if (xGood) { + *angleTol=2; + } else if(yGood) { + *angleTol=2; + } else { + *angleTol=10; + } +} + +void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget) +{ + if (xGood && angleGood && yGood) { + *yTarget+=yIncrement; + } +} + +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=abs(xya.a-angleGoal)<yErr; } \ No newline at end of file