Antoine Laurens / Mbed 2 deprecated TEAM_G_FLOW_RIDA

added

Dependencies:   BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

LOCOMOTION.cpp@26:5ae5ef623b72, 2016-04-06 (annotated)

Committer:
alaurens
Date:
Wed Apr 06 03:54:29 2016 +0000
Revision:
26:5ae5ef623b72
Parent:
25:f3bf8351bbd4 
aa

Who changed what in which revision?

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