File content as of revision 15:7729da55873a:

#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, p20, p19, p18, p17);
LOCALIZE_xya xya;
LOCOMOTION motion(p21, p22, p23, p24, p15, p16);

Ticker t;
bool flag=false;

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);
    while(1) {
        motion.setAngle(0,0,5,ANGLE_TURN);
        //loc.get_angle(&xya);
        loc.get_xy(&xya);
        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
        //setAngle(0);
        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-SPEED_TURN_MIN)*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;
}