3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp@4:6a8d80954323, 2016-03-22 (annotated)
- Committer:
- j_j205
- Date:
- Tue Mar 22 03:00:16 2016 +0000
- Revision:
- 4:6a8d80954323
- Parent:
- 3:992558a021d7
- Child:
- 5:4d5601a9dffe
3/21/16 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 | 3:992558a021d7 | 26 | float temp[13] = {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, |
j_j205 | 3:992558a021d7 | 27 | 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, |
j_j205 | 3:992558a021d7 | 28 | 0.1000}; |
j_j205 | 2:b281034eda86 | 29 | |
j_j205 | 3:992558a021d7 | 30 | for (int i = 0; i < 13; i++) |
j_j205 | 3:992558a021d7 | 31 | DUTY[i] = temp[i]; |
j_j205 | 3:992558a021d7 | 32 | // 0.05 = min duty cycle |
j_j205 | 2:b281034eda86 | 33 | // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708, |
j_j205 | 2:b281034eda86 | 34 | // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, |
j_j205 | 3:992558a021d7 | 35 | // 0.1000}; |
j_j205 | 3:992558a021d7 | 36 | |
j_j205 | 3:992558a021d7 | 37 | obstacle = 0; |
j_j205 | 3:992558a021d7 | 38 | distLeft = 0.0; |
j_j205 | 3:992558a021d7 | 39 | distRight = 0.0; |
j_j205 | 3:992558a021d7 | 40 | distForward = 0.0; |
j_j205 | 2:b281034eda86 | 41 | |
j_j205 | 2:b281034eda86 | 42 | // initializing to avoid status |
j_j205 | 3:992558a021d7 | 43 | avoidFlag = 1; |
j_j205 | 3:992558a021d7 | 44 | huntFlag = 0; |
j_j205 | 3:992558a021d7 | 45 | invertL = -1; |
j_j205 | 3:992558a021d7 | 46 | invertR = 1; |
j_j205 | 3:992558a021d7 | 47 | dutyL = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 48 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 49 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 50 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 51 | wait(0.2); |
j_j205 | 2:b281034eda86 | 52 | pitEnable = 1; |
j_j205 | 3:992558a021d7 | 53 | scanPit.attach(this, &Scanner::scan, period); |
j_j205 | 0:999bb8fcd0b2 | 54 | } |
j_j205 | 0:999bb8fcd0b2 | 55 | |
j_j205 | 0:999bb8fcd0b2 | 56 | // FUNCTION: |
j_j205 | 3:992558a021d7 | 57 | // huntMode() |
j_j205 | 0:999bb8fcd0b2 | 58 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 59 | // None |
j_j205 | 2:b281034eda86 | 60 | // None |
j_j205 | 0:999bb8fcd0b2 | 61 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 62 | // Hunts for victim. |
j_j205 | 3:992558a021d7 | 63 | void Scanner::huntMode() |
j_j205 | 0:999bb8fcd0b2 | 64 | { |
j_j205 | 2:b281034eda86 | 65 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 66 | avoidFlag = 0; |
j_j205 | 3:992558a021d7 | 67 | huntFlag = 1; |
j_j205 | 3:992558a021d7 | 68 | invertL = -1; |
j_j205 | 3:992558a021d7 | 69 | invertR = 1; |
j_j205 | 2:b281034eda86 | 70 | dutyL = HALF_DUTY; |
j_j205 | 2:b281034eda86 | 71 | dutyR = HALF_DUTY; |
j_j205 | 2:b281034eda86 | 72 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 73 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 74 | wait(0.1); |
j_j205 | 2:b281034eda86 | 75 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 76 | } |
j_j205 | 0:999bb8fcd0b2 | 77 | |
j_j205 | 0:999bb8fcd0b2 | 78 | // FUNCTION: |
j_j205 | 3:992558a021d7 | 79 | // avoidMode() |
j_j205 | 0:999bb8fcd0b2 | 80 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 81 | // None |
j_j205 | 0:999bb8fcd0b2 | 82 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 83 | // None |
j_j205 | 0:999bb8fcd0b2 | 84 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 85 | // Scans to avoid obstacles. |
j_j205 | 3:992558a021d7 | 86 | void Scanner::avoidMode() |
j_j205 | 0:999bb8fcd0b2 | 87 | { |
j_j205 | 2:b281034eda86 | 88 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 89 | avoidFlag = 1; |
j_j205 | 3:992558a021d7 | 90 | huntFlag = 0; |
j_j205 | 3:992558a021d7 | 91 | invertL = -1; |
j_j205 | 3:992558a021d7 | 92 | invertR = 1; |
j_j205 | 3:992558a021d7 | 93 | dutyL = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 94 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 95 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 96 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 97 | wait(0.1); |
j_j205 | 2:b281034eda86 | 98 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 99 | } |
j_j205 | 0:999bb8fcd0b2 | 100 | |
j_j205 | 0:999bb8fcd0b2 | 101 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 102 | // localize() |
j_j205 | 0:999bb8fcd0b2 | 103 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 104 | // None |
j_j205 | 0:999bb8fcd0b2 | 105 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 106 | // None |
j_j205 | 0:999bb8fcd0b2 | 107 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 108 | // Checks localization points. |
j_j205 | 3:992558a021d7 | 109 | void Scanner::localize() |
j_j205 | 0:999bb8fcd0b2 | 110 | { |
j_j205 | 3:992558a021d7 | 111 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 112 | dutyL = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 113 | dutyR = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 114 | servoL.write(DUTY[dutyL]); |
j_j205 | 3:992558a021d7 | 115 | servoR.write(DUTY[dutyR]); |
j_j205 | 3:992558a021d7 | 116 | wait(0.1); |
j_j205 | 3:992558a021d7 | 117 | /* distLeft = longRangL.read(); |
j_j205 | 3:992558a021d7 | 118 | distRight = longRangeR.read(); */ |
j_j205 | 3:992558a021d7 | 119 | dutyL = HALF_DUTY; |
j_j205 | 3:992558a021d7 | 120 | dutyR = HALF_DUTY; |
j_j205 | 3:992558a021d7 | 121 | servoL.write(DUTY[dutyL]); |
j_j205 | 3:992558a021d7 | 122 | servoR.write(DUTY[dutyR]); |
j_j205 | 3:992558a021d7 | 123 | wait(0.1); |
j_j205 | 3:992558a021d7 | 124 | /* distForward = ( longRangeL.read() + longRangeR.read() )/2.0 */ |
j_j205 | 3:992558a021d7 | 125 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 126 | } |
j_j205 | 0:999bb8fcd0b2 | 127 | |
j_j205 | 0:999bb8fcd0b2 | 128 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 129 | // scan() |
j_j205 | 0:999bb8fcd0b2 | 130 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 131 | // None |
j_j205 | 0:999bb8fcd0b2 | 132 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 133 | // None |
j_j205 | 0:999bb8fcd0b2 | 134 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 135 | // Ticker function to make servos scan either the inward-facing |
j_j205 | 2:b281034eda86 | 136 | // 90 degrees or the outward-facing 90 degrees. Distance |
j_j205 | 2:b281034eda86 | 137 | // measures are obtained from both the short range and long |
j_j205 | 2:b281034eda86 | 138 | // range sensors at 15 degree intervals. |
j_j205 | 0:999bb8fcd0b2 | 139 | void Scanner::scan() |
j_j205 | 0:999bb8fcd0b2 | 140 | { |
j_j205 | 2:b281034eda86 | 141 | if (pitEnable == 1) |
j_j205 | 2:b281034eda86 | 142 | { |
j_j205 | 2:b281034eda86 | 143 | // obtain distance measures from sensors |
j_j205 | 3:992558a021d7 | 144 | // check avoid flag |
j_j205 | 3:992558a021d7 | 145 | // add obstacle avoid code |
j_j205 | 3:992558a021d7 | 146 | // check hunt flag |
j_j205 | 3:992558a021d7 | 147 | // add hunt code |
j_j205 | 3:992558a021d7 | 148 | |
j_j205 | 3:992558a021d7 | 149 | // increment/decrement servo position |
j_j205 | 2:b281034eda86 | 150 | dutyL += invertL; |
j_j205 | 2:b281034eda86 | 151 | dutyR += invertR; |
j_j205 | 2:b281034eda86 | 152 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 153 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 154 | if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) || |
j_j205 | 2:b281034eda86 | 155 | ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || |
j_j205 | 2:b281034eda86 | 156 | ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) |
j_j205 | 2:b281034eda86 | 157 | { |
j_j205 | 2:b281034eda86 | 158 | invertL *= -1; |
j_j205 | 2:b281034eda86 | 159 | invertR *= -1; |
j_j205 | 2:b281034eda86 | 160 | } |
j_j205 | 2:b281034eda86 | 161 | } |
j_j205 | 3:992558a021d7 | 162 | } |