version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCOMOTION.cpp
- Revision:
- 18:f9012e93edb8
- Parent:
- 17:2f89826b5679
- Child:
- 19:5832e34b7533
--- a/LOCOMOTION.cpp Tue Mar 29 01:11:09 2016 +0000 +++ b/LOCOMOTION.cpp Tue Mar 29 02:12:08 2016 +0000 @@ -11,7 +11,7 @@ _m2dir=0; } -bool LOCOMOTION::setXPos(int target, int current, int error) +bool LOCOMOTION::setXPos(int target, int current, int error, int angle) { //s = 0; if(abs(current-target)<=error) @@ -19,15 +19,25 @@ else s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; if(current>target+error) { - _m1dir=1; - _m2dir=1; + if(angle==0) { + _m1dir=1; + _m2dir=1; + } else { + _m1dir=0; + _m2dir=0; + } _m1f=s; _m1b=s; _m2f=s; _m2b=s; } else if(current<target-error) { - _m1dir=0; - _m2dir=0; + if(angle==0) { + _m1dir=0; + _m2dir=0; + } else { + _m1dir=1; + _m2dir=1; + } _m1f=s; _m1b=s; _m2f=s; @@ -43,7 +53,7 @@ return false; } -bool LOCOMOTION::setYPos(int target, int current, int error) +bool LOCOMOTION::setYPos(int target, int current, int error, int angle) { //float s = 0; if(abs(current-target)<=error) @@ -53,16 +63,26 @@ if(current>target+error) { //_m1dir=1; //_m2dir=1; - _m1f=_m1f*(1+s); - _m1b=_m1b*(1+s); + if(angle==0) { + _m1f=_m1f*(1+s); + _m1b=_m1b*(1+s); + } else { + _m2f=_m2f*(1+s); + _m2b=_m2b*(1+s); + } } else if(current<target-error) { //_m1dir=0; //_m2dir=0; - _m2f=_m2f*(1+s); - _m2b=_m2b*(1+s); + if(angle==0) { + _m2f=_m2f*(1+s); + _m2b=_m2b*(1+s); + } else { + _m1f=_m1f*(1+s); + _m1b=_m1b*(1+s); + } } else { s=0; - + return true; } return false;