Antoine Laurens / Mbed 2 deprecated DEMO2

uhbduhbv

Dependencies:   BNO055_fusion mbed

Fork of DEMO3 by Edwin Cho

LOCOMOTION.cpp@19:5832e34b7533, 2016-03-29 (annotated)

Committer:
alaurens
Date:
Tue Mar 29 21:58:26 2016 +0000
Revision:
19:5832e34b7533
Parent:
18:f9012e93edb8 
everything is beautiful

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 6:0602a9e8118b 1 #include "LOCOMOTION.h"
12104404 15:7729da55873a 2
12104404 15:7729da55873a 3 LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
12104404 16:d6f15a13c3aa 4 _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
12104404 15:7729da55873a 5 {
12104404 15:7729da55873a 6 _m1f=0;
12104404 15:7729da55873a 7 _m1b=0;
12104404 15:7729da55873a 8 _m2f=0;
12104404 15:7729da55873a 9 _m2b=0;
12104404 15:7729da55873a 10 _m1dir=0;
12104404 15:7729da55873a 11 _m2dir=0;
12104404 15:7729da55873a 12 }
12104404 15:7729da55873a 13
12104404 18:f9012e93edb8 14 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
12104404 17:2f89826b5679 15 {
12104404 17:2f89826b5679 16 //s = 0;
12104404 17:2f89826b5679 17 if(abs(current-target)<=error)
12104404 17:2f89826b5679 18 s=0.07;
12104404 17:2f89826b5679 19 else
12104404 17:2f89826b5679 20 s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
12104404 17:2f89826b5679 21 if(current>target+error) {
12104404 18:f9012e93edb8 22 if(angle==0) {
12104404 18:f9012e93edb8 23 _m1dir=1;
12104404 18:f9012e93edb8 24 _m2dir=1;
12104404 18:f9012e93edb8 25 } else {
12104404 18:f9012e93edb8 26 _m1dir=0;
12104404 18:f9012e93edb8 27 _m2dir=0;
12104404 18:f9012e93edb8 28 }
12104404 17:2f89826b5679 29 _m1f=s;
12104404 17:2f89826b5679 30 _m1b=s;
12104404 17:2f89826b5679 31 _m2f=s;
12104404 17:2f89826b5679 32 _m2b=s;
12104404 17:2f89826b5679 33 } else if(current<target-error) {
12104404 18:f9012e93edb8 34 if(angle==0) {
12104404 18:f9012e93edb8 35 _m1dir=0;
12104404 18:f9012e93edb8 36 _m2dir=0;
12104404 18:f9012e93edb8 37 } else {
12104404 18:f9012e93edb8 38 _m1dir=1;
12104404 18:f9012e93edb8 39 _m2dir=1;
12104404 18:f9012e93edb8 40 }
12104404 17:2f89826b5679 41 _m1f=s;
12104404 17:2f89826b5679 42 _m1b=s;
12104404 17:2f89826b5679 43 _m2f=s;
12104404 17:2f89826b5679 44 _m2b=s;
12104404 17:2f89826b5679 45 } else {
12104404 17:2f89826b5679 46 s=0;
12104404 17:2f89826b5679 47 _m1f=s;
12104404 17:2f89826b5679 48 _m1b=s;
12104404 17:2f89826b5679 49 _m2f=s;
12104404 17:2f89826b5679 50 _m2b=s;
12104404 17:2f89826b5679 51 return true;
12104404 17:2f89826b5679 52 }
12104404 17:2f89826b5679 53 return false;
12104404 17:2f89826b5679 54 }
12104404 17:2f89826b5679 55
12104404 18:f9012e93edb8 56 bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
12104404 17:2f89826b5679 57 {
12104404 17:2f89826b5679 58 //float s = 0;
12104404 17:2f89826b5679 59 if(abs(current-target)<=error)
12104404 17:2f89826b5679 60 s=0.50;
12104404 17:2f89826b5679 61 else
12104404 17:2f89826b5679 62 s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
12104404 17:2f89826b5679 63 if(current>target+error) {
12104404 17:2f89826b5679 64 //_m1dir=1;
12104404 17:2f89826b5679 65 //_m2dir=1;
12104404 18:f9012e93edb8 66 if(angle==0) {
12104404 18:f9012e93edb8 67 _m1f=_m1f*(1+s);
12104404 18:f9012e93edb8 68 _m1b=_m1b*(1+s);
12104404 18:f9012e93edb8 69 } else {
12104404 18:f9012e93edb8 70 _m2f=_m2f*(1+s);
12104404 18:f9012e93edb8 71 _m2b=_m2b*(1+s);
12104404 18:f9012e93edb8 72 }
12104404 17:2f89826b5679 73 } else if(current<target-error) {
12104404 17:2f89826b5679 74 //_m1dir=0;
12104404 17:2f89826b5679 75 //_m2dir=0;
12104404 18:f9012e93edb8 76 if(angle==0) {
12104404 18:f9012e93edb8 77 _m2f=_m2f*(1+s);
12104404 18:f9012e93edb8 78 _m2b=_m2b*(1+s);
12104404 18:f9012e93edb8 79 } else {
12104404 18:f9012e93edb8 80 _m1f=_m1f*(1+s);
12104404 18:f9012e93edb8 81 _m1b=_m1b*(1+s);
12104404 18:f9012e93edb8 82 }
12104404 17:2f89826b5679 83 } else {
12104404 17:2f89826b5679 84 s=0;
12104404 18:f9012e93edb8 85
12104404 17:2f89826b5679 86 return true;
12104404 17:2f89826b5679 87 }
12104404 17:2f89826b5679 88 return false;
12104404 17:2f89826b5679 89 }
12104404 17:2f89826b5679 90
12104404 15:7729da55873a 91 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
12104404 15:7729da55873a 92 {
12104404 17:2f89826b5679 93 s = 0;
12104404 15:7729da55873a 94 int diff = 0;
12104404 15:7729da55873a 95 diff = 180-wrap(target);
12104404 15:7729da55873a 96 if(abs(wrap(current+diff)-180)<=error)
12104404 15:7729da55873a 97 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 98 else
12104404 15:7729da55873a 99 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 100 switch(mode) {
12104404 15:7729da55873a 101 case ANGLE_TURN:
12104404 16:d6f15a13c3aa 102 if(wrap(current+diff)>180+error) {
12104404 16:d6f15a13c3aa 103 _m1dir=1;
12104404 16:d6f15a13c3aa 104 _m2dir=0;
12104404 16:d6f15a13c3aa 105 _m1f=s;
12104404 16:d6f15a13c3aa 106 _m1b=s;
12104404 16:d6f15a13c3aa 107 _m2f=s;
12104404 16:d6f15a13c3aa 108 _m2b=s;
12104404 16:d6f15a13c3aa 109 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 110 _m1dir=0;
12104404 16:d6f15a13c3aa 111 _m2dir=1;
12104404 16:d6f15a13c3aa 112 _m1f=s;
12104404 16:d6f15a13c3aa 113 _m1b=s;
12104404 16:d6f15a13c3aa 114 _m2f=s;
12104404 16:d6f15a13c3aa 115 _m2b=s;
12104404 16:d6f15a13c3aa 116 } else {
12104404 16:d6f15a13c3aa 117 s=0;
12104404 16:d6f15a13c3aa 118 _m1f=s;
12104404 16:d6f15a13c3aa 119 _m1b=s;
12104404 16:d6f15a13c3aa 120 _m2f=s;
12104404 16:d6f15a13c3aa 121 _m2b=s;
12104404 16:d6f15a13c3aa 122 return true;
12104404 16:d6f15a13c3aa 123 }
12104404 15:7729da55873a 124 break;
12104404 15:7729da55873a 125 case ANGLE_BIAS:
12104404 15:7729da55873a 126
12104404 15:7729da55873a 127 break;
12104404 15:7729da55873a 128 }
12104404 16:d6f15a13c3aa 129 return false;
12104404 15:7729da55873a 130 }
12104404 15:7729da55873a 131
12104404 15:7729da55873a 132 int LOCOMOTION::wrap(int num)
12104404 15:7729da55873a 133 {
12104404 15:7729da55873a 134 return num%360;
alaurens 19:5832e34b7533 135 }
alaurens 19:5832e34b7533 136 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal)
alaurens 19:5832e34b7533 137 {
alaurens 19:5832e34b7533 138 if (*xCurrState==x_increase|| *xCurrState==x_decrease) {
alaurens 19:5832e34b7533 139 if(angleGood && xGood) {
alaurens 19:5832e34b7533 140 *xCurrState=x_backwards;
alaurens 19:5832e34b7533 141
alaurens 19:5832e34b7533 142 }
alaurens 19:5832e34b7533 143 }
alaurens 19:5832e34b7533 144
alaurens 19:5832e34b7533 145 if(*xCurrState==x_backwards) {
alaurens 19:5832e34b7533 146 if(xGood && angleGood) {
alaurens 19:5832e34b7533 147 if(*angleGoal==0) {
alaurens 19:5832e34b7533 148 *xCurrState=x_increase;
alaurens 19:5832e34b7533 149 } else {
alaurens 19:5832e34b7533 150 *xCurrState=x_decrease;
alaurens 19:5832e34b7533 151 }
alaurens 19:5832e34b7533 152 }
alaurens 19:5832e34b7533 153 }
alaurens 19:5832e34b7533 154 switch(*xCurrState) {
alaurens 19:5832e34b7533 155 case x_increase:
alaurens 19:5832e34b7533 156 *angleGoal=180;
alaurens 19:5832e34b7533 157 *xTarget=xFarGoal;
alaurens 19:5832e34b7533 158 _m1dir=1;
alaurens 19:5832e34b7533 159 _m2dir=1;
alaurens 19:5832e34b7533 160 break;
alaurens 19:5832e34b7533 161
alaurens 19:5832e34b7533 162 case x_decrease:
alaurens 19:5832e34b7533 163 *angleGoal=0;
alaurens 19:5832e34b7533 164 *xTarget=xNearGoal;
alaurens 19:5832e34b7533 165 _m1dir=1;
alaurens 19:5832e34b7533 166 _m2dir=1;
alaurens 19:5832e34b7533 167 break;
alaurens 19:5832e34b7533 168
alaurens 19:5832e34b7533 169 case x_backwards:
alaurens 19:5832e34b7533 170 if (*angleGoal==0) {
alaurens 19:5832e34b7533 171 *xTarget=xNearGoal+backoff;
alaurens 19:5832e34b7533 172 } else {
alaurens 19:5832e34b7533 173 *xTarget=xNearGoal-backoff;
alaurens 19:5832e34b7533 174 }
alaurens 19:5832e34b7533 175 _m1dir=0;
alaurens 19:5832e34b7533 176 _m2dir=0;
alaurens 19:5832e34b7533 177 break;
alaurens 19:5832e34b7533 178 }
alaurens 19:5832e34b7533 179 }
alaurens 19:5832e34b7533 180
alaurens 19:5832e34b7533 181 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
alaurens 19:5832e34b7533 182 {
alaurens 19:5832e34b7533 183 if (xGood) {
alaurens 19:5832e34b7533 184 *angleTol=2;
alaurens 19:5832e34b7533 185 } else if(yGood) {
alaurens 19:5832e34b7533 186 *angleTol=2;
alaurens 19:5832e34b7533 187 } else {
alaurens 19:5832e34b7533 188 *angleTol=10;
alaurens 19:5832e34b7533 189 }
alaurens 19:5832e34b7533 190 }
alaurens 19:5832e34b7533 191
alaurens 19:5832e34b7533 192 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
alaurens 19:5832e34b7533 193 {
alaurens 19:5832e34b7533 194 if (xGood && angleGood && yGood) {
alaurens 19:5832e34b7533 195 *yTarget+=yIncrement;
alaurens 19:5832e34b7533 196 }
alaurens 19:5832e34b7533 197 }
alaurens 19:5832e34b7533 198
alaurens 19:5832e34b7533 199 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)
alaurens 19:5832e34b7533 200 {
alaurens 19:5832e34b7533 201 *xGood=abs(xya.x-xGoal)<xErr;
alaurens 19:5832e34b7533 202 *yGood=abs(xya.y-yGoal)<yErr;
alaurens 19:5832e34b7533 203 *angleGood=abs(xya.a-angleGoal)<yErr;
12104404 17:2f89826b5679 204 }