version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCOMOTION.cpp
- Revision:
- 17:2f89826b5679
- Parent:
- 16:d6f15a13c3aa
- Child:
- 18:f9012e93edb8
--- a/LOCOMOTION.cpp Sun Mar 27 02:39:07 2016 +0000 +++ b/LOCOMOTION.cpp Tue Mar 29 01:11:09 2016 +0000 @@ -11,9 +11,66 @@ _m2dir=0; } +bool LOCOMOTION::setXPos(int target, int current, int error) +{ + //s = 0; + if(abs(current-target)<=error) + s=0.07; + else + s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; + if(current>target+error) { + _m1dir=1; + _m2dir=1; + _m1f=s; + _m1b=s; + _m2f=s; + _m2b=s; + } else if(current<target-error) { + _m1dir=0; + _m2dir=0; + _m1f=s; + _m1b=s; + _m2f=s; + _m2b=s; + } else { + s=0; + _m1f=s; + _m1b=s; + _m2f=s; + _m2b=s; + return true; + } + return false; +} + +bool LOCOMOTION::setYPos(int target, int current, int error) +{ + //float s = 0; + if(abs(current-target)<=error) + s=0.50; + else + s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; + if(current>target+error) { + //_m1dir=1; + //_m2dir=1; + _m1f=_m1f*(1+s); + _m1b=_m1b*(1+s); + } else if(current<target-error) { + //_m1dir=0; + //_m2dir=0; + _m2f=_m2f*(1+s); + _m2b=_m2b*(1+s); + } else { + s=0; + + return true; + } + return false; +} + bool LOCOMOTION::setAngle(int target, int current, int error, int mode) { - float s = 0; + s = 0; int diff = 0; diff = 180-wrap(target); if(abs(wrap(current+diff)-180)<=error) @@ -55,30 +112,4 @@ int LOCOMOTION::wrap(int num) { return num%360; -} -/* -void setAngle(int angle) -{ - float s = 0; - int diff = 0; - diff = 180-wrap(angle); - if(abs(wrap(xya.a+diff)-180)<=5) - s=SPEED_TURN_MIN; - else - s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN; - motor1F=s; - motor1B=s; - motor2F=s; - motor2B=s; - if(wrap(xya.a+diff)>180+2) { - dir1=1; - dir2=0; - } else if(wrap(xya.a+diff)<180-2) { - dir1=0; - dir2=1; - } else { - motor1F=0; - motor1B=0; - motor2F=0; - motor2B=0; - }*/ +} \ No newline at end of file