Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 19:2dd81b864e14
- Parent:
- 18:f9012e93edb8
- Child:
- 23:455f7da3dd7a
--- a/LOCOMOTION.cpp Tue Mar 29 02:12:08 2016 +0000 +++ b/LOCOMOTION.cpp Sun Apr 03 05:48:37 2016 +0000 @@ -11,6 +11,14 @@ _m2dir=0; } +void LOCOMOTION::stopMotors(void) +{ + _m1f=0; + _m1b=0; + _m2f=0; + _m2b=0; +} + bool LOCOMOTION::setXPos(int target, int current, int error, int angle) { //s = 0; @@ -43,11 +51,7 @@ _m2f=s; _m2b=s; } else { - s=0; - _m1f=s; - _m1b=s; - _m2f=s; - _m2b=s; + stopMotors(); return true; } return false; @@ -114,11 +118,7 @@ _m2f=s; _m2b=s; } else { - s=0; - _m1f=s; - _m1b=s; - _m2f=s; - _m2b=s; + stopMotors(); return true; } break; @@ -129,7 +129,8 @@ return false; } -int LOCOMOTION::wrap(int num) +inline int LOCOMOTION::wrap(int num) { return num%360; -} \ No newline at end of file +} +