LOCOMOTION.h

00001 #ifndef LOCOMOTION_H
00002 #define LOCOMOTION_H
00003 
00004 #include "mbed.h"
00005 #include "LOCALIZE.h"
00006 
00007 #define SPEED_TURN_MIN  0.20
00008 #define SPEED_TURN_MAX  0.65
00009 #define X_INCREASE 1
00010 #define X_DECREASE 2
00011 #define X_BACKWARDS 3
00012 #define BACKOFF 20
00013 #define X_NEAR_GOAL 20
00014 #define X_FAR_GOAL 80
00015 #define Y_INCREMENT 20
00016 
00017 
00018 enum {
00019     ANGLE_TURN  = 0,
00020     ANGLE_BIAS  = 1,
00021 };
00022 
00023 class LOCOMOTION
00024 {
00025 public:
00026     LOCOMOTION(PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2);
00027     PwmOut _m1f;
00028     PwmOut _m1b;
00029     PwmOut _m2f;
00030     PwmOut _m2b;
00031     DigitalOut _m1dir;
00032     DigitalOut _m2dir;
00033     void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal);
00034     void setAngleTol(int *angleTol,bool yGood, bool xGood);
00035     void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget);
00036     void adjustY(int XcurrState,int Ytarget);
00037     bool setXPos(int target, int current, int error, int angle);
00038     bool setYPos(int target, int current, int error, int angle);
00039     bool setAngle(int target, int current, int error, int mode);
00040     void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr);
00041     int wrap(int num);
00042 
00043 protected:
00044     float s;
00045 };
00046 #endif