main.cpp

00001 #include "LOCALIZE.h"
00002 #include "LOCOMOTION.h"
00003 #include "SAFETY.h"
00004 
00005 #define WATCHDOG_TIME   10
00006 #define PC_MODE         1
00007 
00008 #if defined (PC_MODE)
00009 Serial pc(USBTX, USBRX);
00010 #else
00011 Serial pc(p13, p14);
00012 Ticker t;
00013 void send();
00014 #endif
00015 
00016 DigitalOut led1(LED1);
00017 DigitalOut led2(LED2);
00018 DigitalOut led3(LED3);
00019 DigitalOut led4(LED4);
00020 
00021 SAFETY safe;
00022 PwmOut suction(p26);
00023 I2C i2c1(p28, p27);
00024 I2C i2c2(p9, p10);
00025 LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
00026 LOCALIZE_xya xya;
00027 LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4);
00028 
00029 void BrownOut();
00030 
00031 int xTarget=20;
00032 int angle_error=2;
00033 bool xGood=false;
00034 bool yGood=false;
00035 bool angleGood=false;
00036 int xState=X_INCREASE;
00037 int angleTarget=0;
00038 int yTarget=30;
00039 
00040 //bool flag=false;
00041 //int target=20;
00042 //int angle_error=2;
00043 //bool xGood=false;
00044 //int angleTarget=0;
00045 
00046 int main()
00047 {
00048     //Set ESC Period
00049     suction.period_ms(2);
00050     //Initialize to 0
00051     suction.pulsewidth_us(1000);
00052     //Watchdog Reset Indicator
00053     if ((LPC_WDT->WDMOD>>2)&1)
00054         led4=1;
00055     else
00056         led3=1;
00057     //Start Watchdog
00058     safe.kick(WATCHDOG_TIME);
00059     //Power Down Ethernet and USB and Enable Brown Out Interrupt
00060     safe.init((unsigned)BrownOut);
00061     //Serial Baudrate
00062     pc.baud(9600);
00063     //Attach Periodic Wireless Printing
00064 #if not defined(PC_MODE)
00065     t.attach(&send,1);
00066 #endif
00067     led1=0;
00068     led2=0;
00069     led3=0;
00070     led4=0;
00071     suction.pulsewidth_us(1000);
00072     while(1) {
00073         if (yTarget>120){
00074         motion.mStop();
00075         safe.kick();
00076         continue;
00077         }
00078         //suction.pulsewidth_us(1900);
00079         loc.get_xy(&xya);
00080         motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
00081         
00082         motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error);
00083         motion.setAngleTol(&angle_error,yGood,xGood);
00084         motion.setYgoal(xGood,angleGood,yGood,&yTarget);
00085         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
00086             motion.setXPos(xTarget,xya.x,2,angleTarget);
00087             motion.setYBias(yTarget,xya.y,2,angleTarget);
00088 
00089         }
00090         /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
00091             xGood = motion.setXPos(target,xya.x,2,angleTarget);
00092             if(motion.setYBias(130,xya.y,2,angleTarget)) {
00093                 angle_error=2;
00094             } else if(xGood) {
00095                 target=target==20?80:20;
00096                 angleTarget=angleTarget==0?180:0;
00097                 angle_error=2;
00098             } else
00099                 angle_error=10;
00100         }*/
00101 
00102 
00103 #if defined(PC_MODE)
00104         pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
00105 #endif
00106         //Feed the dog
00107         safe.kick();
00108     }
00109 }
00110 
00111 void send()
00112 {
00113     //Print over serial port to WiFi MCU
00114     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
00115 }
00116 
00117 void BrownOut()
00118 {
00119     //Stop Motors and Driver
00120     motion.eStop();
00121     //Ensure suction on
00122     suction.pulsewidth_us(2000);
00123     //Light Up Error Light Pattern
00124     led1=1;
00125     led2=0;
00126     led3=1;
00127     led4=0;
00128     //Power Down Non Essential
00129     Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO));
00130     //Sleep wake by interrupt
00131     //Sleep();
00132 }