version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: main.cpp
- Revision:
- 20:1da786e205eb
- Parent:
- 19:5832e34b7533
diff -r 5832e34b7533 -r 1da786e205eb main.cpp --- a/main.cpp Tue Mar 29 21:58:26 2016 +0000 +++ b/main.cpp Tue Mar 29 22:49:05 2016 +0000 @@ -19,15 +19,15 @@ Ticker t; Ticker tTarget; bool flag=false; -int target=20; +int xTarget=20; int angle_error=2; bool xGood=false; bool yGood=false; bool angleGood=false; - +int xState=X_INCREASE; int angleTarget=0; int yTarget=30; -void setTarget(); +//void setTarget(); void send(); //void setAngle(int angle); int wrap(int a); @@ -42,23 +42,23 @@ while(1) { //loc.get_angle(&xya); loc.get_xy(&xya); - motion.check_xya(&xGood,&yGood,&angleGood,target,angleTarget,yTarget,angle_error) + motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,2,2,angle_error); + + motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget); + motion.setAngleTol(&angle_error,yGood,xGood); + motion.setYgoal(xGood,angleGood,yGood,&yTarget); 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 if(xGood) { - target=target==20?80:20; - angleTarget=angleTarget==0?180:0; - angle_error=2; - } else - angle_error=10; + motion.setXPos(xTarget,xya.x,2,angleTarget); + motion.setYPos(yTarget,xya.y,2,angleTarget); + } + //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); wdt.kick(); } } + void send() { pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); @@ -66,5 +66,5 @@ void setTarget() { - target=target==20?80:20; + xTarget=xTarget==20?80:20; } \ No newline at end of file