Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 16:d6f15a13c3aa
- Parent:
- 15:7729da55873a
- Child:
- 17:2f89826b5679
diff -r 7729da55873a -r d6f15a13c3aa LOCOMOTION.cpp --- a/LOCOMOTION.cpp Thu Mar 24 07:20:51 2016 +0000 +++ b/LOCOMOTION.cpp Sun Mar 27 02:39:07 2016 +0000 @@ -1,7 +1,7 @@ #include "LOCOMOTION.h" LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): - _m1f(motor1F), _m1b(motor2F), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) + _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) { _m1f=0; _m1b=0; @@ -20,21 +20,36 @@ s=SPEED_TURN_MIN; else s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; - if(wrap(current+diff)>180+error) { - - } else if(wrap(current+diff)<180-error) { - - } else { - s = 0; - } switch(mode) { case ANGLE_TURN: - + if(wrap(current+diff)>180+error) { + _m1dir=1; + _m2dir=0; + _m1f=s; + _m1b=s; + _m2f=s; + _m2b=s; + } else if(wrap(current+diff)<180-error) { + _m1dir=0; + _m2dir=1; + _m1f=s; + _m1b=s; + _m2f=s; + _m2b=s; + } else { + s=0; + _m1f=s; + _m1b=s; + _m2f=s; + _m2b=s; + return true; + } break; case ANGLE_BIAS: break; } + return false; } int LOCOMOTION::wrap(int num)