3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp@6:5e24ff86b743, 2016-03-25 (annotated)
- Committer:
- j_j205
- Date:
- Fri Mar 25 19:51:11 2016 +0000
- Revision:
- 6:5e24ff86b743
- Parent:
- 5:4d5601a9dffe
- Child:
- 7:52c4be3bfc1b
3/25/16 2:50 JJ
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
j_j205 | 0:999bb8fcd0b2 | 1 | #include "scanner.h" |
j_j205 | 0:999bb8fcd0b2 | 2 | #include "mbed.h" |
j_j205 | 4:6a8d80954323 | 3 | #include "LongRangeSensor.h" |
j_j205 | 0:999bb8fcd0b2 | 4 | #include "VL6180x.h" |
j_j205 | 0:999bb8fcd0b2 | 5 | |
j_j205 | 0:999bb8fcd0b2 | 6 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 7 | // Scanner() |
j_j205 | 0:999bb8fcd0b2 | 8 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 9 | // Serial &pc1, PinName _servoL, PinName _servoR, |
j_j205 | 2:b281034eda86 | 10 | // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, |
j_j205 | 2:b281034eda86 | 11 | // Gp2x &_longRangeR, float _period = 10e-3 |
j_j205 | 0:999bb8fcd0b2 | 12 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 13 | // None |
j_j205 | 0:999bb8fcd0b2 | 14 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 15 | // Constructor. |
j_j205 | 2:b281034eda86 | 16 | Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR, |
j_j205 | 4:6a8d80954323 | 17 | VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL, |
j_j205 | 4:6a8d80954323 | 18 | LongRangeSensor &_longRangeR, float _period) : pc(pc1), servoL(_servoL), |
j_j205 | 2:b281034eda86 | 19 | servoR(_servoR), shortRangeL(_shortRangeL), |
j_j205 | 2:b281034eda86 | 20 | shortRangeR(_shortRangeR), longRangeL(_longRangeL), |
j_j205 | 0:999bb8fcd0b2 | 21 | longRangeR(_longRangeR), period(_period) |
j_j205 | 0:999bb8fcd0b2 | 22 | { |
j_j205 | 0:999bb8fcd0b2 | 23 | servoL.period(1.0/50.0); |
j_j205 | 0:999bb8fcd0b2 | 24 | servoR.period(1.0/50.0); |
j_j205 | 2:b281034eda86 | 25 | |
j_j205 | 6:5e24ff86b743 | 26 | float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925, |
j_j205 | 6:5e24ff86b743 | 27 | 0.1013, 0.1100}; // sub-micro |
j_j205 | 2:b281034eda86 | 28 | |
j_j205 | 6:5e24ff86b743 | 29 | for (int i = 0; i < 7; i++) |
j_j205 | 3:992558a021d7 | 30 | DUTY[i] = temp[i]; |
j_j205 | 3:992558a021d7 | 31 | |
j_j205 | 3:992558a021d7 | 32 | obstacle = 0; |
j_j205 | 3:992558a021d7 | 33 | distLeft = 0.0; |
j_j205 | 3:992558a021d7 | 34 | distRight = 0.0; |
j_j205 | 6:5e24ff86b743 | 35 | distForwardL = 0.0; |
j_j205 | 6:5e24ff86b743 | 36 | distForwardR = 0.0; |
j_j205 | 2:b281034eda86 | 37 | |
j_j205 | 2:b281034eda86 | 38 | // initializing to avoid status |
j_j205 | 3:992558a021d7 | 39 | avoidFlag = 1; |
j_j205 | 3:992558a021d7 | 40 | huntFlag = 0; |
j_j205 | 3:992558a021d7 | 41 | invertL = -1; |
j_j205 | 3:992558a021d7 | 42 | invertR = 1; |
j_j205 | 3:992558a021d7 | 43 | dutyL = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 44 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 45 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 46 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 47 | wait(0.2); |
j_j205 | 2:b281034eda86 | 48 | pitEnable = 1; |
j_j205 | 3:992558a021d7 | 49 | scanPit.attach(this, &Scanner::scan, period); |
j_j205 | 6:5e24ff86b743 | 50 | pc.printf("Scanner created\n"); |
j_j205 | 0:999bb8fcd0b2 | 51 | } |
j_j205 | 0:999bb8fcd0b2 | 52 | |
j_j205 | 0:999bb8fcd0b2 | 53 | // FUNCTION: |
j_j205 | 3:992558a021d7 | 54 | // huntMode() |
j_j205 | 0:999bb8fcd0b2 | 55 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 56 | // None |
j_j205 | 2:b281034eda86 | 57 | // None |
j_j205 | 0:999bb8fcd0b2 | 58 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 59 | // Hunts for victim. |
j_j205 | 3:992558a021d7 | 60 | void Scanner::huntMode() |
j_j205 | 0:999bb8fcd0b2 | 61 | { |
j_j205 | 2:b281034eda86 | 62 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 63 | avoidFlag = 0; |
j_j205 | 3:992558a021d7 | 64 | huntFlag = 1; |
j_j205 | 3:992558a021d7 | 65 | invertL = -1; |
j_j205 | 3:992558a021d7 | 66 | invertR = 1; |
j_j205 | 6:5e24ff86b743 | 67 | dutyL = MAX_DUTY; |
j_j205 | 6:5e24ff86b743 | 68 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 69 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 70 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 71 | wait(0.1); |
j_j205 | 2:b281034eda86 | 72 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 73 | } |
j_j205 | 0:999bb8fcd0b2 | 74 | |
j_j205 | 0:999bb8fcd0b2 | 75 | // FUNCTION: |
j_j205 | 3:992558a021d7 | 76 | // avoidMode() |
j_j205 | 0:999bb8fcd0b2 | 77 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 78 | // None |
j_j205 | 0:999bb8fcd0b2 | 79 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 80 | // None |
j_j205 | 0:999bb8fcd0b2 | 81 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 82 | // Scans to avoid obstacles. |
j_j205 | 3:992558a021d7 | 83 | void Scanner::avoidMode() |
j_j205 | 0:999bb8fcd0b2 | 84 | { |
j_j205 | 2:b281034eda86 | 85 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 86 | avoidFlag = 1; |
j_j205 | 3:992558a021d7 | 87 | huntFlag = 0; |
j_j205 | 3:992558a021d7 | 88 | invertL = -1; |
j_j205 | 3:992558a021d7 | 89 | invertR = 1; |
j_j205 | 3:992558a021d7 | 90 | dutyL = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 91 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 92 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 93 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 94 | wait(0.1); |
j_j205 | 2:b281034eda86 | 95 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 96 | } |
j_j205 | 0:999bb8fcd0b2 | 97 | |
j_j205 | 0:999bb8fcd0b2 | 98 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 99 | // localize() |
j_j205 | 0:999bb8fcd0b2 | 100 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 101 | // None |
j_j205 | 0:999bb8fcd0b2 | 102 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 103 | // None |
j_j205 | 0:999bb8fcd0b2 | 104 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 105 | // Checks localization points. |
j_j205 | 3:992558a021d7 | 106 | void Scanner::localize() |
j_j205 | 0:999bb8fcd0b2 | 107 | { |
j_j205 | 3:992558a021d7 | 108 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 109 | dutyL = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 110 | dutyR = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 111 | servoL.write(DUTY[dutyL]); |
j_j205 | 3:992558a021d7 | 112 | servoR.write(DUTY[dutyR]); |
j_j205 | 3:992558a021d7 | 113 | wait(0.1); |
j_j205 | 5:4d5601a9dffe | 114 | distLeft = longRangeL.distInchesL(); |
j_j205 | 5:4d5601a9dffe | 115 | distRight = longRangeR.distInchesR(); |
j_j205 | 6:5e24ff86b743 | 116 | dutyL = MAX_DUTY; |
j_j205 | 6:5e24ff86b743 | 117 | dutyR = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 118 | servoL.write(DUTY[dutyL]); |
j_j205 | 3:992558a021d7 | 119 | servoR.write(DUTY[dutyR]); |
j_j205 | 3:992558a021d7 | 120 | wait(0.1); |
j_j205 | 6:5e24ff86b743 | 121 | distForwardL = longRangeL.distInchesL(); |
j_j205 | 6:5e24ff86b743 | 122 | distForwardR = longRangeR.distInchesR(); |
j_j205 | 3:992558a021d7 | 123 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 124 | } |
j_j205 | 0:999bb8fcd0b2 | 125 | |
j_j205 | 0:999bb8fcd0b2 | 126 | // FUNCTION: |
j_j205 | 6:5e24ff86b743 | 127 | // void localizeLeft() |
j_j205 | 6:5e24ff86b743 | 128 | // IN-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 129 | // None |
j_j205 | 6:5e24ff86b743 | 130 | // OUT-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 131 | // None |
j_j205 | 6:5e24ff86b743 | 132 | // DESCRIPTION: |
j_j205 | 6:5e24ff86b743 | 133 | // Using sensor longRangeR this method will localize to the wall |
j_j205 | 6:5e24ff86b743 | 134 | // to the left during stretches of forward motion where robot |
j_j205 | 6:5e24ff86b743 | 135 | // should remain xxx distance from the left wall. |
j_j205 | 6:5e24ff86b743 | 136 | void Scanner::localizeLeft() |
j_j205 | 6:5e24ff86b743 | 137 | { |
j_j205 | 6:5e24ff86b743 | 138 | /****** pseudocode ***** |
j_j205 | 6:5e24ff86b743 | 139 | |
j_j205 | 6:5e24ff86b743 | 140 | dist = longRangeR dist measure |
j_j205 | 6:5e24ff86b743 | 141 | |
j_j205 | 6:5e24ff86b743 | 142 | if (dist > xxx dist) |
j_j205 | 6:5e24ff86b743 | 143 | pitEnable = 0 |
j_j205 | 6:5e24ff86b743 | 144 | pause current route |
j_j205 | 6:5e24ff86b743 | 145 | adjust angle to left |
j_j205 | 6:5e24ff86b743 | 146 | unpause route |
j_j205 | 6:5e24ff86b743 | 147 | pitEnable = 1 |
j_j205 | 6:5e24ff86b743 | 148 | |
j_j205 | 6:5e24ff86b743 | 149 | |
j_j205 | 6:5e24ff86b743 | 150 | else if (dist < xxx dist) |
j_j205 | 6:5e24ff86b743 | 151 | pitEnable = 0 |
j_j205 | 6:5e24ff86b743 | 152 | pause current route |
j_j205 | 6:5e24ff86b743 | 153 | adjust angle to right |
j_j205 | 6:5e24ff86b743 | 154 | unpause route |
j_j205 | 6:5e24ff86b743 | 155 | pitEnable = 1 |
j_j205 | 6:5e24ff86b743 | 156 | */ |
j_j205 | 6:5e24ff86b743 | 157 | } |
j_j205 | 6:5e24ff86b743 | 158 | |
j_j205 | 6:5e24ff86b743 | 159 | // FUNCTION: |
j_j205 | 6:5e24ff86b743 | 160 | // void localizeRight() |
j_j205 | 6:5e24ff86b743 | 161 | // IN-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 162 | // None |
j_j205 | 6:5e24ff86b743 | 163 | // OUT-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 164 | // None |
j_j205 | 6:5e24ff86b743 | 165 | // DESCRIPTION: |
j_j205 | 6:5e24ff86b743 | 166 | // Using sensor longRangeL this method will localize to the wall |
j_j205 | 6:5e24ff86b743 | 167 | // to the left during stretches of forward motion where robot |
j_j205 | 6:5e24ff86b743 | 168 | // should remain xxx distance from the left wall. This function |
j_j205 | 6:5e24ff86b743 | 169 | // will be called from scan when the localizeRightFlag is set. |
j_j205 | 6:5e24ff86b743 | 170 | void localizeRight() |
j_j205 | 6:5e24ff86b743 | 171 | { |
j_j205 | 6:5e24ff86b743 | 172 | /****** pseudocode ***** |
j_j205 | 6:5e24ff86b743 | 173 | |
j_j205 | 6:5e24ff86b743 | 174 | dist = longRangeL dist measure |
j_j205 | 6:5e24ff86b743 | 175 | |
j_j205 | 6:5e24ff86b743 | 176 | if (dist > xxx dist) |
j_j205 | 6:5e24ff86b743 | 177 | pitEnable = 0 |
j_j205 | 6:5e24ff86b743 | 178 | pause current route |
j_j205 | 6:5e24ff86b743 | 179 | adjust angle to right |
j_j205 | 6:5e24ff86b743 | 180 | unpause route |
j_j205 | 6:5e24ff86b743 | 181 | pitEnable = 1 |
j_j205 | 6:5e24ff86b743 | 182 | |
j_j205 | 6:5e24ff86b743 | 183 | else if (dist < xxx dist) |
j_j205 | 6:5e24ff86b743 | 184 | pitEnable = 0 |
j_j205 | 6:5e24ff86b743 | 185 | pause current route |
j_j205 | 6:5e24ff86b743 | 186 | adjust angle to left |
j_j205 | 6:5e24ff86b743 | 187 | unpause route |
j_j205 | 6:5e24ff86b743 | 188 | pitEnable = 1 |
j_j205 | 6:5e24ff86b743 | 189 | */ |
j_j205 | 6:5e24ff86b743 | 190 | } |
j_j205 | 6:5e24ff86b743 | 191 | |
j_j205 | 6:5e24ff86b743 | 192 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 193 | // scan() |
j_j205 | 0:999bb8fcd0b2 | 194 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 195 | // None |
j_j205 | 0:999bb8fcd0b2 | 196 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 197 | // None |
j_j205 | 0:999bb8fcd0b2 | 198 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 199 | // Ticker function to make servos scan either the inward-facing |
j_j205 | 2:b281034eda86 | 200 | // 90 degrees or the outward-facing 90 degrees. Distance |
j_j205 | 2:b281034eda86 | 201 | // measures are obtained from both the short range and long |
j_j205 | 2:b281034eda86 | 202 | // range sensors at 15 degree intervals. |
j_j205 | 0:999bb8fcd0b2 | 203 | void Scanner::scan() |
j_j205 | 0:999bb8fcd0b2 | 204 | { |
j_j205 | 2:b281034eda86 | 205 | if (pitEnable == 1) |
j_j205 | 2:b281034eda86 | 206 | { |
j_j205 | 6:5e24ff86b743 | 207 | /*// obtain distance measures from sensors |
j_j205 | 5:4d5601a9dffe | 208 | distLeft = longRangeL.distInchesL(); |
j_j205 | 5:4d5601a9dffe | 209 | distRight = longRangeR.distInchesR(); |
j_j205 | 5:4d5601a9dffe | 210 | |
j_j205 | 6:5e24ff86b743 | 211 | //check localize flags and make necessary method calls |
j_j205 | 6:5e24ff86b743 | 212 | |
j_j205 | 3:992558a021d7 | 213 | // check avoid flag |
j_j205 | 5:4d5601a9dffe | 214 | if (avoidFlag == 1) |
j_j205 | 5:4d5601a9dffe | 215 | { |
j_j205 | 5:4d5601a9dffe | 216 | // add obstacle avoid code |
j_j205 | 5:4d5601a9dffe | 217 | } |
j_j205 | 5:4d5601a9dffe | 218 | |
j_j205 | 3:992558a021d7 | 219 | // check hunt flag |
j_j205 | 6:5e24ff86b743 | 220 | if (huntFlag == 1) |
j_j205 | 5:4d5601a9dffe | 221 | { |
j_j205 | 5:4d5601a9dffe | 222 | // add hunt code |
j_j205 | 6:5e24ff86b743 | 223 | }*/ |
j_j205 | 3:992558a021d7 | 224 | |
j_j205 | 3:992558a021d7 | 225 | // increment/decrement servo position |
j_j205 | 2:b281034eda86 | 226 | dutyL += invertL; |
j_j205 | 2:b281034eda86 | 227 | dutyR += invertR; |
j_j205 | 2:b281034eda86 | 228 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 229 | servoR.write(DUTY[dutyR]); |
j_j205 | 6:5e24ff86b743 | 230 | if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || |
j_j205 | 2:b281034eda86 | 231 | ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) |
j_j205 | 2:b281034eda86 | 232 | { |
j_j205 | 2:b281034eda86 | 233 | invertL *= -1; |
j_j205 | 2:b281034eda86 | 234 | invertR *= -1; |
j_j205 | 2:b281034eda86 | 235 | } |
j_j205 | 2:b281034eda86 | 236 | } |
j_j205 | 3:992558a021d7 | 237 | } |