File content as of revision 40:9a97c4403c0a:

#ifndef LOCOMOTION_H
#define LOCOMOTION_H
 
#include "mbed.h"
#include "LOCALIZE.h"
#include "math.h"
 
#define SPEED_TURN_MIN  0.20//0.15
#define SPEED_TURN_MAX  0.35//0.45
#define Y_BIAS_MIN      0.95
#define Y_BIAS_MAX      1.00
#define SPEED_X_MIN     0.70//0.15
#define SPEED_X_MAX     0.90//0.25
#define GAIN_GRAVITY    0.5
#define M_PI            3.1415926535897932384
#define BIAS_SPEED      0.75
 
#define X_STOP 0
#define X_INCREASE 1
#define X_DECREASE 2
#define X_BACKWARDS 3
#define ROTATE_1 4
#define ROTATE_2 5
#define X_BEGINING 6
#define BARRIER_CROSS 7
#define BACKOFF 10
#define X_NEAR_GOAL 30
#define X_FAR_GOAL 120
#define Y_INCREMENT 7
#define FRAME_HE 180
#define ROB_SIZE 34
#define TILTZZ -5
 
enum {
    ANGLE_TURN  = 0,
    ANGLE_BIAS  = 1,
};
 
class LOCOMOTION
{
public:
    LOCOMOTION(PinName en, PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4);
    DigitalOut _en;
    PwmOut _m1f;
    PwmOut _m1b;
    PwmOut _m2f;
    PwmOut _m2b;
    DigitalOut _m1dir;
    DigitalOut _m2dir;
    DigitalOut _led1;
    DigitalOut _led2;
    DigitalOut _led3;
    DigitalOut _led4;
    void eStop(void);
    void mStop(void);
    bool setXPos(int target, int current, int error, int angle);
    bool setYBias(int target, int current, int error, int angle);
    bool setAngle(int target, int current, int error, int mode);
    
    void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget );
    void setAngleTol(int *angleTol,bool yGood, bool xGood);
    void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget);
    void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr);
    void moveF(int angle);
    void moveB(void);
    
protected:
    float s;
    void setMotors(float x);
    void setMotors12(float x, float y);
    int wrap(int num);
    inline float compG(float speed, bool dir, int angle);
    
};
#endif