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@40:9a97c4403c0a, 2016-05-03 (annotated)

Committer:
12104404
Date:
Tue May 03 19:17:04 2016 +0000
Revision:
40:9a97c4403c0a
Parent:
38:16208e003dc9 
WORKING;

Who changed what in which revision?

UserRevisionLine numberNew 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