File content as of revision 11:9518f8285906:

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


#define SPEED_TURN_MIN  0.20
#define SPEED_TURN_MAX  0.35
#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);
LOCALIZE_xya xya;

DigitalIn sw1(p20);
DigitalIn sw2(p19);
DigitalIn sw3(p18);
DigitalIn sw4(p17);
DigitalOut dir1(p15);
DigitalOut dir2(p16);

PwmOut motor1F(p21);
PwmOut motor1B(p22);
PwmOut motor2F(p23);
PwmOut motor2B(p24);

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

Ticker t;
bool flag=false;

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

int main()
{
    wdt.kick(5);
    pc.baud(9600);
    dir1=0;
    dir2=0;
    motor1F=0;
    motor1B=0;
    motor2F=0;
    motor2B=0;
    //pc.printf("Initialized Localization: %d\n",loc.init());
    t.attach(&send,1);
    sw1.mode(PullUp);
    sw2.mode(PullUp);
    sw3.mode(PullUp);
    sw4.mode(PullUp);
    while(1) {
        led1=!sw1;
        led2=!sw2;
        led3=!sw3;
        led4=!sw4;
        //loc.get_angle(&xya);
        loc.get_xy(&xya);
        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
        setAngle(20);
        wdt.kick();
    }
}

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

void setAngle(int angle)
{
    float s = 0;
    int diff = 0;
    diff = 180-wrap(angle);
    if(abs(wrap(xya.a+diff)-180)<=5)
        s=SPEED_TURN_MIN;
    else
        s=(SPEED_TURN_MAX*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
    motor1F=s;
    motor1B=s;
    motor2F=s;
    motor2B=s;
    if(wrap(xya.a+diff)>180+2) {
        dir1=1;
        dir2=0;
    } else if(wrap(xya.a+diff)<180-2) {
        dir1=0;
        dir2=1;
    } else {
        motor1F=0;
        motor1B=0;
        motor2F=0;
        motor2B=0;
    }
}

int wrap(int a)
{
    return a%360;
}