File content as of revision 20:10a305298e27:

#include "PowerControl/PowerControl.h"
#include "PowerControl/EthernetPowerControl.h"
#include "LOCALIZE.h"
#include "LOCOMOTION.h"
#include "WATCHDOG.h"

#define WATCHDOG_TIME   10
//#define PC_MODE         1
#define USR_POWERDOWN   (0x104)

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

int semihost_powerdown()
{
    uint32_t arg;
    return __semihost(USR_POWERDOWN, &arg);
}

Watchdog wdt;
PwmOut suction(p26);
I2C i2c1(p28, p27);
I2C i2c2(p9, p10);
LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5);
LOCALIZE_xya xya;
LOCOMOTION motion(p21, p22, p23, p24, p15, p16);

void BrownOut();

//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);
    //Start Watchdog
    wdt.kick(WATCHDOG_TIME);
    //Power Down Ethernet
    PHY_PowerDown();
    //Power Down USB Chip
    semihost_powerdown();
    //Setup Brown Out Interrupt
    NVIC_SetVector(BOD_IRQn, (unsigned)BrownOut);
    //Enable Brown Out Interrupt
    NVIC_EnableIRQ(BOD_IRQn);
    //Serial Baudrate
    pc.baud(9600);
    //Attach Periodic Wireless Printing
#if not defined(PC_MODE)
    t.attach(&send,1);
#endif
    while(1) {
        //loc.get_xy(&xya);
        /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
            xGood = motion.setXPos(target,xya.x,2,angleTarget);
            if(motion.setYPos(130,xya.y,2,angleTarget)) {
                angle_error=2;
            } else if(xGood) {
                target=target==20?80:20;
                angleTarget=angleTarget==0?180:0;
                angle_error=2;
            } else
                angle_error=10;
        }*/
#if defined(PC_MODE)
        pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
#endif
        //Feed the dog
        wdt.kick();
    }
}

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

void BrownOut()
{
    //Ensure suction on
    motion.stopMotors();
    suction.pulsewidth_us(2000);
    //Power Down Non Essential
    Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0));
    //Sleep wake by interrupt
    Sleep();
}