File content as of revision 34:083073e54dbd:

#include "LOCALIZE.h"
#include "LOCOMOTION.h"
#include "SAFETY.h"
#include "BMP280.h"

#define WATCHDOG_TIME   10
//#define PC_MODE         1

#if defined (PC_MODE)
Serial pc(USBTX, USBRX);
#else
Serial pc(p13, p14);
Ticker t;
void send();
#endif

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

SAFETY safe;
PwmOut suction(p26);
I2C i2c2(p28, p27);
I2C i2c1(p9, p10);
BMP280 pres(i2c2);
LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
LOCALIZE_xya xya;
LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4);
//LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4);

void BrownOut();

int xTarget=120;
int angle_error=2;
bool xGood=false;
bool yGood=false;
bool angleGood=false;
int xState=X_INCREASE;
int angleTarget=0;
int yTarget=+10+ROB_SIZE/2;
int pressure;

//bool flag=false;
//int target=20;
//int angle_error=2;
//bool xGood=false;
//int angleTarget=0;

int main()
{
    //Set ESC Period
    suction.period_ms(2);
    //Initialize to 0
    suction.pulsewidth_us(1000);
    wait(2);
    /*suction.pulsewidth_us(1000);
    wait(2);*/
    //Watchdog Reset Indicator
    if ((LPC_WDT->WDMOD>>2)&1)
        led4=1;
    else
        led3=1;
    //Start Watchdog
    safe.kick(WATCHDOG_TIME);
    //Power Down Ethernet and USB and Enable Brown Out Interrupt
    safe.init((unsigned)BrownOut);
    //Serial Baudrate
    pc.baud(9600);
    //Initialize Locomotion
    loc.init();
    //pres.initialize();
    //Attach Periodic Wireless Printing
#if not defined(PC_MODE)
    t.attach(&send,1);
#endif
    led1=0;
    led2=0;
    led3=0;
    led4=0;
    suction.pulsewidth_us(1000);
    wait(0.5);
    suction.pulsewidth_us(1050);
    wait(0.5);
    suction.pulsewidth_us(1100);
    wait(0.5);
    suction.pulsewidth_us(1150);
    wait(0.5);
    suction.pulsewidth_us(1250);
    wait(0.5);
    suction.pulsewidth_us(1300);
    wait(0.5);
    while(1) {
        suction.pulsewidth_us(1350);
        /*pressure=(int)read[0];//(int)pres.getTemperature();
        if(pressure==88)
            led1=1;
        else
            led1=0;*/
        //uncomment this part if you want the robot to just drive down the window with no separtor
        if (xState==0) {
            motion.mStop();
            safe.kick();
            continue;
        }
        loc.get_xy(&xya);
        motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);

        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error,yTarget);
        //motion.setAngleTol(&angle_error,yGood,xGood);
       // motion.setYgoal(xGood,angleGood,yGood,&yTarget);
        if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
            motion.setXPos(xTarget,xya.x,2,angleTarget);
         //   motion.setYBias(yTarget,xya.y,2,angleTarget);

        }
        //loc.get_xy(&xya);
#if defined(PC_MODE)
        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
#endif
        //Feed the dog
        safe.kick();
    }
}

void send()
{
    //Print over serial port to WiFi MCU
    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xGood,xya.a/10,xya.a%10);
}

void BrownOut()
{
    //Stop Motors and Driver
    motion.eStop();
    //Ensure suction on
    suction.pulsewidth_us(1250);
    //Light Up Error Light Pattern
    led1=1;
    led2=0;
    led3=1;
    led4=0;
    //Power Down Non Essential
    Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO));
    //Sleep wake by interrupt
    //Sleep();
}