DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
LOCOMOTION.cpp@38:16208e003dc9, 2016-04-27 (annotated)
- Committer:
- 12104404
- Date:
- Wed Apr 27 04:24:11 2016 +0000
- Revision:
- 38:16208e003dc9
- Parent:
- 37:e8a6ea255c09
good good;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12104404 | 6:0602a9e8118b | 1 | #include "LOCOMOTION.h" |
alaurens | 31:69caabc10879 | 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 | } |
alaurens | 31:69caabc10879 | 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 | 27:fb1298d35bd1 | 23 | |
12104404 | 27:fb1298d35bd1 | 24 | inline void LOCOMOTION::setMotors12(float x,float y) |
12104404 | 27:fb1298d35bd1 | 25 | { |
12104404 | 27:fb1298d35bd1 | 26 | _m1f=x; |
12104404 | 27:fb1298d35bd1 | 27 | _m1b=x; |
12104404 | 27:fb1298d35bd1 | 28 | _m2f=y; |
12104404 | 27:fb1298d35bd1 | 29 | _m2b=y; |
12104404 | 27:fb1298d35bd1 | 30 | } |
12104404 | 28:65daccc10f31 | 31 | |
12104404 | 28:65daccc10f31 | 32 | inline float LOCOMOTION::compG(float speed, bool dir, int angle) |
12104404 | 28:65daccc10f31 | 33 | { |
12104404 | 28:65daccc10f31 | 34 | if(dir) |
12104404 | 29:e8ef4a2e628d | 35 | return (sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; |
12104404 | 28:65daccc10f31 | 36 | else |
12104404 | 29:e8ef4a2e628d | 37 | return -1*(sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed; |
12104404 | 28:65daccc10f31 | 38 | } |
alaurens | 31:69caabc10879 | 39 | |
12104404 | 23:455f7da3dd7a | 40 | inline int LOCOMOTION::wrap(int num) |
12104404 | 19:2dd81b864e14 | 41 | { |
12104404 | 23:455f7da3dd7a | 42 | return num%360; |
12104404 | 23:455f7da3dd7a | 43 | } |
alaurens | 31:69caabc10879 | 44 | |
12104404 | 23:455f7da3dd7a | 45 | void LOCOMOTION::eStop(void) |
12104404 | 23:455f7da3dd7a | 46 | { |
12104404 | 23:455f7da3dd7a | 47 | //Stop Motors |
12104404 | 25:f3bf8351bbd4 | 48 | setMotors(0); |
12104404 | 23:455f7da3dd7a | 49 | //Disable Motor Drivers |
12104404 | 23:455f7da3dd7a | 50 | _en=0; |
12104404 | 19:2dd81b864e14 | 51 | } |
alaurens | 31:69caabc10879 | 52 | |
12104404 | 36:dc69442c4c47 | 53 | bool LOCOMOTION::setXPos(int target, int current, int error, int angle) |
12104404 | 17:2f89826b5679 | 54 | { |
12104404 | 35:68917796c30a | 55 | /*if(abs(current-target)<=error) |
12104404 | 25:f3bf8351bbd4 | 56 | s=SPEED_X_MIN; |
12104404 | 17:2f89826b5679 | 57 | else |
12104404 | 35:68917796c30a | 58 | s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;*/ |
12104404 | 38:16208e003dc9 | 59 | if(abs(current-target)<30) |
12104404 | 25:f3bf8351bbd4 | 60 | s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN; |
12104404 | 35:68917796c30a | 61 | else |
12104404 | 35:68917796c30a | 62 | s=SPEED_X_MAX; |
12104404 | 17:2f89826b5679 | 63 | if(current>target+error) { |
12104404 | 35:68917796c30a | 64 | if(angle==0) { |
12104404 | 36:dc69442c4c47 | 65 | _m1dir=1; |
12104404 | 36:dc69442c4c47 | 66 | _m2dir=1; |
12104404 | 36:dc69442c4c47 | 67 | } else { |
alaurens | 34:083073e54dbd | 68 | _m1dir=0; |
alaurens | 34:083073e54dbd | 69 | _m2dir=0; |
12104404 | 18:f9012e93edb8 | 70 | } |
12104404 | 37:e8a6ea255c09 | 71 | setMotors12(s,s); |
12104404 | 17:2f89826b5679 | 72 | } else if(current<target-error) { |
12104404 | 35:68917796c30a | 73 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 74 | _m1dir=0; |
12104404 | 18:f9012e93edb8 | 75 | _m2dir=0; |
12104404 | 18:f9012e93edb8 | 76 | } else { |
12104404 | 18:f9012e93edb8 | 77 | _m1dir=1; |
12104404 | 18:f9012e93edb8 | 78 | _m2dir=1; |
12104404 | 18:f9012e93edb8 | 79 | } |
12104404 | 37:e8a6ea255c09 | 80 | setMotors12(s,s); |
12104404 | 17:2f89826b5679 | 81 | } else { |
12104404 | 25:f3bf8351bbd4 | 82 | setMotors(0); |
12104404 | 17:2f89826b5679 | 83 | return true; |
12104404 | 17:2f89826b5679 | 84 | } |
12104404 | 17:2f89826b5679 | 85 | return false; |
12104404 | 17:2f89826b5679 | 86 | } |
alaurens | 31:69caabc10879 | 87 | |
12104404 | 25:f3bf8351bbd4 | 88 | bool LOCOMOTION::setYBias(int target, int current, int error, int angle) |
12104404 | 17:2f89826b5679 | 89 | { |
12104404 | 17:2f89826b5679 | 90 | if(abs(current-target)<=error) |
12104404 | 25:f3bf8351bbd4 | 91 | s=Y_BIAS_MIN; |
12104404 | 17:2f89826b5679 | 92 | else |
alaurens | 31:69caabc10879 | 93 | s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_HE)+Y_BIAS_MIN; |
12104404 | 17:2f89826b5679 | 94 | if(current>target+error) { |
12104404 | 17:2f89826b5679 | 95 | //_m1dir=1; |
12104404 | 17:2f89826b5679 | 96 | //_m2dir=1; |
12104404 | 18:f9012e93edb8 | 97 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 98 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 99 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 100 | } else { |
12104404 | 18:f9012e93edb8 | 101 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 102 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 103 | } |
12104404 | 17:2f89826b5679 | 104 | } else if(current<target-error) { |
12104404 | 17:2f89826b5679 | 105 | //_m1dir=0; |
12104404 | 17:2f89826b5679 | 106 | //_m2dir=0; |
12104404 | 18:f9012e93edb8 | 107 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 108 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 109 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 110 | } else { |
12104404 | 18:f9012e93edb8 | 111 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 112 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 113 | } |
12104404 | 17:2f89826b5679 | 114 | } else { |
12104404 | 17:2f89826b5679 | 115 | s=0; |
12104404 | 17:2f89826b5679 | 116 | return true; |
12104404 | 17:2f89826b5679 | 117 | } |
12104404 | 17:2f89826b5679 | 118 | return false; |
12104404 | 17:2f89826b5679 | 119 | } |
alaurens | 31:69caabc10879 | 120 | |
12104404 | 15:7729da55873a | 121 | bool LOCOMOTION::setAngle(int target, int current, int error, int mode) |
12104404 | 15:7729da55873a | 122 | { |
12104404 | 35:68917796c30a | 123 | float a = 0; |
12104404 | 15:7729da55873a | 124 | int diff = 0; |
12104404 | 15:7729da55873a | 125 | diff = 180-wrap(target); |
12104404 | 15:7729da55873a | 126 | if(abs(wrap(current+diff)-180)<=error) |
12104404 | 35:68917796c30a | 127 | a=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 128 | else |
12104404 | 35:68917796c30a | 129 | a=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 130 | switch(mode) { |
12104404 | 15:7729da55873a | 131 | case ANGLE_TURN: |
12104404 | 16:d6f15a13c3aa | 132 | if(wrap(current+diff)>180+error) { |
12104404 | 29:e8ef4a2e628d | 133 | _m1dir=0; |
12104404 | 29:e8ef4a2e628d | 134 | _m2dir=1; |
12104404 | 35:68917796c30a | 135 | setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current)); |
12104404 | 30:116cd143fdf7 | 136 | //_m2dir=0; |
12104404 | 29:e8ef4a2e628d | 137 | } else if(wrap(current+diff)<180-error) { |
12104404 | 16:d6f15a13c3aa | 138 | _m1dir=1; |
12104404 | 16:d6f15a13c3aa | 139 | _m2dir=0; |
12104404 | 35:68917796c30a | 140 | setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current)); |
12104404 | 30:116cd143fdf7 | 141 | //_m2dir=1; |
12104404 | 16:d6f15a13c3aa | 142 | } else { |
12104404 | 25:f3bf8351bbd4 | 143 | setMotors(0); |
12104404 | 16:d6f15a13c3aa | 144 | return true; |
12104404 | 16:d6f15a13c3aa | 145 | } |
12104404 | 15:7729da55873a | 146 | break; |
12104404 | 35:68917796c30a | 147 | case ANGLE_BIAS: |
12104404 | 35:68917796c30a | 148 | if(wrap(current+diff)>180+error) { |
12104404 | 36:dc69442c4c47 | 149 | if(_m1dir==0) |
12104404 | 37:e8a6ea255c09 | 150 | setMotors12(s*1.4,s*0.6); |
12104404 | 36:dc69442c4c47 | 151 | else |
12104404 | 37:e8a6ea255c09 | 152 | setMotors12(s*0.6,s*1.4); |
12104404 | 35:68917796c30a | 153 | //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); |
12104404 | 35:68917796c30a | 154 | //_m2dir=0; |
12104404 | 35:68917796c30a | 155 | } else if(wrap(current+diff)<180-error) { |
12104404 | 35:68917796c30a | 156 | //_m1dir=1; |
12104404 | 35:68917796c30a | 157 | //_m2dir=0; |
12104404 | 36:dc69442c4c47 | 158 | if(_m1dir==0) |
12104404 | 37:e8a6ea255c09 | 159 | setMotors12(s*0.6,s*1.4); |
12104404 | 36:dc69442c4c47 | 160 | else |
12104404 | 37:e8a6ea255c09 | 161 | setMotors12(s*1.4,s*0.6); |
12104404 | 35:68917796c30a | 162 | //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current)); |
12104404 | 35:68917796c30a | 163 | //_m2dir=1; |
12104404 | 35:68917796c30a | 164 | } else { |
12104404 | 35:68917796c30a | 165 | //setMotors(0); |
12104404 | 35:68917796c30a | 166 | return true; |
12104404 | 36:dc69442c4c47 | 167 | } |
12104404 | 35:68917796c30a | 168 | break; |
12104404 | 25:f3bf8351bbd4 | 169 | } |
12104404 | 25:f3bf8351bbd4 | 170 | return false; |
12104404 | 25:f3bf8351bbd4 | 171 | } |
alaurens | 31:69caabc10879 | 172 | |
alaurens | 31:69caabc10879 | 173 | void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget) |
12104404 | 25:f3bf8351bbd4 | 174 | { |
alaurens | 31:69caabc10879 | 175 | if (yTarget>=FRAME_HE-ROB_SIZE/2 && xGood) { |
alaurens | 31:69caabc10879 | 176 | *xCurrState=X_STOP; |
12104404 | 25:f3bf8351bbd4 | 177 | } |
alaurens | 31:69caabc10879 | 178 | if (*xCurrState==X_INCREASE) { |
alaurens | 31:69caabc10879 | 179 | if(angleGood && xGood) { |
12104404 | 26:0ea6a550a99d | 180 | *xCurrState=X_DECREASE; |
12104404 | 26:0ea6a550a99d | 181 | wait(0.5); |
12104404 | 26:0ea6a550a99d | 182 | } |
12104404 | 26:0ea6a550a99d | 183 | } |
alaurens | 33:baf24c59affc | 184 | else if (*xCurrState==X_DECREASE) { |
alaurens | 31:69caabc10879 | 185 | if(angleGood && xGood) { |
alaurens | 31:69caabc10879 | 186 | *xCurrState=X_INCREASE; |
alaurens | 31:69caabc10879 | 187 | wait(0.5); |
alaurens | 31:69caabc10879 | 188 | } |
alaurens | 31:69caabc10879 | 189 | |
alaurens | 31:69caabc10879 | 190 | } |
alaurens | 31:69caabc10879 | 191 | |
12104404 | 25:f3bf8351bbd4 | 192 | switch(*xCurrState) { |
alaurens | 31:69caabc10879 | 193 | |
12104404 | 25:f3bf8351bbd4 | 194 | case X_INCREASE: |
alaurens | 34:083073e54dbd | 195 | *angleGoal=0+TILTZZ; |
alaurens | 33:baf24c59affc | 196 | _m1dir=0; |
alaurens | 33:baf24c59affc | 197 | _m2dir=0; |
alaurens | 31:69caabc10879 | 198 | *xTarget=X_FAR_GOAL; |
12104404 | 25:f3bf8351bbd4 | 199 | break; |
alaurens | 31:69caabc10879 | 200 | |
alaurens | 31:69caabc10879 | 201 | case X_DECREASE: |
alaurens | 34:083073e54dbd | 202 | *angleGoal=0-TILTZZ; |
alaurens | 33:baf24c59affc | 203 | _m1dir=1; |
alaurens | 33:baf24c59affc | 204 | _m2dir=1; |
alaurens | 31:69caabc10879 | 205 | *xTarget=X_NEAR_GOAL; |
12104404 | 15:7729da55873a | 206 | break; |
alaurens | 31:69caabc10879 | 207 | |
12104404 | 15:7729da55873a | 208 | } |
12104404 | 25:f3bf8351bbd4 | 209 | } |
12104404 | 25:f3bf8351bbd4 | 210 | void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood) |
12104404 | 25:f3bf8351bbd4 | 211 | { |
12104404 | 25:f3bf8351bbd4 | 212 | if (xGood||yGood) { |
12104404 | 25:f3bf8351bbd4 | 213 | *angleTol=2; |
12104404 | 25:f3bf8351bbd4 | 214 | } else { |
12104404 | 25:f3bf8351bbd4 | 215 | *angleTol=10; |
12104404 | 25:f3bf8351bbd4 | 216 | } |
12104404 | 25:f3bf8351bbd4 | 217 | } |
alaurens | 31:69caabc10879 | 218 | |
12104404 | 25:f3bf8351bbd4 | 219 | void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget) |
12104404 | 25:f3bf8351bbd4 | 220 | { |
alaurens | 31:69caabc10879 | 221 | if (yGood && xGood) { |
12104404 | 25:f3bf8351bbd4 | 222 | *yTarget+=Y_INCREMENT; |
12104404 | 25:f3bf8351bbd4 | 223 | } |
alaurens | 31:69caabc10879 | 224 | if (*yTarget>(FRAME_HE-ROB_SIZE/2)) { |
alaurens | 31:69caabc10879 | 225 | *yTarget=FRAME_HE-ROB_SIZE/2; |
alaurens | 31:69caabc10879 | 226 | } |
12104404 | 25:f3bf8351bbd4 | 227 | } |
alaurens | 31:69caabc10879 | 228 | |
12104404 | 25:f3bf8351bbd4 | 229 | 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 | 230 | { |
12104404 | 25:f3bf8351bbd4 | 231 | *xGood=abs(xya.x-xGoal)<xErr; |
12104404 | 25:f3bf8351bbd4 | 232 | *yGood=abs(xya.y-yGoal)<yErr; |
12104404 | 26:0ea6a550a99d | 233 | *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN); |
12104404 | 26:0ea6a550a99d | 234 | } |
alaurens | 31:69caabc10879 | 235 | |
12104404 | 26:0ea6a550a99d | 236 | void LOCOMOTION::mStop(void) |
12104404 | 26:0ea6a550a99d | 237 | { |
12104404 | 26:0ea6a550a99d | 238 | setMotors(0); |
12104404 | 27:fb1298d35bd1 | 239 | } |
12104404 | 27:fb1298d35bd1 | 240 | void LOCOMOTION::moveF(int angle) |
12104404 | 27:fb1298d35bd1 | 241 | { |
12104404 | 27:fb1298d35bd1 | 242 | _m1dir=1; |
12104404 | 27:fb1298d35bd1 | 243 | _m2dir=1; |
alaurens | 31:69caabc10879 | 244 | setMotors12(compG(0.2,_m1dir,angle),compG(0.2,_m2dir,angle)); |
12104404 | 27:fb1298d35bd1 | 245 | } |
12104404 | 27:fb1298d35bd1 | 246 | |
12104404 | 27:fb1298d35bd1 | 247 | void LOCOMOTION::moveB(void) |
12104404 | 27:fb1298d35bd1 | 248 | { |
12104404 | 27:fb1298d35bd1 | 249 | _m1dir=1; |
12104404 | 27:fb1298d35bd1 | 250 | _m2dir=1; |
12104404 | 27:fb1298d35bd1 | 251 | setMotors(0.15) ; |
12104404 | 27:fb1298d35bd1 | 252 | } |
12104404 | 27:fb1298d35bd1 | 253 |