Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.cpp
- Revision:
- 28:65daccc10f31
- Parent:
- 27:fb1298d35bd1
- Child:
- 29:e8ef4a2e628d
--- a/LOCOMOTION.cpp Wed Apr 13 03:14:43 2016 +0000 +++ b/LOCOMOTION.cpp Wed Apr 13 07:19:18 2016 +0000 @@ -28,6 +28,14 @@ _m2f=y; _m2b=y; } + +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; +} inline int LOCOMOTION::wrap(int num) { @@ -245,10 +253,3 @@ 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