File content as of revision 20:1da786e205eb:

#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 xTarget=20;
int angle_error=2;
bool xGood=false;
bool yGood=false;
bool angleGood=false;
int xState=X_INCREASE;
int angleTarget=0;
int yTarget=30;
//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);
        motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,2,2,angle_error);
        
        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget);
        motion.setAngleTol(&angle_error,yGood,xGood);
        motion.setYgoal(xGood,angleGood,yGood,&yTarget);
        if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
            motion.setXPos(xTarget,xya.x,2,angleTarget);
            motion.setYPos(yTarget,xya.y,2,angleTarget);

        }

        //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()
{
    xTarget=xTarget==20?80:20;
}