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@28:65daccc10f31, 2016-04-13 (annotated)

Committer:
12104404
Date:
Wed Apr 13 07:19:18 2016 +0000
Revision:
28:65daccc10f31
Parent:
27:fb1298d35bd1
Child:
29:e8ef4a2e628d
ultra working p4

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 28:65daccc10f31 35 return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
12104404 28:65daccc10f31 36 else
12104404 28:65daccc10f31 37 return -1*(cos((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 16:d6f15a13c3aa 130 _m1dir=1;
12104404 16:d6f15a13c3aa 131 _m2dir=0;
12104404 27:fb1298d35bd1 132 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
12104404 16:d6f15a13c3aa 133 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 134 _m1dir=0;
12104404 16:d6f15a13c3aa 135 _m2dir=1;
12104404 27:fb1298d35bd1 136 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
12104404 16:d6f15a13c3aa 137 } else {
12104404 25:f3bf8351bbd4 138 setMotors(0);
12104404 16:d6f15a13c3aa 139 return true;
12104404 16:d6f15a13c3aa 140 }
12104404 15:7729da55873a 141 break;
12104404 25:f3bf8351bbd4 142 }
12104404 25:f3bf8351bbd4 143 return false;
12104404 25:f3bf8351bbd4 144 }
12104404 26:0ea6a550a99d 145
12104404 26:0ea6a550a99d 146 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
12104404 25:f3bf8351bbd4 147 {
12104404 25:f3bf8351bbd4 148 if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
12104404 25:f3bf8351bbd4 149 if(angleGood && xGood) {
12104404 25:f3bf8351bbd4 150 *xCurrState=X_BACKWARDS;
12104404 26:0ea6a550a99d 151 wait(0.5);
12104404 26:0ea6a550a99d 152
12104404 25:f3bf8351bbd4 153 }
12104404 25:f3bf8351bbd4 154 }
12104404 26:0ea6a550a99d 155
12104404 26:0ea6a550a99d 156 else if(xGood && angleGood && *xCurrState==X_BACKWARDS) {
12104404 26:0ea6a550a99d 157 if(*angleGoal==0) {
12104404 26:0ea6a550a99d 158 *xCurrState=ROTATE_1;
12104404 26:0ea6a550a99d 159 wait(0.5);
12104404 26:0ea6a550a99d 160 } else {
12104404 26:0ea6a550a99d 161 *xCurrState=ROTATE_2;
12104404 26:0ea6a550a99d 162 wait(0.5);
12104404 25:f3bf8351bbd4 163 }
12104404 25:f3bf8351bbd4 164 }
12104404 26:0ea6a550a99d 165
12104404 26:0ea6a550a99d 166 else if (xGood && angleGood) {
12104404 26:0ea6a550a99d 167 if (*xCurrState==ROTATE_1) {
12104404 26:0ea6a550a99d 168 *xCurrState=X_INCREASE;
12104404 26:0ea6a550a99d 169 wait(0.5);
12104404 26:0ea6a550a99d 170 } else {
12104404 26:0ea6a550a99d 171 *xCurrState=X_DECREASE;
12104404 26:0ea6a550a99d 172 wait(0.5);
12104404 26:0ea6a550a99d 173 }
12104404 26:0ea6a550a99d 174 }
12104404 26:0ea6a550a99d 175
12104404 25:f3bf8351bbd4 176 switch(*xCurrState) {
12104404 25:f3bf8351bbd4 177 case X_INCREASE:
12104404 25:f3bf8351bbd4 178 *angleGoal=180;
12104404 25:f3bf8351bbd4 179 *xTarget=X_FAR_GOAL;
12104404 25:f3bf8351bbd4 180 _m1dir=1;
12104404 25:f3bf8351bbd4 181 _m2dir=1;
12104404 25:f3bf8351bbd4 182 break;
12104404 26:0ea6a550a99d 183
12104404 25:f3bf8351bbd4 184 case X_DECREASE:
12104404 25:f3bf8351bbd4 185 *angleGoal=0;
12104404 25:f3bf8351bbd4 186 *xTarget=X_NEAR_GOAL;
12104404 26:0ea6a550a99d 187 //_m1dir=1;
12104404 26:0ea6a550a99d 188 //_m2dir=1;
12104404 25:f3bf8351bbd4 189 break;
12104404 26:0ea6a550a99d 190
12104404 25:f3bf8351bbd4 191 case X_BACKWARDS:
12104404 25:f3bf8351bbd4 192 if (*angleGoal==0) {
12104404 25:f3bf8351bbd4 193 *xTarget=X_NEAR_GOAL+BACKOFF;
12104404 25:f3bf8351bbd4 194 } else {
12104404 25:f3bf8351bbd4 195 *xTarget=X_FAR_GOAL-BACKOFF;
12104404 25:f3bf8351bbd4 196 }
12104404 26:0ea6a550a99d 197 //_m1dir=0;
12104404 26:0ea6a550a99d 198 //_m2dir=0;
12104404 15:7729da55873a 199 break;
12104404 26:0ea6a550a99d 200
12104404 26:0ea6a550a99d 201 case ROTATE_1:
12104404 26:0ea6a550a99d 202 *xTarget=*xTarget;
12104404 26:0ea6a550a99d 203 *angleTol=2;
12104404 26:0ea6a550a99d 204 *angleGoal=180;
12104404 26:0ea6a550a99d 205 break;
12104404 26:0ea6a550a99d 206
12104404 26:0ea6a550a99d 207 case ROTATE_2:
12104404 26:0ea6a550a99d 208 *xTarget=*xTarget;
12104404 26:0ea6a550a99d 209 *angleTol=2;
12104404 26:0ea6a550a99d 210 *angleGoal=0;
12104404 26:0ea6a550a99d 211 break;
12104404 26:0ea6a550a99d 212
12104404 15:7729da55873a 213 }
12104404 25:f3bf8351bbd4 214 }
12104404 25:f3bf8351bbd4 215 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
12104404 25:f3bf8351bbd4 216 {
12104404 25:f3bf8351bbd4 217 if (xGood||yGood) {
12104404 25:f3bf8351bbd4 218 *angleTol=2;
12104404 25:f3bf8351bbd4 219 } else {
12104404 25:f3bf8351bbd4 220 *angleTol=10;
12104404 25:f3bf8351bbd4 221 }
12104404 25:f3bf8351bbd4 222 }
12104404 26:0ea6a550a99d 223
12104404 25:f3bf8351bbd4 224 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
12104404 25:f3bf8351bbd4 225 {
12104404 26:0ea6a550a99d 226 if (yGood) {
12104404 25:f3bf8351bbd4 227 *yTarget+=Y_INCREMENT;
12104404 25:f3bf8351bbd4 228 }
12104404 25:f3bf8351bbd4 229 }
12104404 26:0ea6a550a99d 230
12104404 25:f3bf8351bbd4 231 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 232 {
12104404 25:f3bf8351bbd4 233 *xGood=abs(xya.x-xGoal)<xErr;
12104404 25:f3bf8351bbd4 234 *yGood=abs(xya.y-yGoal)<yErr;
12104404 26:0ea6a550a99d 235 *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
12104404 26:0ea6a550a99d 236 }
12104404 26:0ea6a550a99d 237
12104404 26:0ea6a550a99d 238 void LOCOMOTION::mStop(void)
12104404 26:0ea6a550a99d 239 {
12104404 26:0ea6a550a99d 240 setMotors(0);
12104404 27:fb1298d35bd1 241 }
12104404 27:fb1298d35bd1 242 void LOCOMOTION::moveF(int angle)
12104404 27:fb1298d35bd1 243 {
12104404 27:fb1298d35bd1 244 _m1dir=1;
12104404 27:fb1298d35bd1 245 _m2dir=1;
12104404 27:fb1298d35bd1 246 setMotors12(compG(0.5,_m1dir,angle),compG(0.5,_m2dir,angle));
12104404 27:fb1298d35bd1 247 }
12104404 27:fb1298d35bd1 248
12104404 27:fb1298d35bd1 249 void LOCOMOTION::moveB(void)
12104404 27:fb1298d35bd1 250 {
12104404 27:fb1298d35bd1 251 _m1dir=1;
12104404 27:fb1298d35bd1 252 _m2dir=1;
12104404 27:fb1298d35bd1 253 setMotors(0.15) ;
12104404 27:fb1298d35bd1 254 }
12104404 27:fb1298d35bd1 255