Localization
Dependencies: BNO055_fusion mbed
main.cpp
00001 #include "LOCALIZE.h" 00002 #include "LOCOMOTION.h" 00003 #include "WATCHDOG.h" 00004 00005 #define SPEED_FB_MIN 0.15 00006 #define SPEED_FB_MAX 0.50 00007 00008 Serial pc(p13, p14); 00009 //Serial pc(USBTX, USBRX); 00010 00011 Watchdog wdt; 00012 00013 I2C i2c1(p28, p27); 00014 I2C i2c2(p9, p10); 00015 LOCALIZE loc(i2c1, i2c2, p26, p8, p7, p6, p5); 00016 LOCALIZE_xya xya; 00017 LOCOMOTION motion(p21, p22, p23, p24, p15, p16); 00018 00019 Ticker t; 00020 Ticker tTarget; 00021 bool flag=false; 00022 int target=20; 00023 int angle_error=2; 00024 bool xGood=false; 00025 int angleTarget=0; 00026 00027 void setTarget(); 00028 void send(); 00029 //void setAngle(int angle); 00030 int wrap(int a); 00031 00032 int main() 00033 { 00034 wdt.kick(5); 00035 pc.baud(9600); 00036 //pc.printf("Initialized Localization: %d\n",loc.init()); 00037 t.attach(&send,1); 00038 //tTarget.attach(&setTarget,7); 00039 while(1) { 00040 //loc.get_angle(&xya); 00041 loc.get_xy(&xya); 00042 if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { 00043 xGood = motion.setXPos(target,xya.x,2,angleTarget); 00044 if(motion.setYPos(130,xya.y,2,angleTarget)) { 00045 angle_error=2; 00046 } else if(xGood) { 00047 target=target==20?80:20; 00048 angleTarget=angleTarget==0?180:0; 00049 angle_error=2; 00050 } else 00051 angle_error=10; 00052 } 00053 //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); 00054 wdt.kick(); 00055 } 00056 } 00057 00058 void send() 00059 { 00060 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); 00061 } 00062 00063 void setTarget() 00064 { 00065 target=target==20?80:20; 00066 }
Generated on Fri Jul 15 2022 21:18:53 by 1.7.2