File content as of revision 17:2f89826b5679:

#include "LOCALIZE.h"
#include "LOCOMOTION.h"
#include "WATCHDOG.h"

#define SPEED_FB_MIN    0.15
#define SPEED_FB_MAX    0.50

Serial pc(p13, p14);
//Serial pc(USBTX, USBRX);

Watchdog wdt;

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

Ticker t;
Ticker tTarget;
bool flag=false;
int target=20;
int angle_error=2;
bool xGood=false;

void setTarget();
void send();
//void setAngle(int angle);
int wrap(int a);

int main()
{
    wdt.kick(5);
    pc.baud(9600);
    //pc.printf("Initialized Localization: %d\n",loc.init());
    t.attach(&send,1);
    tTarget.attach(&setTarget,7);
    while(1) {
        //loc.get_angle(&xya);
        loc.get_xy(&xya);
        if(motion.setAngle(0,xya.a,angle_error,ANGLE_TURN)) {
            xGood = motion.setXPos(target,xya.x,2);
            if(motion.setYPos(130,xya.y,2) || xGood)
                angle_error=2;
            else
                angle_error=10;
        }
        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
        wdt.kick();
    }
}

void send()
{
    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
}

void setTarget()
{
    target=target==20?80:20;
}