version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
LOCOMOTION.cpp@20:1da786e205eb, 2016-03-29 (annotated)
- Committer:
- alaurens
- Date:
- Tue Mar 29 22:49:05 2016 +0000
- Revision:
- 20:1da786e205eb
- Parent:
- 19:5832e34b7533
should theoretically work
Who changed what in which revision?
User | Revision | Line number | New 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 | 18:f9012e93edb8 | 64 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 65 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 66 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 67 | } else { |
12104404 | 18:f9012e93edb8 | 68 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 69 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 70 | } |
12104404 | 17:2f89826b5679 | 71 | } else if(current<target-error) { |
12104404 | 18:f9012e93edb8 | 72 | if(angle==0) { |
12104404 | 18:f9012e93edb8 | 73 | _m2f=_m2f*(1+s); |
12104404 | 18:f9012e93edb8 | 74 | _m2b=_m2b*(1+s); |
12104404 | 18:f9012e93edb8 | 75 | } else { |
12104404 | 18:f9012e93edb8 | 76 | _m1f=_m1f*(1+s); |
12104404 | 18:f9012e93edb8 | 77 | _m1b=_m1b*(1+s); |
12104404 | 18:f9012e93edb8 | 78 | } |
12104404 | 17:2f89826b5679 | 79 | } else { |
12104404 | 17:2f89826b5679 | 80 | s=0; |
12104404 | 18:f9012e93edb8 | 81 | |
12104404 | 17:2f89826b5679 | 82 | return true; |
12104404 | 17:2f89826b5679 | 83 | } |
12104404 | 17:2f89826b5679 | 84 | return false; |
12104404 | 17:2f89826b5679 | 85 | } |
12104404 | 17:2f89826b5679 | 86 | |
12104404 | 15:7729da55873a | 87 | bool LOCOMOTION::setAngle(int target, int current, int error, int mode) |
12104404 | 15:7729da55873a | 88 | { |
12104404 | 17:2f89826b5679 | 89 | s = 0; |
12104404 | 15:7729da55873a | 90 | int diff = 0; |
12104404 | 15:7729da55873a | 91 | diff = 180-wrap(target); |
12104404 | 15:7729da55873a | 92 | if(abs(wrap(current+diff)-180)<=error) |
12104404 | 15:7729da55873a | 93 | s=SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 94 | else |
12104404 | 15:7729da55873a | 95 | s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; |
12104404 | 15:7729da55873a | 96 | switch(mode) { |
12104404 | 15:7729da55873a | 97 | case ANGLE_TURN: |
12104404 | 16:d6f15a13c3aa | 98 | if(wrap(current+diff)>180+error) { |
12104404 | 16:d6f15a13c3aa | 99 | _m1dir=1; |
12104404 | 16:d6f15a13c3aa | 100 | _m2dir=0; |
12104404 | 16:d6f15a13c3aa | 101 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 102 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 103 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 104 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 105 | } else if(wrap(current+diff)<180-error) { |
12104404 | 16:d6f15a13c3aa | 106 | _m1dir=0; |
12104404 | 16:d6f15a13c3aa | 107 | _m2dir=1; |
12104404 | 16:d6f15a13c3aa | 108 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 109 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 110 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 111 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 112 | } else { |
12104404 | 16:d6f15a13c3aa | 113 | s=0; |
12104404 | 16:d6f15a13c3aa | 114 | _m1f=s; |
12104404 | 16:d6f15a13c3aa | 115 | _m1b=s; |
12104404 | 16:d6f15a13c3aa | 116 | _m2f=s; |
12104404 | 16:d6f15a13c3aa | 117 | _m2b=s; |
12104404 | 16:d6f15a13c3aa | 118 | return true; |
12104404 | 16:d6f15a13c3aa | 119 | } |
12104404 | 15:7729da55873a | 120 | break; |
12104404 | 15:7729da55873a | 121 | case ANGLE_BIAS: |
12104404 | 15:7729da55873a | 122 | |
12104404 | 15:7729da55873a | 123 | break; |
12104404 | 15:7729da55873a | 124 | } |
12104404 | 16:d6f15a13c3aa | 125 | return false; |
12104404 | 15:7729da55873a | 126 | } |
12104404 | 15:7729da55873a | 127 | |
12104404 | 15:7729da55873a | 128 | int LOCOMOTION::wrap(int num) |
12104404 | 15:7729da55873a | 129 | { |
12104404 | 15:7729da55873a | 130 | return num%360; |
alaurens | 19:5832e34b7533 | 131 | } |
alaurens | 19:5832e34b7533 | 132 | void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal) |
alaurens | 19:5832e34b7533 | 133 | { |
alaurens | 20:1da786e205eb | 134 | if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) { |
alaurens | 19:5832e34b7533 | 135 | if(angleGood && xGood) { |
alaurens | 20:1da786e205eb | 136 | *xCurrState=X_BACKWARDS; |
alaurens | 19:5832e34b7533 | 137 | |
alaurens | 19:5832e34b7533 | 138 | } |
alaurens | 19:5832e34b7533 | 139 | } |
alaurens | 19:5832e34b7533 | 140 | |
alaurens | 20:1da786e205eb | 141 | if(*xCurrState==X_BACKWARDS) { |
alaurens | 19:5832e34b7533 | 142 | if(xGood && angleGood) { |
alaurens | 19:5832e34b7533 | 143 | if(*angleGoal==0) { |
alaurens | 20:1da786e205eb | 144 | *xCurrState=X_INCREASE; |
alaurens | 19:5832e34b7533 | 145 | } else { |
alaurens | 20:1da786e205eb | 146 | *xCurrState=X_DECREASE; |
alaurens | 19:5832e34b7533 | 147 | } |
alaurens | 19:5832e34b7533 | 148 | } |
alaurens | 19:5832e34b7533 | 149 | } |
alaurens | 19:5832e34b7533 | 150 | switch(*xCurrState) { |
alaurens | 20:1da786e205eb | 151 | case X_INCREASE: |
alaurens | 19:5832e34b7533 | 152 | *angleGoal=180; |
alaurens | 20:1da786e205eb | 153 | *xTarget=X_FAR_GOAL; |
alaurens | 19:5832e34b7533 | 154 | _m1dir=1; |
alaurens | 19:5832e34b7533 | 155 | _m2dir=1; |
alaurens | 19:5832e34b7533 | 156 | break; |
alaurens | 19:5832e34b7533 | 157 | |
alaurens | 20:1da786e205eb | 158 | case X_DECREASE: |
alaurens | 19:5832e34b7533 | 159 | *angleGoal=0; |
alaurens | 20:1da786e205eb | 160 | *xTarget=X_NEAR_GOAL; |
alaurens | 19:5832e34b7533 | 161 | _m1dir=1; |
alaurens | 19:5832e34b7533 | 162 | _m2dir=1; |
alaurens | 19:5832e34b7533 | 163 | break; |
alaurens | 19:5832e34b7533 | 164 | |
alaurens | 20:1da786e205eb | 165 | case X_BACKWARDS: |
alaurens | 19:5832e34b7533 | 166 | if (*angleGoal==0) { |
alaurens | 20:1da786e205eb | 167 | *xTarget=X_NEAR_GOAL+BACKOFF; |
alaurens | 19:5832e34b7533 | 168 | } else { |
alaurens | 20:1da786e205eb | 169 | *xTarget=X_FAR_GOAL-BACKOFF; |
alaurens | 19:5832e34b7533 | 170 | } |
alaurens | 19:5832e34b7533 | 171 | _m1dir=0; |
alaurens | 19:5832e34b7533 | 172 | _m2dir=0; |
alaurens | 19:5832e34b7533 | 173 | break; |
alaurens | 19:5832e34b7533 | 174 | } |
alaurens | 19:5832e34b7533 | 175 | } |
alaurens | 19:5832e34b7533 | 176 | |
alaurens | 19:5832e34b7533 | 177 | void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood) |
alaurens | 19:5832e34b7533 | 178 | { |
alaurens | 20:1da786e205eb | 179 | if (xGood||yGood) { |
alaurens | 19:5832e34b7533 | 180 | *angleTol=2; |
alaurens | 19:5832e34b7533 | 181 | } else { |
alaurens | 19:5832e34b7533 | 182 | *angleTol=10; |
alaurens | 19:5832e34b7533 | 183 | } |
alaurens | 19:5832e34b7533 | 184 | } |
alaurens | 19:5832e34b7533 | 185 | |
alaurens | 19:5832e34b7533 | 186 | void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget) |
alaurens | 19:5832e34b7533 | 187 | { |
alaurens | 19:5832e34b7533 | 188 | if (xGood && angleGood && yGood) { |
alaurens | 20:1da786e205eb | 189 | *yTarget+=Y_INCREMENT; |
alaurens | 19:5832e34b7533 | 190 | } |
alaurens | 19:5832e34b7533 | 191 | } |
alaurens | 19:5832e34b7533 | 192 | |
alaurens | 19:5832e34b7533 | 193 | 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 | 194 | { |
alaurens | 19:5832e34b7533 | 195 | *xGood=abs(xya.x-xGoal)<xErr; |
alaurens | 19:5832e34b7533 | 196 | *yGood=abs(xya.y-yGoal)<yErr; |
alaurens | 19:5832e34b7533 | 197 | *angleGood=abs(xya.a-angleGoal)<yErr; |
12104404 | 17:2f89826b5679 | 198 | } |