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@34:083073e54dbd, 2016-04-20 (annotated)

Committer:
alaurens
Date:
Wed Apr 20 18:25:43 2016 +0000
Revision:
34:083073e54dbd
Parent:
33:baf24c59affc 
df

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 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) {
alaurens 34:083073e54dbd 60 if(angle==0+TILTZZ) {
alaurens 34:083073e54dbd 61 _m1dir=0;
alaurens 34:083073e54dbd 62 _m2dir=0;
alaurens 34:083073e54dbd 63 } else {
12104404 18:f9012e93edb8 64 _m1dir=1;
12104404 18:f9012e93edb8 65 _m2dir=1;
12104404 18:f9012e93edb8 66 }
12104404 25:f3bf8351bbd4 67 setMotors(s);
12104404 17:2f89826b5679 68 } else if(current<target-error) {
alaurens 34:083073e54dbd 69 if(angle==0+TILTZZ) {
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 }
alaurens 31:69caabc10879 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
alaurens 31:69caabc10879 89 s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_HE)+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 }
alaurens 31:69caabc10879 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 15:7729da55873a 127 case ANGLE_TURN:
12104404 16:d6f15a13c3aa 128 if(wrap(current+diff)>180+error) {
12104404 29:e8ef4a2e628d 129 _m1dir=0;
12104404 29:e8ef4a2e628d 130 _m2dir=1;
12104404 29:e8ef4a2e628d 131 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
12104404 30:116cd143fdf7 132 //_m2dir=0;
12104404 29:e8ef4a2e628d 133 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 134 _m1dir=1;
12104404 16:d6f15a13c3aa 135 _m2dir=0;
12104404 27:fb1298d35bd1 136 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
12104404 30:116cd143fdf7 137 //_m2dir=1;
12104404 16:d6f15a13c3aa 138 } else {
12104404 25:f3bf8351bbd4 139 setMotors(0);
12104404 16:d6f15a13c3aa 140 return true;
12104404 16:d6f15a13c3aa 141 }
12104404 15:7729da55873a 142 break;
12104404 25:f3bf8351bbd4 143 }
12104404 25:f3bf8351bbd4 144 return false;
12104404 25:f3bf8351bbd4 145 }
alaurens 31:69caabc10879 146
alaurens 31:69caabc10879 147 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget)
12104404 25:f3bf8351bbd4 148 {
alaurens 31:69caabc10879 149 if (yTarget>=FRAME_HE-ROB_SIZE/2 && xGood) {
alaurens 31:69caabc10879 150 *xCurrState=X_STOP;
12104404 25:f3bf8351bbd4 151 }
alaurens 31:69caabc10879 152 if (*xCurrState==X_INCREASE) {
alaurens 31:69caabc10879 153 if(angleGood && xGood) {
12104404 26:0ea6a550a99d 154 *xCurrState=X_DECREASE;
12104404 26:0ea6a550a99d 155 wait(0.5);
12104404 26:0ea6a550a99d 156 }
12104404 26:0ea6a550a99d 157 }
alaurens 33:baf24c59affc 158 else if (*xCurrState==X_DECREASE) {
alaurens 31:69caabc10879 159 if(angleGood && xGood) {
alaurens 31:69caabc10879 160 *xCurrState=X_INCREASE;
alaurens 31:69caabc10879 161 wait(0.5);
alaurens 31:69caabc10879 162 }
alaurens 31:69caabc10879 163
alaurens 31:69caabc10879 164 }
alaurens 31:69caabc10879 165
12104404 25:f3bf8351bbd4 166 switch(*xCurrState) {
alaurens 31:69caabc10879 167
12104404 25:f3bf8351bbd4 168 case X_INCREASE:
alaurens 34:083073e54dbd 169 *angleGoal=0+TILTZZ;
alaurens 33:baf24c59affc 170 _m1dir=0;
alaurens 33:baf24c59affc 171 _m2dir=0;
alaurens 31:69caabc10879 172 *xTarget=X_FAR_GOAL;
12104404 25:f3bf8351bbd4 173 break;
alaurens 31:69caabc10879 174
alaurens 31:69caabc10879 175 case X_DECREASE:
alaurens 34:083073e54dbd 176 *angleGoal=0-TILTZZ;
alaurens 33:baf24c59affc 177 _m1dir=1;
alaurens 33:baf24c59affc 178 _m2dir=1;
alaurens 31:69caabc10879 179 *xTarget=X_NEAR_GOAL;
12104404 15:7729da55873a 180 break;
alaurens 31:69caabc10879 181
12104404 15:7729da55873a 182 }
12104404 25:f3bf8351bbd4 183 }
12104404 25:f3bf8351bbd4 184 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
12104404 25:f3bf8351bbd4 185 {
12104404 25:f3bf8351bbd4 186 if (xGood||yGood) {
12104404 25:f3bf8351bbd4 187 *angleTol=2;
12104404 25:f3bf8351bbd4 188 } else {
12104404 25:f3bf8351bbd4 189 *angleTol=10;
12104404 25:f3bf8351bbd4 190 }
12104404 25:f3bf8351bbd4 191 }
alaurens 31:69caabc10879 192
12104404 25:f3bf8351bbd4 193 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
12104404 25:f3bf8351bbd4 194 {
alaurens 31:69caabc10879 195 if (yGood && xGood) {
12104404 25:f3bf8351bbd4 196 *yTarget+=Y_INCREMENT;
12104404 25:f3bf8351bbd4 197 }
alaurens 31:69caabc10879 198 if (*yTarget>(FRAME_HE-ROB_SIZE/2)) {
alaurens 31:69caabc10879 199 *yTarget=FRAME_HE-ROB_SIZE/2;
alaurens 31:69caabc10879 200 }
12104404 25:f3bf8351bbd4 201 }
alaurens 31:69caabc10879 202
12104404 25:f3bf8351bbd4 203 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 204 {
12104404 25:f3bf8351bbd4 205 *xGood=abs(xya.x-xGoal)<xErr;
12104404 25:f3bf8351bbd4 206 *yGood=abs(xya.y-yGoal)<yErr;
12104404 26:0ea6a550a99d 207 *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
12104404 26:0ea6a550a99d 208 }
alaurens 31:69caabc10879 209
12104404 26:0ea6a550a99d 210 void LOCOMOTION::mStop(void)
12104404 26:0ea6a550a99d 211 {
12104404 26:0ea6a550a99d 212 setMotors(0);
12104404 27:fb1298d35bd1 213 }
12104404 27:fb1298d35bd1 214 void LOCOMOTION::moveF(int angle)
12104404 27:fb1298d35bd1 215 {
12104404 27:fb1298d35bd1 216 _m1dir=1;
12104404 27:fb1298d35bd1 217 _m2dir=1;
alaurens 31:69caabc10879 218 setMotors12(compG(0.2,_m1dir,angle),compG(0.2,_m2dir,angle));
12104404 27:fb1298d35bd1 219 }
12104404 27:fb1298d35bd1 220
12104404 27:fb1298d35bd1 221 void LOCOMOTION::moveB(void)
12104404 27:fb1298d35bd1 222 {
12104404 27:fb1298d35bd1 223 _m1dir=1;
12104404 27:fb1298d35bd1 224 _m2dir=1;
12104404 27:fb1298d35bd1 225 setMotors(0.15) ;
12104404 27:fb1298d35bd1 226 }
12104404 27:fb1298d35bd1 227