main.cpp

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