Edwin Cho / Mbed 2 deprecated FLOW_DERP

DERPS

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of STRAIGHT_DRIVE_NO_SEP by Antoine Laurens

LOCOMOTION.cpp@30:116cd143fdf7, 2016-04-19 (annotated)

Committer:
12104404
Date:
Tue Apr 19 02:04:10 2016 +0000
Revision:
30:116cd143fdf7
Parent:
29:e8ef4a2e628d
Child:
31:69caabc10879
explode

Who changed what in which revision?

UserRevisionLine numberNew 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 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 }
12104404 26:0ea6a550a99d 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 }
12104404 26:0ea6a550a99d 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 }
12104404 26:0ea6a550a99d 52
12104404 18:f9012e93edb8 53 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
12104404 17:2f89826b5679 54 {
12104404 17:2f89826b5679 55 if(abs(current-target)<=error)
12104404 25:f3bf8351bbd4 56 s=SPEED_X_MIN;
12104404 17:2f89826b5679 57 else
12104404 25:f3bf8351bbd4 58 s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
12104404 17:2f89826b5679 59 if(current>target+error) {
12104404 18:f9012e93edb8 60 if(angle==0) {
12104404 18:f9012e93edb8 61 _m1dir=1;
12104404 18:f9012e93edb8 62 _m2dir=1;
12104404 18:f9012e93edb8 63 } else {
12104404 18:f9012e93edb8 64 _m1dir=0;
12104404 18:f9012e93edb8 65 _m2dir=0;
12104404 18:f9012e93edb8 66 }
12104404 25:f3bf8351bbd4 67 setMotors(s);
12104404 17:2f89826b5679 68 } else if(current<target-error) {
12104404 18:f9012e93edb8 69 if(angle==0) {
12104404 18:f9012e93edb8 70 _m1dir=0;
12104404 18:f9012e93edb8 71 _m2dir=0;
12104404 18:f9012e93edb8 72 } else {
12104404 18:f9012e93edb8 73 _m1dir=1;
12104404 18:f9012e93edb8 74 _m2dir=1;
12104404 18:f9012e93edb8 75 }
12104404 25:f3bf8351bbd4 76 setMotors(s);
12104404 17:2f89826b5679 77 } else {
12104404 25:f3bf8351bbd4 78 setMotors(0);
12104404 17:2f89826b5679 79 return true;
12104404 17:2f89826b5679 80 }
12104404 17:2f89826b5679 81 return false;
12104404 17:2f89826b5679 82 }
12104404 26:0ea6a550a99d 83
12104404 25:f3bf8351bbd4 84 bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
12104404 17:2f89826b5679 85 {
12104404 17:2f89826b5679 86 if(abs(current-target)<=error)
12104404 25:f3bf8351bbd4 87 s=Y_BIAS_MIN;
12104404 17:2f89826b5679 88 else
12104404 25:f3bf8351bbd4 89 s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN;
12104404 17:2f89826b5679 90 if(current>target+error) {
12104404 17:2f89826b5679 91 //_m1dir=1;
12104404 17:2f89826b5679 92 //_m2dir=1;
12104404 18:f9012e93edb8 93 if(angle==0) {
12104404 18:f9012e93edb8 94 _m1f=_m1f*(1+s);
12104404 18:f9012e93edb8 95 _m1b=_m1b*(1+s);
12104404 18:f9012e93edb8 96 } else {
12104404 18:f9012e93edb8 97 _m2f=_m2f*(1+s);
12104404 18:f9012e93edb8 98 _m2b=_m2b*(1+s);
12104404 18:f9012e93edb8 99 }
12104404 17:2f89826b5679 100 } else if(current<target-error) {
12104404 17:2f89826b5679 101 //_m1dir=0;
12104404 17:2f89826b5679 102 //_m2dir=0;
12104404 18:f9012e93edb8 103 if(angle==0) {
12104404 18:f9012e93edb8 104 _m2f=_m2f*(1+s);
12104404 18:f9012e93edb8 105 _m2b=_m2b*(1+s);
12104404 18:f9012e93edb8 106 } else {
12104404 18:f9012e93edb8 107 _m1f=_m1f*(1+s);
12104404 18:f9012e93edb8 108 _m1b=_m1b*(1+s);
12104404 18:f9012e93edb8 109 }
12104404 17:2f89826b5679 110 } else {
12104404 17:2f89826b5679 111 s=0;
12104404 17:2f89826b5679 112 return true;
12104404 17:2f89826b5679 113 }
12104404 17:2f89826b5679 114 return false;
12104404 17:2f89826b5679 115 }
12104404 26:0ea6a550a99d 116
12104404 15:7729da55873a 117 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
12104404 15:7729da55873a 118 {
12104404 17:2f89826b5679 119 s = 0;
12104404 15:7729da55873a 120 int diff = 0;
12104404 15:7729da55873a 121 diff = 180-wrap(target);
12104404 15:7729da55873a 122 if(abs(wrap(current+diff)-180)<=error)
12104404 15:7729da55873a 123 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 124 else
12104404 15:7729da55873a 125 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 126 switch(mode) {
12104404 25:f3bf8351bbd4 127 default:
12104404 15:7729da55873a 128 case ANGLE_TURN:
12104404 16:d6f15a13c3aa 129 if(wrap(current+diff)>180+error) {
12104404 29:e8ef4a2e628d 130 _m1dir=0;
12104404 29:e8ef4a2e628d 131 _m2dir=1;
12104404 29:e8ef4a2e628d 132 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
12104404 30:116cd143fdf7 133 //_m2dir=0;
12104404 29:e8ef4a2e628d 134 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 135 _m1dir=1;
12104404 16:d6f15a13c3aa 136 _m2dir=0;
12104404 27:fb1298d35bd1 137 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
12104404 30:116cd143fdf7 138 //_m2dir=1;
12104404 16:d6f15a13c3aa 139 } else {
12104404 25:f3bf8351bbd4 140 setMotors(0);
12104404 16:d6f15a13c3aa 141 return true;
12104404 16:d6f15a13c3aa 142 }
12104404 15:7729da55873a 143 break;
12104404 25:f3bf8351bbd4 144 }
12104404 25:f3bf8351bbd4 145 return false;
12104404 25:f3bf8351bbd4 146 }
12104404 26:0ea6a550a99d 147
12104404 26:0ea6a550a99d 148 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
12104404 25:f3bf8351bbd4 149 {
12104404 25:f3bf8351bbd4 150 if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
12104404 25:f3bf8351bbd4 151 if(angleGood && xGood) {
12104404 25:f3bf8351bbd4 152 *xCurrState=X_BACKWARDS;
12104404 26:0ea6a550a99d 153 wait(0.5);
12104404 26:0ea6a550a99d 154
12104404 25:f3bf8351bbd4 155 }
12104404 25:f3bf8351bbd4 156 }
12104404 26:0ea6a550a99d 157
12104404 26:0ea6a550a99d 158 else if(xGood && angleGood && *xCurrState==X_BACKWARDS) {
12104404 26:0ea6a550a99d 159 if(*angleGoal==0) {
12104404 26:0ea6a550a99d 160 *xCurrState=ROTATE_1;
12104404 26:0ea6a550a99d 161 wait(0.5);
12104404 26:0ea6a550a99d 162 } else {
12104404 26:0ea6a550a99d 163 *xCurrState=ROTATE_2;
12104404 26:0ea6a550a99d 164 wait(0.5);
12104404 25:f3bf8351bbd4 165 }
12104404 25:f3bf8351bbd4 166 }
12104404 26:0ea6a550a99d 167
12104404 26:0ea6a550a99d 168 else if (xGood && angleGood) {
12104404 26:0ea6a550a99d 169 if (*xCurrState==ROTATE_1) {
12104404 26:0ea6a550a99d 170 *xCurrState=X_INCREASE;
12104404 26:0ea6a550a99d 171 wait(0.5);
12104404 26:0ea6a550a99d 172 } else {
12104404 26:0ea6a550a99d 173 *xCurrState=X_DECREASE;
12104404 26:0ea6a550a99d 174 wait(0.5);
12104404 26:0ea6a550a99d 175 }
12104404 26:0ea6a550a99d 176 }
12104404 26:0ea6a550a99d 177
12104404 25:f3bf8351bbd4 178 switch(*xCurrState) {
12104404 25:f3bf8351bbd4 179 case X_INCREASE:
12104404 25:f3bf8351bbd4 180 *angleGoal=180;
12104404 25:f3bf8351bbd4 181 *xTarget=X_FAR_GOAL;
12104404 25:f3bf8351bbd4 182 _m1dir=1;
12104404 25:f3bf8351bbd4 183 _m2dir=1;
12104404 25:f3bf8351bbd4 184 break;
12104404 26:0ea6a550a99d 185
12104404 25:f3bf8351bbd4 186 case X_DECREASE:
12104404 25:f3bf8351bbd4 187 *angleGoal=0;
12104404 25:f3bf8351bbd4 188 *xTarget=X_NEAR_GOAL;
12104404 26:0ea6a550a99d 189 //_m1dir=1;
12104404 26:0ea6a550a99d 190 //_m2dir=1;
12104404 25:f3bf8351bbd4 191 break;
12104404 26:0ea6a550a99d 192
12104404 25:f3bf8351bbd4 193 case X_BACKWARDS:
12104404 25:f3bf8351bbd4 194 if (*angleGoal==0) {
12104404 25:f3bf8351bbd4 195 *xTarget=X_NEAR_GOAL+BACKOFF;
12104404 25:f3bf8351bbd4 196 } else {
12104404 25:f3bf8351bbd4 197 *xTarget=X_FAR_GOAL-BACKOFF;
12104404 25:f3bf8351bbd4 198 }
12104404 26:0ea6a550a99d 199 //_m1dir=0;
12104404 26:0ea6a550a99d 200 //_m2dir=0;
12104404 15:7729da55873a 201 break;
12104404 26:0ea6a550a99d 202
12104404 26:0ea6a550a99d 203 case ROTATE_1:
12104404 26:0ea6a550a99d 204 *xTarget=*xTarget;
12104404 26:0ea6a550a99d 205 *angleTol=2;
12104404 26:0ea6a550a99d 206 *angleGoal=180;
12104404 26:0ea6a550a99d 207 break;
12104404 26:0ea6a550a99d 208
12104404 26:0ea6a550a99d 209 case ROTATE_2:
12104404 26:0ea6a550a99d 210 *xTarget=*xTarget;
12104404 26:0ea6a550a99d 211 *angleTol=2;
12104404 26:0ea6a550a99d 212 *angleGoal=0;
12104404 26:0ea6a550a99d 213 break;
12104404 26:0ea6a550a99d 214
12104404 15:7729da55873a 215 }
12104404 25:f3bf8351bbd4 216 }
12104404 25:f3bf8351bbd4 217 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
12104404 25:f3bf8351bbd4 218 {
12104404 25:f3bf8351bbd4 219 if (xGood||yGood) {
12104404 25:f3bf8351bbd4 220 *angleTol=2;
12104404 25:f3bf8351bbd4 221 } else {
12104404 25:f3bf8351bbd4 222 *angleTol=10;
12104404 25:f3bf8351bbd4 223 }
12104404 25:f3bf8351bbd4 224 }
12104404 26:0ea6a550a99d 225
12104404 25:f3bf8351bbd4 226 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
12104404 25:f3bf8351bbd4 227 {
12104404 26:0ea6a550a99d 228 if (yGood) {
12104404 25:f3bf8351bbd4 229 *yTarget+=Y_INCREMENT;
12104404 25:f3bf8351bbd4 230 }
12104404 25:f3bf8351bbd4 231 }
12104404 26:0ea6a550a99d 232
12104404 25:f3bf8351bbd4 233 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 234 {
12104404 25:f3bf8351bbd4 235 *xGood=abs(xya.x-xGoal)<xErr;
12104404 25:f3bf8351bbd4 236 *yGood=abs(xya.y-yGoal)<yErr;
12104404 26:0ea6a550a99d 237 *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
12104404 26:0ea6a550a99d 238 }
12104404 26:0ea6a550a99d 239
12104404 26:0ea6a550a99d 240 void LOCOMOTION::mStop(void)
12104404 26:0ea6a550a99d 241 {
12104404 26:0ea6a550a99d 242 setMotors(0);
12104404 27:fb1298d35bd1 243 }
12104404 27:fb1298d35bd1 244 void LOCOMOTION::moveF(int angle)
12104404 27:fb1298d35bd1 245 {
12104404 27:fb1298d35bd1 246 _m1dir=1;
12104404 27:fb1298d35bd1 247 _m2dir=1;
12104404 30:116cd143fdf7 248 setMotors12(compG(0.2,_m1dir,angle),compG(0.2,_m2dir,angle));
12104404 27:fb1298d35bd1 249 }
12104404 27:fb1298d35bd1 250
12104404 27:fb1298d35bd1 251 void LOCOMOTION::moveB(void)
12104404 27:fb1298d35bd1 252 {
12104404 27:fb1298d35bd1 253 _m1dir=1;
12104404 27:fb1298d35bd1 254 _m2dir=1;
12104404 27:fb1298d35bd1 255 setMotors(0.15) ;
12104404 27:fb1298d35bd1 256 }
12104404 27:fb1298d35bd1 257