version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
main.cpp
- Committer:
- alaurens
- Date:
- 2016-03-29
- Revision:
- 20:1da786e205eb
- Parent:
- 19:5832e34b7533
File content as of revision 20:1da786e205eb:
#include "LOCALIZE.h" #include "LOCOMOTION.h" #include "WATCHDOG.h" #define SPEED_FB_MIN 0.15 #define SPEED_FB_MAX 0.50 Serial pc(p13, p14); //Serial pc(USBTX, USBRX); Watchdog wdt; I2C i2c1(p28, p27); I2C i2c2(p9, p10); LOCALIZE loc(i2c1, i2c2, p26, p8, p7, p6, p5); LOCALIZE_xya xya; LOCOMOTION motion(p21, p22, p23, p24, p15, p16); Ticker t; Ticker tTarget; bool flag=false; 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 send(); //void setAngle(int angle); int wrap(int a); int main() { wdt.kick(5); pc.baud(9600); //pc.printf("Initialized Localization: %d\n",loc.init()); t.attach(&send,1); //tTarget.attach(&setTarget,7); while(1) { //loc.get_angle(&xya); loc.get_xy(&xya); 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)) { 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); } void setTarget() { xTarget=xTarget==20?80:20; }