Localization
Dependencies: BNO055_fusion mbed
Revision 18:f9012e93edb8, committed 2016-03-29
- Comitter:
- 12104404
- Date:
- Tue Mar 29 02:12:08 2016 +0000
- Parent:
- 17:2f89826b5679
- Commit message:
- ROBOT CLEANS THE PATH;
Changed in this revision
diff -r 2f89826b5679 -r f9012e93edb8 LOCOMOTION.cpp --- 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;
diff -r 2f89826b5679 -r f9012e93edb8 LOCOMOTION.h --- a/LOCOMOTION.h Tue Mar 29 01:11:09 2016 +0000 +++ b/LOCOMOTION.h Tue Mar 29 02:12:08 2016 +0000 @@ -22,8 +22,8 @@ PwmOut _m2b; DigitalOut _m1dir; DigitalOut _m2dir; - bool setXPos(int target, int current, int error); - bool setYPos(int target, int current, int error); + bool setXPos(int target, int current, int error, int angle); + bool setYPos(int target, int current, int error, int angle); bool setAngle(int target, int current, int error, int mode); int wrap(int num);
diff -r 2f89826b5679 -r f9012e93edb8 main.cpp --- a/main.cpp Tue Mar 29 01:11:09 2016 +0000 +++ b/main.cpp Tue Mar 29 02:12:08 2016 +0000 @@ -22,6 +22,7 @@ int target=20; int angle_error=2; bool xGood=false; +int angleTarget=0; void setTarget(); void send(); @@ -34,15 +35,19 @@ pc.baud(9600); //pc.printf("Initialized Localization: %d\n",loc.init()); t.attach(&send,1); - tTarget.attach(&setTarget,7); + //tTarget.attach(&setTarget,7); while(1) { //loc.get_angle(&xya); loc.get_xy(&xya); - if(motion.setAngle(0,xya.a,angle_error,ANGLE_TURN)) { - xGood = motion.setXPos(target,xya.x,2); - if(motion.setYPos(130,xya.y,2) || xGood) + if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { + xGood = motion.setXPos(target,xya.x,2,angleTarget); + if(motion.setYPos(130,xya.y,2,angleTarget)) { angle_error=2; - else + } else if(xGood) { + target=target==20?80:20; + angleTarget=angleTarget==0?180:0; + angle_error=2; + } else angle_error=10; } //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);