LOCOMOTION.h

00001 #ifndef LOCOMOTION_H
00002 #define LOCOMOTION_H
00003  
00004 #include "mbed.h"
00005 #include "LOCALIZE.h"
00006 #include "math.h"
00007  
00008 #define SPEED_TURN_MIN  0.20//0.15
00009 #define SPEED_TURN_MAX  0.35//0.45
00010 #define Y_BIAS_MIN      0.95
00011 #define Y_BIAS_MAX      1.00
00012 #define SPEED_X_MIN     0.70//0.15
00013 #define SPEED_X_MAX     0.90//0.25
00014 #define GAIN_GRAVITY    0.5
00015 #define M_PI            3.1415926535897932384
00016 #define BIAS_SPEED      0.75
00017  
00018 #define X_STOP 0
00019 #define X_INCREASE 1
00020 #define X_DECREASE 2
00021 #define X_BACKWARDS 3
00022 #define ROTATE_1 4
00023 #define ROTATE_2 5
00024 #define X_BEGINING 6
00025 #define BARRIER_CROSS 7
00026 #define BACKOFF 10
00027 #define X_NEAR_GOAL 30
00028 #define X_FAR_GOAL 120
00029 #define Y_INCREMENT 7
00030 #define FRAME_HE 180
00031 #define ROB_SIZE 34
00032 #define TILTZZ -5
00033  
00034 enum {
00035     ANGLE_TURN  = 0,
00036     ANGLE_BIAS  = 1,
00037 };
00038  
00039 class LOCOMOTION
00040 {
00041 public:
00042     LOCOMOTION(PinName en, PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4);
00043     DigitalOut _en;
00044     PwmOut _m1f;
00045     PwmOut _m1b;
00046     PwmOut _m2f;
00047     PwmOut _m2b;
00048     DigitalOut _m1dir;
00049     DigitalOut _m2dir;
00050     DigitalOut _led1;
00051     DigitalOut _led2;
00052     DigitalOut _led3;
00053     DigitalOut _led4;
00054     void eStop(void);
00055     void mStop(void);
00056     bool setXPos(int target, int current, int error, int angle);
00057     bool setYBias(int target, int current, int error, int angle);
00058     bool setAngle(int target, int current, int error, int mode);
00059     
00060     void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget );
00061     void setAngleTol(int *angleTol,bool yGood, bool xGood);
00062     void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget);
00063     void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr);
00064     void moveF(int angle);
00065     void moveB(void);
00066     
00067 protected:
00068     float s;
00069     void setMotors(float x);
00070     void setMotors12(float x, float y);
00071     int wrap(int num);
00072     inline float compG(float speed, bool dir, int angle);
00073     
00074 };
00075 #endif