DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
LOCOMOTION.h@37:e8a6ea255c09, 2016-04-26 (annotated)
- Committer:
- 12104404
- Date:
- Tue Apr 26 20:09:25 2016 +0000
- Revision:
- 37:e8a6ea255c09
- Parent:
- 36:dc69442c4c47
WORKING REALLY WORKING
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12104404 | 6:0602a9e8118b | 1 | #ifndef LOCOMOTION_H |
12104404 | 6:0602a9e8118b | 2 | #define LOCOMOTION_H |
12104404 | 26:0ea6a550a99d | 3 | |
12104404 | 6:0602a9e8118b | 4 | #include "mbed.h" |
12104404 | 17:2f89826b5679 | 5 | #include "LOCALIZE.h" |
12104404 | 27:fb1298d35bd1 | 6 | #include "math.h" |
12104404 | 26:0ea6a550a99d | 7 | |
12104404 | 35:68917796c30a | 8 | #define SPEED_TURN_MIN 0.20//0.15 |
12104404 | 35:68917796c30a | 9 | #define SPEED_TURN_MAX 0.35//0.45 |
12104404 | 35:68917796c30a | 10 | #define Y_BIAS_MIN 0.95 |
12104404 | 25:f3bf8351bbd4 | 11 | #define Y_BIAS_MAX 1.00 |
12104404 | 36:dc69442c4c47 | 12 | #define SPEED_X_MIN 0.70//0.15 |
12104404 | 36:dc69442c4c47 | 13 | #define SPEED_X_MAX 0.90//0.25 |
alaurens | 34:083073e54dbd | 14 | #define GAIN_GRAVITY 0.5 |
12104404 | 27:fb1298d35bd1 | 15 | #define M_PI 3.1415926535897932384 |
12104404 | 37:e8a6ea255c09 | 16 | #define BIAS_SPEED 0.75 |
12104404 | 26:0ea6a550a99d | 17 | |
alaurens | 31:69caabc10879 | 18 | #define X_STOP 0 |
12104404 | 25:f3bf8351bbd4 | 19 | #define X_INCREASE 1 |
12104404 | 25:f3bf8351bbd4 | 20 | #define X_DECREASE 2 |
12104404 | 25:f3bf8351bbd4 | 21 | #define X_BACKWARDS 3 |
12104404 | 26:0ea6a550a99d | 22 | #define ROTATE_1 4 |
12104404 | 26:0ea6a550a99d | 23 | #define ROTATE_2 5 |
alaurens | 31:69caabc10879 | 24 | #define X_BEGINING 6 |
alaurens | 31:69caabc10879 | 25 | #define BARRIER_CROSS 7 |
12104404 | 26:0ea6a550a99d | 26 | #define BACKOFF 10 |
alaurens | 33:baf24c59affc | 27 | #define X_NEAR_GOAL 30 |
alaurens | 33:baf24c59affc | 28 | #define X_FAR_GOAL 120 |
12104404 | 26:0ea6a550a99d | 29 | #define Y_INCREMENT 7 |
alaurens | 31:69caabc10879 | 30 | #define FRAME_HE 180 |
alaurens | 31:69caabc10879 | 31 | #define ROB_SIZE 34 |
alaurens | 34:083073e54dbd | 32 | #define TILTZZ -5 |
12104404 | 26:0ea6a550a99d | 33 | |
12104404 | 15:7729da55873a | 34 | enum { |
12104404 | 15:7729da55873a | 35 | ANGLE_TURN = 0, |
12104404 | 15:7729da55873a | 36 | ANGLE_BIAS = 1, |
12104404 | 15:7729da55873a | 37 | }; |
12104404 | 26:0ea6a550a99d | 38 | |
12104404 | 6:0602a9e8118b | 39 | class LOCOMOTION |
12104404 | 6:0602a9e8118b | 40 | { |
12104404 | 6:0602a9e8118b | 41 | public: |
12104404 | 24:fb1f083ebd62 | 42 | LOCOMOTION(PinName en, PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4); |
12104404 | 23:455f7da3dd7a | 43 | DigitalOut _en; |
12104404 | 15:7729da55873a | 44 | PwmOut _m1f; |
12104404 | 15:7729da55873a | 45 | PwmOut _m1b; |
12104404 | 15:7729da55873a | 46 | PwmOut _m2f; |
12104404 | 15:7729da55873a | 47 | PwmOut _m2b; |
12104404 | 15:7729da55873a | 48 | DigitalOut _m1dir; |
12104404 | 15:7729da55873a | 49 | DigitalOut _m2dir; |
12104404 | 24:fb1f083ebd62 | 50 | DigitalOut _led1; |
12104404 | 24:fb1f083ebd62 | 51 | DigitalOut _led2; |
12104404 | 24:fb1f083ebd62 | 52 | DigitalOut _led3; |
12104404 | 24:fb1f083ebd62 | 53 | DigitalOut _led4; |
12104404 | 23:455f7da3dd7a | 54 | void eStop(void); |
12104404 | 26:0ea6a550a99d | 55 | void mStop(void); |
12104404 | 36:dc69442c4c47 | 56 | bool setXPos(int target, int current, int error, int angle); |
12104404 | 25:f3bf8351bbd4 | 57 | bool setYBias(int target, int current, int error, int angle); |
12104404 | 15:7729da55873a | 58 | bool setAngle(int target, int current, int error, int mode); |
12104404 | 25:f3bf8351bbd4 | 59 | |
alaurens | 31:69caabc10879 | 60 | void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget ); |
12104404 | 25:f3bf8351bbd4 | 61 | void setAngleTol(int *angleTol,bool yGood, bool xGood); |
12104404 | 25:f3bf8351bbd4 | 62 | void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget); |
12104404 | 25:f3bf8351bbd4 | 63 | void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr); |
12104404 | 27:fb1298d35bd1 | 64 | void moveF(int angle); |
12104404 | 27:fb1298d35bd1 | 65 | void moveB(void); |
12104404 | 27:fb1298d35bd1 | 66 | |
12104404 | 15:7729da55873a | 67 | protected: |
12104404 | 17:2f89826b5679 | 68 | float s; |
12104404 | 25:f3bf8351bbd4 | 69 | void setMotors(float x); |
12104404 | 27:fb1298d35bd1 | 70 | void setMotors12(float x, float y); |
12104404 | 23:455f7da3dd7a | 71 | int wrap(int num); |
12104404 | 27:fb1298d35bd1 | 72 | inline float compG(float speed, bool dir, int angle); |
12104404 | 19:2dd81b864e14 | 73 | |
12104404 | 6:0602a9e8118b | 74 | }; |
12104404 | 6:0602a9e8118b | 75 | #endif |