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 xTarget=20;
00023 int angle_error=2;
00024 bool xGood=false;
00025 bool yGood=false;
00026 bool angleGood=false;
00027 int xState=X_INCREASE;
00028 int angleTarget=0;
00029 int yTarget=30;
00030 //void setTarget();
00031 void send();
00032 //void setAngle(int angle);
00033 int wrap(int a);
00034 
00035 int main()
00036 {
00037     wdt.kick(5);
00038     pc.baud(9600);
00039     //pc.printf("Initialized Localization: %d\n",loc.init());
00040     t.attach(&send,1);
00041     //tTarget.attach(&setTarget,7);
00042     while(1) {
00043         //loc.get_angle(&xya);
00044         loc.get_xy(&xya);
00045         motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,2,2,angle_error);
00046         
00047         motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget);
00048         motion.setAngleTol(&angle_error,yGood,xGood);
00049         motion.setYgoal(xGood,angleGood,yGood,&yTarget);
00050         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
00051             motion.setXPos(xTarget,xya.x,2,angleTarget);
00052             motion.setYPos(yTarget,xya.y,2,angleTarget);
00053 
00054         }
00055 
00056         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
00057         wdt.kick();
00058     }
00059 }
00060 
00061 
00062 void send()
00063 {
00064     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
00065 }
00066 
00067 void setTarget()
00068 {
00069     xTarget=xTarget==20?80:20;
00070 }