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@35:68917796c30a, 2016-04-24 (annotated)

Committer:
12104404
Date:
Sun Apr 24 22:37:07 2016 +0000
Revision:
35:68917796c30a
Parent:
34:083073e54dbd
Child:
36:dc69442c4c47
angle bias;

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