version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCOMOTION.h
- Revision:
- 19:5832e34b7533
- Parent:
- 18:f9012e93edb8
- Child:
- 20:1da786e205eb
--- a/LOCOMOTION.h Tue Mar 29 02:12:08 2016 +0000 +++ b/LOCOMOTION.h Tue Mar 29 21:58:26 2016 +0000 @@ -6,6 +6,14 @@ #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, @@ -22,9 +30,14 @@ 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: