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 }