version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCOMOTION.cpp
- Revision:
- 20:1da786e205eb
- Parent:
- 19:5832e34b7533
--- a/LOCOMOTION.cpp Tue Mar 29 21:58:26 2016 +0000 +++ b/LOCOMOTION.cpp Tue Mar 29 22:49:05 2016 +0000 @@ -61,8 +61,6 @@ else s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; if(current>target+error) { - //_m1dir=1; - //_m2dir=1; if(angle==0) { _m1f=_m1f*(1+s); _m1b=_m1b*(1+s); @@ -71,8 +69,6 @@ _m2b=_m2b*(1+s); } } else if(current<target-error) { - //_m1dir=0; - //_m2dir=0; if(angle==0) { _m2f=_m2f*(1+s); _m2b=_m2b*(1+s); @@ -135,42 +131,42 @@ } void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal) { - if (*xCurrState==x_increase|| *xCurrState==x_decrease) { + if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) { if(angleGood && xGood) { - *xCurrState=x_backwards; + *xCurrState=X_BACKWARDS; } } - if(*xCurrState==x_backwards) { + if(*xCurrState==X_BACKWARDS) { if(xGood && angleGood) { if(*angleGoal==0) { - *xCurrState=x_increase; + *xCurrState=X_INCREASE; } else { - *xCurrState=x_decrease; + *xCurrState=X_DECREASE; } } } switch(*xCurrState) { - case x_increase: + case X_INCREASE: *angleGoal=180; - *xTarget=xFarGoal; + *xTarget=X_FAR_GOAL; _m1dir=1; _m2dir=1; break; - case x_decrease: + case X_DECREASE: *angleGoal=0; - *xTarget=xNearGoal; + *xTarget=X_NEAR_GOAL; _m1dir=1; _m2dir=1; break; - case x_backwards: + case X_BACKWARDS: if (*angleGoal==0) { - *xTarget=xNearGoal+backoff; + *xTarget=X_NEAR_GOAL+BACKOFF; } else { - *xTarget=xNearGoal-backoff; + *xTarget=X_FAR_GOAL-BACKOFF; } _m1dir=0; _m2dir=0; @@ -180,9 +176,7 @@ void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood) { - if (xGood) { - *angleTol=2; - } else if(yGood) { + if (xGood||yGood) { *angleTol=2; } else { *angleTol=10; @@ -192,7 +186,7 @@ void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget) { if (xGood && angleGood && yGood) { - *yTarget+=yIncrement; + *yTarget+=Y_INCREMENT; } }