DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: LOCOMOTION.h
- Revision:
- 25:f3bf8351bbd4
- Parent:
- 24:fb1f083ebd62
- Child:
- 26:0ea6a550a99d
diff -r fb1f083ebd62 -r f3bf8351bbd4 LOCOMOTION.h --- a/LOCOMOTION.h Sun Apr 03 07:25:07 2016 +0000 +++ b/LOCOMOTION.h Tue Apr 05 02:30:40 2016 +0000 @@ -4,8 +4,20 @@ #include "mbed.h" #include "LOCALIZE.h" -#define SPEED_TURN_MIN 0.20 -#define SPEED_TURN_MAX 0.65 +#define SPEED_TURN_MIN 0.15 +#define SPEED_TURN_MAX 0.45 +#define Y_BIAS_MIN 0.50 +#define Y_BIAS_MAX 1.00 +#define SPEED_X_MIN 0.07 +#define SPEED_X_MAX 0.17 + +#define X_INCREASE 1 +#define X_DECREASE 2 +#define X_BACKWARDS 3 +#define BACKOFF 30 +#define X_NEAR_GOAL 20 +#define X_FAR_GOAL 80 +#define Y_INCREMENT 20 enum { ANGLE_TURN = 0, @@ -29,12 +41,17 @@ DigitalOut _led4; void eStop(void); bool setXPos(int target, int current, int error, int angle); - bool setYPos(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); + 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); protected: float s; - void stopMotors(void); + void setMotors(float x); int wrap(int num); };