Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 27:fb1298d35bd1
- Parent:
- 26:0ea6a550a99d
- Child:
- 28:65daccc10f31
--- a/LOCOMOTION.cpp Wed Apr 06 04:04:00 2016 +0000 +++ b/LOCOMOTION.cpp Wed Apr 13 03:14:43 2016 +0000 @@ -20,7 +20,14 @@ _m2f=x; _m2b=x; } - + +inline void LOCOMOTION::setMotors12(float x,float y) +{ + _m1f=x; + _m1b=x; + _m2f=y; + _m2b=y; +} inline int LOCOMOTION::wrap(int num) { @@ -114,11 +121,11 @@ if(wrap(current+diff)>180+error) { _m1dir=1; _m2dir=0; - setMotors(s); + setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); } else if(wrap(current+diff)<180-error) { _m1dir=0; _m2dir=1; - setMotors(s); + setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); } else { setMotors(0); return true; @@ -223,4 +230,25 @@ void LOCOMOTION::mStop(void) { setMotors(0); +} +void LOCOMOTION::moveF(int angle) +{ + _m1dir=1; + _m2dir=1; + setMotors12(compG(0.5,_m1dir,angle),compG(0.5,_m2dir,angle)); +} + +void LOCOMOTION::moveB(void) +{ + _m1dir=1; + _m2dir=1; + setMotors(0.15) ; +} + +inline float LOCOMOTION::compG(float speed, bool dir, int angle) +{ + if(dir) + return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; + else + return -1*(cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; } \ No newline at end of file