Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 23:455f7da3dd7a
- Parent:
- 19:2dd81b864e14
- Child:
- 24:fb1f083ebd62
--- a/LOCOMOTION.cpp Sun Apr 03 06:15:52 2016 +0000 +++ b/LOCOMOTION.cpp Sun Apr 03 07:08:18 2016 +0000 @@ -1,22 +1,35 @@ #include "LOCOMOTION.h" -LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): - _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) +LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): + _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) +{ + s=0; + stopMotors(); + _m1dir=0; + _m2dir=0; + _en=1; +} + +inline void LOCOMOTION::stopMotors(void) { _m1f=0; _m1b=0; _m2f=0; _m2b=0; - _m1dir=0; - _m2dir=0; } -void LOCOMOTION::stopMotors(void) + +inline int LOCOMOTION::wrap(int num) { - _m1f=0; - _m1b=0; - _m2f=0; - _m2b=0; + return num%360; +} + +void LOCOMOTION::eStop(void) +{ + //Stop Motors + stopMotors(); + //Disable Motor Drivers + _en=0; } bool LOCOMOTION::setXPos(int target, int current, int error, int angle) @@ -127,10 +140,4 @@ break; } return false; -} - -inline int LOCOMOTION::wrap(int num) -{ - return num%360; -} - +} \ No newline at end of file