File content as of revision 6:0602a9e8118b:

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

#define SPEED_TURN_MIN  0.15
#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 turn(int angle);

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(abs(xya.a-180)>5) {
        loc.get_angle(&xya);
        turn(180);
    }
    while(1) {
        led1=!sw1;
        led2=!sw2;
        led3=!sw3;
        led4=!sw4;
        loc.get_angle(&xya);
        //loc.get_xy(&xya);
        loc.get_raw_xy();
        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
        /*if(loc._rx_n>20) {
            dir1=1;
            dir2=1;
        } else if(loc._rx_n<10) {
            dir1=0;
            dir2=0;
        }*/

        if((!sw1==1 && !sw2==1) && flag) {
            motor1F=0.3;
            motor1B=0.3;
            motor2F=0.3;
            motor2B=0.3;
            dir1=0;
            dir2=0;
            wait(0.5);
            while((xya.a>5 && xya.a<355)) {
                loc.get_angle(&xya);
                turn(0);
            }
            flag=false;
        } else if((!sw1==1 && !sw2==1) && !flag) {
            motor1F=0.3;
            motor1B=0.3;
            motor2F=0.3;
            motor2B=0.3;
            dir1=0;
            dir2=0;
            wait(0.5);
            while(abs(xya.a-180)>=5) {
                loc.get_angle(&xya);
                turn(180);
            }
            flag=true;
        } else {
            float s=0;
            if(loc._rx_n<=10)
                s=0.1;
            else if(loc._rx_n<=30)
                s=0.4*abs(loc._rx_n-30)/30+0.1;
            else
                s=0.5;
            motor1F=s;
            motor1B=s;
            motor2F=s;
            motor2B=s;
            dir1=1;
            dir2=1;
        }

        wdt.kick();
    }
}

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

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