drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: LOCOMOTION.cpp
- Revision:
- 34:083073e54dbd
- Parent:
- 33:baf24c59affc
--- a/LOCOMOTION.cpp Wed Apr 20 16:11:08 2016 +0000 +++ b/LOCOMOTION.cpp Wed Apr 20 18:25:43 2016 +0000 @@ -57,16 +57,16 @@ else s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN; if(current>target+error) { - if(angle==0) { + if(angle==0+TILTZZ) { + _m1dir=0; + _m2dir=0; + } else { _m1dir=1; _m2dir=1; - } else { - _m1dir=0; - _m2dir=0; } setMotors(s); } else if(current<target-error) { - if(angle==0) { + if(angle==0+TILTZZ) { _m1dir=0; _m2dir=0; } else { @@ -124,7 +124,6 @@ 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=0; @@ -167,14 +166,14 @@ switch(*xCurrState) { case X_INCREASE: - *angleGoal=0; + *angleGoal=0+TILTZZ; _m1dir=0; _m2dir=0; *xTarget=X_FAR_GOAL; break; case X_DECREASE: - *angleGoal=0; + *angleGoal=0-TILTZZ; _m1dir=1; _m2dir=1; *xTarget=X_NEAR_GOAL;