DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
LOCOMOTION.cpp@26:0ea6a550a99d, 2016-04-06 (annotated)
- Committer:
- 12104404
- Date:
- Wed Apr 06 04:04:00 2016 +0000
- Revision:
- 26:0ea6a550a99d
- Parent:
- 25:f3bf8351bbd4
- Child:
- 27:fb1298d35bd1
added antoine stuff
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12104404 | 6:0602a9e8118b | 1 | #include "LOCOMOTION.h" |
12104404 | 26:0ea6a550a99d | 2 | |
12104404 | 24:fb1f083ebd62 | 3 | LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4): |
12104404 | 24:fb1f083ebd62 | 4 | _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4) |
12104404 | 23:455f7da3dd7a | 5 | { |
12104404 | 23:455f7da3dd7a | 6 | s=0; |
12104404 | 25:f3bf8351bbd4 | 7 | setMotors(0); |
12104404 | 23:455f7da3dd7a | 8 | _m1dir=0; |
12104404 | 23:455f7da3dd7a | 9 | _m2dir=0; |
12104404 | 23:455f7da3dd7a | 10 | _en=1; |
12104404 | 24:fb1f083ebd62 | 11 | _led1=1; |
12104404 | 24:fb1f083ebd62 | 12 | wait(0.01); |
12104404 | 24:fb1f083ebd62 | 13 | _led1=0; |
12104404 | 23:455f7da3dd7a | 14 | } |
12104404 | 26:0ea6a550a99d | 15 | |
12104404 | 25:f3bf8351bbd4 | 16 | inline void LOCOMOTION::setMotors(float x) |
12104404 | 15:7729da55873a | 17 | { |
12104404 | 25:f3bf8351bbd4 | 18 | _m1f=x; |
12104404 | 25:f3bf8351bbd4 | 19 | _m1b=x; |
12104404 | 25:f3bf8351bbd4 | 20 | _m2f=x; |
12104404 | 25:f3bf8351bbd4 | 21 | _m2b=x; |
12104404 | 15:7729da55873a | 22 | } |
12104404 | 26:0ea6a550a99d | 23 | |
12104404 | 26:0ea6a550a99d | 24 | |
12104404 | 23:455f7da3dd7a | 25 | inline int LOCOMOTION::wrap(int num) |
12104404 | 19:2dd81b864e14 | 26 | { |
12104404 | 23:455f7da3dd7a | 27 | return num%360; |
12104404 | 23:455f7da3dd7a | 28 | } |
12104404 | 26:0ea6a550a99d | 29 | |
12104404 | 23:455f7da3dd7a | 30 | void LOCOMOTION::eStop(void) |
12104404 | 23:455f7da3dd7a | 31 | { |
12104404 | 23:455f7da3dd7a | 32 | //Stop Motors |
12104404 | 25:f3bf8351bbd4 | 33 | setMotors(0); |
12104404 | 23:455f7da3dd7a | 34 | //Disable Motor Drivers |
12104404 | 23:455f7da3dd7a | 35 | _en=0; |
12104404 | 19:2dd81b864e14 | 36 | } |
12104404 | 26:0ea6a550a99d | 37 | |
12104404 | 18:f9012e93edb8 | 38 | bool LOCOMOTION::setXPos(int target, int current, int error, int angle) |
12104404 | 17:2f89826b5679 | 39 | { |
12104404 | 17:2f89826b5679 | 40 | if(abs(current-target)<=error) |
12104404 | 25:f3bf8351bbd4 | 41 | s=SPEED_X_MIN; |
12104404 | 17:2f89826b5679 | 42 | else |
12104404 | 25:f3bf8351bbd4 | 43 | s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN; |
12104404 | 17:2f89826b5679 | 44 | if(current>target+error) { |
12104404 | 18:f9012e93edb8 | 45 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 46 | _m1dir=1; |
12104404 | 18:f9012e93edb8 | 47 | _m2dir=1; |
12104404 | 18:f9012e93edb8 | 48 | } else { |
12104404 | 18:f9012e93edb8 | 49 | _m1dir=0; |
12104404 | 18:f9012e93edb8 | 50 | _m2dir=0; |
12104404 | 18:f9012e93edb8 | 51 | } |
12104404 | 25:f3bf8351bbd4 | 52 | setMotors(s); |
12104404 | 17:2f89826b5679 | 53 | } else if(current<target-error) { |
12104404 | 18:f9012e93edb8 | 54 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 55 | _m1dir=0; |
12104404 | 18:f9012e93edb8 | 56 | _m2dir=0; |
12104404 | 18:f9012e93edb8 | 57 | } else { |
12104404 | 18:f9012e93edb8 | 58 | _m1dir=1; |
12104404 | 18:f9012e93edb8 | 59 | _m2dir=1; |
12104404 | 18:f9012e93edb8 | 60 | } |
12104404 | 25:f3bf8351bbd4 | 61 | setMotors(s); |
12104404 | 17:2f89826b5679 | 62 | } else { |
12104404 | 25:f3bf8351bbd4 | 63 | setMotors(0); |
12104404 | 17:2f89826b5679 | 64 | return true; |
12104404 | 17:2f89826b5679 | 65 | } |
12104404 | 17:2f89826b5679 | 66 | return false; |
12104404 | 17:2f89826b5679 | 67 | } |
12104404 | 26:0ea6a550a99d | 68 | |
12104404 | 25:f3bf8351bbd4 | 69 | bool LOCOMOTION::setYBias(int target, int current, int error, int angle) |
12104404 | 17:2f89826b5679 | 70 | { |
12104404 | 17:2f89826b5679 | 71 | if(abs(current-target)<=error) |
12104404 | 25:f3bf8351bbd4 | 72 | s=Y_BIAS_MIN; |
12104404 | 17:2f89826b5679 | 73 | else |
12104404 | 25:f3bf8351bbd4 | 74 | s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN; |
12104404 | 17:2f89826b5679 | 75 | if(current>target+error) { |
12104404 | 17:2f89826b5679 | 76 | //_m1dir=1; |
12104404 | 17:2f89826b5679 | 77 | //_m2dir=1; |
12104404 | 18:f9012e93edb8 | 78 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 79 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 80 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 81 | } else { |
12104404 | 18:f9012e93edb8 | 82 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 83 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 84 | } |
12104404 | 17:2f89826b5679 | 85 | } else if(current<target-error) { |
12104404 | 17:2f89826b5679 | 86 | //_m1dir=0; |
12104404 | 17:2f89826b5679 | 87 | //_m2dir=0; |
12104404 | 18:f9012e93edb8 | 88 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 89 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 90 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 91 | } else { |
12104404 | 18:f9012e93edb8 | 92 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 93 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 94 | } |
12104404 | 17:2f89826b5679 | 95 | } else { |
12104404 | 17:2f89826b5679 | 96 | s=0; |
12104404 | 17:2f89826b5679 | 97 | return true; |
12104404 | 17:2f89826b5679 | 98 | } |
12104404 | 17:2f89826b5679 | 99 | return false; |
12104404 | 17:2f89826b5679 | 100 | } |
12104404 | 26:0ea6a550a99d | 101 | |
12104404 | 15:7729da55873a | 102 | bool LOCOMOTION::setAngle(int target, int current, int error, int mode) |
12104404 | 15:7729da55873a | 103 | { |
12104404 | 17:2f89826b5679 | 104 | s = 0; |
12104404 | 15:7729da55873a | 105 | int diff = 0; |
12104404 | 15:7729da55873a | 106 | diff = 180-wrap(target); |
12104404 | 15:7729da55873a | 107 | if(abs(wrap(current+diff)-180)<=error) |
12104404 | 15:7729da55873a | 108 | s=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 109 | else |
12104404 | 15:7729da55873a | 110 | s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 111 | switch(mode) { |
12104404 | 25:f3bf8351bbd4 | 112 | default: |
12104404 | 15:7729da55873a | 113 | case ANGLE_TURN: |
12104404 | 16:d6f15a13c3aa | 114 | if(wrap(current+diff)>180+error) { |
12104404 | 16:d6f15a13c3aa | 115 | _m1dir=1; |
12104404 | 16:d6f15a13c3aa | 116 | _m2dir=0; |
12104404 | 25:f3bf8351bbd4 | 117 | setMotors(s); |
12104404 | 16:d6f15a13c3aa | 118 | } else if(wrap(current+diff)<180-error) { |
12104404 | 16:d6f15a13c3aa | 119 | _m1dir=0; |
12104404 | 16:d6f15a13c3aa | 120 | _m2dir=1; |
12104404 | 25:f3bf8351bbd4 | 121 | setMotors(s); |
12104404 | 16:d6f15a13c3aa | 122 | } else { |
12104404 | 25:f3bf8351bbd4 | 123 | setMotors(0); |
12104404 | 16:d6f15a13c3aa | 124 | return true; |
12104404 | 16:d6f15a13c3aa | 125 | } |
12104404 | 15:7729da55873a | 126 | break; |
12104404 | 25:f3bf8351bbd4 | 127 | } |
12104404 | 25:f3bf8351bbd4 | 128 | return false; |
12104404 | 25:f3bf8351bbd4 | 129 | } |
12104404 | 26:0ea6a550a99d | 130 | |
12104404 | 26:0ea6a550a99d | 131 | void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol) |
12104404 | 25:f3bf8351bbd4 | 132 | { |
12104404 | 25:f3bf8351bbd4 | 133 | if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) { |
12104404 | 25:f3bf8351bbd4 | 134 | if(angleGood && xGood) { |
12104404 | 25:f3bf8351bbd4 | 135 | *xCurrState=X_BACKWARDS; |
12104404 | 26:0ea6a550a99d | 136 | wait(0.5); |
12104404 | 26:0ea6a550a99d | 137 | |
12104404 | 25:f3bf8351bbd4 | 138 | } |
12104404 | 25:f3bf8351bbd4 | 139 | } |
12104404 | 26:0ea6a550a99d | 140 | |
12104404 | 26:0ea6a550a99d | 141 | else if(xGood && angleGood && *xCurrState==X_BACKWARDS) { |
12104404 | 26:0ea6a550a99d | 142 | if(*angleGoal==0) { |
12104404 | 26:0ea6a550a99d | 143 | *xCurrState=ROTATE_1; |
12104404 | 26:0ea6a550a99d | 144 | wait(0.5); |
12104404 | 26:0ea6a550a99d | 145 | } else { |
12104404 | 26:0ea6a550a99d | 146 | *xCurrState=ROTATE_2; |
12104404 | 26:0ea6a550a99d | 147 | wait(0.5); |
12104404 | 25:f3bf8351bbd4 | 148 | } |
12104404 | 25:f3bf8351bbd4 | 149 | } |
12104404 | 26:0ea6a550a99d | 150 | |
12104404 | 26:0ea6a550a99d | 151 | else if (xGood && angleGood) { |
12104404 | 26:0ea6a550a99d | 152 | if (*xCurrState==ROTATE_1) { |
12104404 | 26:0ea6a550a99d | 153 | *xCurrState=X_INCREASE; |
12104404 | 26:0ea6a550a99d | 154 | wait(0.5); |
12104404 | 26:0ea6a550a99d | 155 | } else { |
12104404 | 26:0ea6a550a99d | 156 | *xCurrState=X_DECREASE; |
12104404 | 26:0ea6a550a99d | 157 | wait(0.5); |
12104404 | 26:0ea6a550a99d | 158 | } |
12104404 | 26:0ea6a550a99d | 159 | } |
12104404 | 26:0ea6a550a99d | 160 | |
12104404 | 25:f3bf8351bbd4 | 161 | switch(*xCurrState) { |
12104404 | 25:f3bf8351bbd4 | 162 | case X_INCREASE: |
12104404 | 25:f3bf8351bbd4 | 163 | *angleGoal=180; |
12104404 | 25:f3bf8351bbd4 | 164 | *xTarget=X_FAR_GOAL; |
12104404 | 25:f3bf8351bbd4 | 165 | _m1dir=1; |
12104404 | 25:f3bf8351bbd4 | 166 | _m2dir=1; |
12104404 | 25:f3bf8351bbd4 | 167 | break; |
12104404 | 26:0ea6a550a99d | 168 | |
12104404 | 25:f3bf8351bbd4 | 169 | case X_DECREASE: |
12104404 | 25:f3bf8351bbd4 | 170 | *angleGoal=0; |
12104404 | 25:f3bf8351bbd4 | 171 | *xTarget=X_NEAR_GOAL; |
12104404 | 26:0ea6a550a99d | 172 | //_m1dir=1; |
12104404 | 26:0ea6a550a99d | 173 | //_m2dir=1; |
12104404 | 25:f3bf8351bbd4 | 174 | break; |
12104404 | 26:0ea6a550a99d | 175 | |
12104404 | 25:f3bf8351bbd4 | 176 | case X_BACKWARDS: |
12104404 | 25:f3bf8351bbd4 | 177 | if (*angleGoal==0) { |
12104404 | 25:f3bf8351bbd4 | 178 | *xTarget=X_NEAR_GOAL+BACKOFF; |
12104404 | 25:f3bf8351bbd4 | 179 | } else { |
12104404 | 25:f3bf8351bbd4 | 180 | *xTarget=X_FAR_GOAL-BACKOFF; |
12104404 | 25:f3bf8351bbd4 | 181 | } |
12104404 | 26:0ea6a550a99d | 182 | //_m1dir=0; |
12104404 | 26:0ea6a550a99d | 183 | //_m2dir=0; |
12104404 | 15:7729da55873a | 184 | break; |
12104404 | 26:0ea6a550a99d | 185 | |
12104404 | 26:0ea6a550a99d | 186 | case ROTATE_1: |
12104404 | 26:0ea6a550a99d | 187 | *xTarget=*xTarget; |
12104404 | 26:0ea6a550a99d | 188 | *angleTol=2; |
12104404 | 26:0ea6a550a99d | 189 | *angleGoal=180; |
12104404 | 26:0ea6a550a99d | 190 | break; |
12104404 | 26:0ea6a550a99d | 191 | |
12104404 | 26:0ea6a550a99d | 192 | case ROTATE_2: |
12104404 | 26:0ea6a550a99d | 193 | *xTarget=*xTarget; |
12104404 | 26:0ea6a550a99d | 194 | *angleTol=2; |
12104404 | 26:0ea6a550a99d | 195 | *angleGoal=0; |
12104404 | 26:0ea6a550a99d | 196 | break; |
12104404 | 26:0ea6a550a99d | 197 | |
12104404 | 15:7729da55873a | 198 | } |
12104404 | 25:f3bf8351bbd4 | 199 | } |
12104404 | 25:f3bf8351bbd4 | 200 | void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood) |
12104404 | 25:f3bf8351bbd4 | 201 | { |
12104404 | 25:f3bf8351bbd4 | 202 | if (xGood||yGood) { |
12104404 | 25:f3bf8351bbd4 | 203 | *angleTol=2; |
12104404 | 25:f3bf8351bbd4 | 204 | } else { |
12104404 | 25:f3bf8351bbd4 | 205 | *angleTol=10; |
12104404 | 25:f3bf8351bbd4 | 206 | } |
12104404 | 25:f3bf8351bbd4 | 207 | } |
12104404 | 26:0ea6a550a99d | 208 | |
12104404 | 25:f3bf8351bbd4 | 209 | void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget) |
12104404 | 25:f3bf8351bbd4 | 210 | { |
12104404 | 26:0ea6a550a99d | 211 | if (yGood) { |
12104404 | 25:f3bf8351bbd4 | 212 | *yTarget+=Y_INCREMENT; |
12104404 | 25:f3bf8351bbd4 | 213 | } |
12104404 | 25:f3bf8351bbd4 | 214 | } |
12104404 | 26:0ea6a550a99d | 215 | |
12104404 | 25:f3bf8351bbd4 | 216 | void LOCOMOTION::check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr) |
12104404 | 25:f3bf8351bbd4 | 217 | { |
12104404 | 25:f3bf8351bbd4 | 218 | *xGood=abs(xya.x-xGoal)<xErr; |
12104404 | 25:f3bf8351bbd4 | 219 | *yGood=abs(xya.y-yGoal)<yErr; |
12104404 | 26:0ea6a550a99d | 220 | *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN); |
12104404 | 26:0ea6a550a99d | 221 | } |
12104404 | 26:0ea6a550a99d | 222 | |
12104404 | 26:0ea6a550a99d | 223 | void LOCOMOTION::mStop(void) |
12104404 | 26:0ea6a550a99d | 224 | { |
12104404 | 26:0ea6a550a99d | 225 | setMotors(0); |
12104404 | 23:455f7da3dd7a | 226 | } |