Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

LOCOMOTION.cpp@27:fb1298d35bd1, 2016-04-13 (annotated)

Committer:
12104404
Date:
Wed Apr 13 03:14:43 2016 +0000
Revision:
27:fb1298d35bd1
Parent:
26:0ea6a550a99d
Child:
28:65daccc10f31
gravity compensation;

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