File content as of revision 19:5832e34b7533:

#ifndef LOCOMOTION_H
#define LOCOMOTION_H

#include "mbed.h"
#include "LOCALIZE.h"

#define SPEED_TURN_MIN  0.20
#define SPEED_TURN_MAX  0.65
#define x_increase 1
#define x_decrease 2
#define x_backwards 3
#define backoff 20
#define xNearGoal 20
#define xFarGoal 80
#define yIncrement 20


enum {
    ANGLE_TURN  = 0,
    ANGLE_BIAS  = 1,
};

class LOCOMOTION
{
public:
    LOCOMOTION(PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2);
    PwmOut _m1f;
    PwmOut _m1b;
    PwmOut _m2f;
    PwmOut _m2b;
    DigitalOut _m1dir;
    DigitalOut _m2dir;
    void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal);
    void setAngleTol(int *angleTol,bool yGood, bool xGood);
    void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget);
    void adjustY(int XcurrState,int Ytarget);
    bool setXPos(int target, int current, int error, int angle);
    bool setYPos(int target, int current, int error, int angle);
    bool setAngle(int target, int current, int error, int mode);
    void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr);
    int wrap(int num);

protected:
    float s;
};
#endif