DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
LOCOMOTION.h
- Committer:
- 12104404
- Date:
- 2016-04-26
- Revision:
- 37:e8a6ea255c09
- Parent:
- 36:dc69442c4c47
File content as of revision 37:e8a6ea255c09:
#ifndef LOCOMOTION_H #define LOCOMOTION_H #include "mbed.h" #include "LOCALIZE.h" #include "math.h" #define SPEED_TURN_MIN 0.20//0.15 #define SPEED_TURN_MAX 0.35//0.45 #define Y_BIAS_MIN 0.95 #define Y_BIAS_MAX 1.00 #define SPEED_X_MIN 0.70//0.15 #define SPEED_X_MAX 0.90//0.25 #define GAIN_GRAVITY 0.5 #define M_PI 3.1415926535897932384 #define BIAS_SPEED 0.75 #define X_STOP 0 #define X_INCREASE 1 #define X_DECREASE 2 #define X_BACKWARDS 3 #define ROTATE_1 4 #define ROTATE_2 5 #define X_BEGINING 6 #define BARRIER_CROSS 7 #define BACKOFF 10 #define X_NEAR_GOAL 30 #define X_FAR_GOAL 120 #define Y_INCREMENT 7 #define FRAME_HE 180 #define ROB_SIZE 34 #define TILTZZ -5 enum { ANGLE_TURN = 0, ANGLE_BIAS = 1, }; class LOCOMOTION { public: LOCOMOTION(PinName en, PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4); DigitalOut _en; PwmOut _m1f; PwmOut _m1b; PwmOut _m2f; PwmOut _m2b; DigitalOut _m1dir; DigitalOut _m2dir; DigitalOut _led1; DigitalOut _led2; DigitalOut _led3; DigitalOut _led4; void eStop(void); void mStop(void); bool setXPos(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,int *angleTol,int yTarget ); 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