Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCOMOTION.h
- Revision:
- 27:fb1298d35bd1
- Parent:
- 26:0ea6a550a99d
- Child:
- 29:e8ef4a2e628d
diff -r 0ea6a550a99d -r fb1298d35bd1 LOCOMOTION.h --- a/LOCOMOTION.h Wed Apr 06 04:04:00 2016 +0000 +++ b/LOCOMOTION.h Wed Apr 13 03:14:43 2016 +0000 @@ -3,13 +3,16 @@ #include "mbed.h" #include "LOCALIZE.h" +#include "math.h" -#define SPEED_TURN_MIN 0.15 -#define SPEED_TURN_MAX 0.45 +#define SPEED_TURN_MIN 0.25//0.15 +#define SPEED_TURN_MAX 0.50//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 GAIN_GRAVITY 0.7 +#define M_PI 3.1415926535897932384 #define X_INCREASE 1 #define X_DECREASE 2 @@ -51,11 +54,15 @@ 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 \ No newline at end of file