Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 29:e8ef4a2e628d
- Parent:
- 28:65daccc10f31
- Child:
- 30:116cd143fdf7
diff -r 65daccc10f31 -r e8ef4a2e628d LOCOMOTION.cpp --- a/LOCOMOTION.cpp Wed Apr 13 07:19:18 2016 +0000 +++ b/LOCOMOTION.cpp Wed Apr 13 07:48:49 2016 +0000 @@ -32,9 +32,9 @@ inline float LOCOMOTION::compG(float speed, bool dir, int angle) { if(dir) - return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; + return (sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; else - return -1*(cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; + return -1*(sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; } inline int LOCOMOTION::wrap(int num) @@ -127,13 +127,13 @@ default: case ANGLE_TURN: if(wrap(current+diff)>180+error) { + _m1dir=0; + _m2dir=1; + setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); + } else 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;