3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp@16:4e76b53cdef2, 2016-04-01 (annotated)
- Committer:
- j_j205
- Date:
- Fri Apr 01 22:21:27 2016 +0000
- Revision:
- 16:4e76b53cdef2
- Parent:
- 15:3967cb2d93f5
stepper test 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" |
Hellkite85 | 7:52c4be3bfc1b | 4 | #include "StepperDrive.h" |
Hellkite85 | 7:52c4be3bfc1b | 5 | #include <stack> |
j_j205 | 0:999bb8fcd0b2 | 6 | |
j_j205 | 0:999bb8fcd0b2 | 7 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 8 | // Scanner() |
j_j205 | 0:999bb8fcd0b2 | 9 | // IN-PARAMETERS: |
Hellkite85 | 7:52c4be3bfc1b | 10 | // Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, |
j_j205 | 2:b281034eda86 | 11 | // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, |
j_j205 | 2:b281034eda86 | 12 | // Gp2x &_longRangeR, float _period = 10e-3 |
j_j205 | 0:999bb8fcd0b2 | 13 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 14 | // None |
j_j205 | 0:999bb8fcd0b2 | 15 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 16 | // Constructor. |
Hellkite85 | 7:52c4be3bfc1b | 17 | Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, |
j_j205 | 16:4e76b53cdef2 | 18 | LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, float _period) : pc(pc1), |
j_j205 | 16:4e76b53cdef2 | 19 | drive(_drive), servoL(_servoL), servoR(_servoR), longRangeL(_longRangeL), |
j_j205 | 16:4e76b53cdef2 | 20 | longRangeR(_longRangeR), period(_period) |
j_j205 | 0:999bb8fcd0b2 | 21 | { |
j_j205 | 0:999bb8fcd0b2 | 22 | servoL.period(1.0/50.0); |
j_j205 | 0:999bb8fcd0b2 | 23 | servoR.period(1.0/50.0); |
j_j205 | 2:b281034eda86 | 24 | |
j_j205 | 6:5e24ff86b743 | 25 | float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925, |
j_j205 | 6:5e24ff86b743 | 26 | 0.1013, 0.1100}; // sub-micro |
j_j205 | 2:b281034eda86 | 27 | |
j_j205 | 6:5e24ff86b743 | 28 | for (int i = 0; i < 7; i++) |
j_j205 | 3:992558a021d7 | 29 | DUTY[i] = temp[i]; |
j_j205 | 3:992558a021d7 | 30 | |
j_j205 | 3:992558a021d7 | 31 | obstacle = 0; |
j_j205 | 3:992558a021d7 | 32 | distLeft = 0.0; |
j_j205 | 3:992558a021d7 | 33 | distRight = 0.0; |
j_j205 | 6:5e24ff86b743 | 34 | distForwardL = 0.0; |
j_j205 | 6:5e24ff86b743 | 35 | distForwardR = 0.0; |
j_j205 | 2:b281034eda86 | 36 | |
j_j205 | 14:d327852dafd1 | 37 | // initializing to localizeRight status |
j_j205 | 14:d327852dafd1 | 38 | avoidFlag = 0; |
j_j205 | 3:992558a021d7 | 39 | huntFlag = 0; |
j_j205 | 16:4e76b53cdef2 | 40 | localizeRightFlag = 0; |
j_j205 | 14:d327852dafd1 | 41 | localizeLeftFlag = 0; |
j_j205 | 3:992558a021d7 | 42 | invertL = -1; |
j_j205 | 3:992558a021d7 | 43 | invertR = 1; |
j_j205 | 14:d327852dafd1 | 44 | dutyL = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 45 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 46 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 47 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 48 | wait(0.2); |
j_j205 | 2:b281034eda86 | 49 | pitEnable = 1; |
j_j205 | 3:992558a021d7 | 50 | scanPit.attach(this, &Scanner::scan, period); |
j_j205 | 6:5e24ff86b743 | 51 | pc.printf("Scanner created\n"); |
j_j205 | 0:999bb8fcd0b2 | 52 | } |
j_j205 | 0:999bb8fcd0b2 | 53 | |
j_j205 | 0:999bb8fcd0b2 | 54 | // FUNCTION: |
j_j205 | 14:d327852dafd1 | 55 | // void localizeLeftMode() |
j_j205 | 14:d327852dafd1 | 56 | // IN-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 57 | // None |
j_j205 | 14:d327852dafd1 | 58 | // OUT-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 59 | // None |
j_j205 | 14:d327852dafd1 | 60 | // DESCRIPTION: |
j_j205 | 14:d327852dafd1 | 61 | // This method initializes to localizeLeft() |
j_j205 | 14:d327852dafd1 | 62 | void Scanner::localizeLeftMode() |
j_j205 | 14:d327852dafd1 | 63 | { |
j_j205 | 14:d327852dafd1 | 64 | localizeRightFlag = 0; |
j_j205 | 14:d327852dafd1 | 65 | |
j_j205 | 14:d327852dafd1 | 66 | dutyL = MAX_DUTY; |
j_j205 | 14:d327852dafd1 | 67 | dutyR = MAX_DUTY; |
j_j205 | 14:d327852dafd1 | 68 | servoL.write(DUTY[dutyL]); |
j_j205 | 14:d327852dafd1 | 69 | servoR.write(DUTY[dutyR]); |
j_j205 | 14:d327852dafd1 | 70 | wait(0.2); |
j_j205 | 14:d327852dafd1 | 71 | |
j_j205 | 14:d327852dafd1 | 72 | localizeLeftFlag = 1; |
j_j205 | 14:d327852dafd1 | 73 | } |
j_j205 | 14:d327852dafd1 | 74 | |
j_j205 | 14:d327852dafd1 | 75 | // FUNCTION: |
j_j205 | 6:5e24ff86b743 | 76 | // void localizeLeft() |
j_j205 | 6:5e24ff86b743 | 77 | // IN-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 78 | // None |
j_j205 | 6:5e24ff86b743 | 79 | // OUT-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 80 | // None |
j_j205 | 6:5e24ff86b743 | 81 | // DESCRIPTION: |
j_j205 | 6:5e24ff86b743 | 82 | // Using sensor longRangeR this method will localize to the wall |
j_j205 | 6:5e24ff86b743 | 83 | // to the left during stretches of forward motion where robot |
j_j205 | 6:5e24ff86b743 | 84 | // should remain xxx distance from the left wall. |
j_j205 | 6:5e24ff86b743 | 85 | void Scanner::localizeLeft() |
Hellkite85 | 7:52c4be3bfc1b | 86 | { |
j_j205 | 16:4e76b53cdef2 | 87 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 88 | float distLocalR = longRangeR.distInchesR(); |
j_j205 | 6:5e24ff86b743 | 89 | |
j_j205 | 16:4e76b53cdef2 | 90 | if (distLocalR >= 9.2) |
Hellkite85 | 7:52c4be3bfc1b | 91 | { |
Hellkite85 | 7:52c4be3bfc1b | 92 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 93 | drive.move(0.0, ((-5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 94 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 95 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 96 | { |
j_j205 | 14:d327852dafd1 | 97 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 98 | } |
Hellkite85 | 7:52c4be3bfc1b | 99 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 100 | } |
j_j205 | 16:4e76b53cdef2 | 101 | else if (distLocalR <= 8.2) |
Hellkite85 | 7:52c4be3bfc1b | 102 | { |
Hellkite85 | 7:52c4be3bfc1b | 103 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 104 | drive.move(0.0, ((5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 105 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 106 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 107 | { |
j_j205 | 14:d327852dafd1 | 108 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 109 | } |
Hellkite85 | 7:52c4be3bfc1b | 110 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 111 | } |
j_j205 | 16:4e76b53cdef2 | 112 | |
j_j205 | 16:4e76b53cdef2 | 113 | // wait for move to complete |
j_j205 | 16:4e76b53cdef2 | 114 | while(!drive.isMoveDone()) |
j_j205 | 16:4e76b53cdef2 | 115 | { |
j_j205 | 16:4e76b53cdef2 | 116 | wait(1e-6); |
j_j205 | 16:4e76b53cdef2 | 117 | } |
j_j205 | 16:4e76b53cdef2 | 118 | pitEnable = 1; |
j_j205 | 6:5e24ff86b743 | 119 | } |
j_j205 | 6:5e24ff86b743 | 120 | |
j_j205 | 6:5e24ff86b743 | 121 | // FUNCTION: |
j_j205 | 14:d327852dafd1 | 122 | // void localizeRightMode() |
j_j205 | 14:d327852dafd1 | 123 | // IN-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 124 | // None |
j_j205 | 14:d327852dafd1 | 125 | // OUT-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 126 | // None |
j_j205 | 14:d327852dafd1 | 127 | // DESCRIPTION: |
j_j205 | 14:d327852dafd1 | 128 | // Initializes to localizeRight() |
Hellkite85 | 15:3967cb2d93f5 | 129 | void Scanner::localizeRightMode() |
j_j205 | 14:d327852dafd1 | 130 | { |
j_j205 | 14:d327852dafd1 | 131 | localizeLeftFlag = 0; |
j_j205 | 14:d327852dafd1 | 132 | |
j_j205 | 14:d327852dafd1 | 133 | dutyL = MIN_DUTY; |
j_j205 | 14:d327852dafd1 | 134 | dutyR = MIN_DUTY; |
j_j205 | 14:d327852dafd1 | 135 | servoL.write(DUTY[dutyL]); |
j_j205 | 14:d327852dafd1 | 136 | servoR.write(DUTY[dutyR]); |
j_j205 | 14:d327852dafd1 | 137 | wait(0.2); |
j_j205 | 14:d327852dafd1 | 138 | |
j_j205 | 14:d327852dafd1 | 139 | localizeRightFlag = 1; |
j_j205 | 14:d327852dafd1 | 140 | } |
j_j205 | 14:d327852dafd1 | 141 | |
j_j205 | 14:d327852dafd1 | 142 | // FUNCTION: |
j_j205 | 6:5e24ff86b743 | 143 | // void localizeRight() |
j_j205 | 6:5e24ff86b743 | 144 | // IN-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 145 | // None |
j_j205 | 6:5e24ff86b743 | 146 | // OUT-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 147 | // None |
j_j205 | 6:5e24ff86b743 | 148 | // DESCRIPTION: |
j_j205 | 6:5e24ff86b743 | 149 | // Using sensor longRangeL this method will localize to the wall |
j_j205 | 6:5e24ff86b743 | 150 | // to the left during stretches of forward motion where robot |
j_j205 | 6:5e24ff86b743 | 151 | // should remain xxx distance from the left wall. This function |
j_j205 | 6:5e24ff86b743 | 152 | // will be called from scan when the localizeRightFlag is set. |
Hellkite85 | 7:52c4be3bfc1b | 153 | void Scanner::localizeRight() |
j_j205 | 6:5e24ff86b743 | 154 | { |
j_j205 | 16:4e76b53cdef2 | 155 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 156 | float distLocalL = longRangeL.distInchesL(); |
j_j205 | 6:5e24ff86b743 | 157 | |
j_j205 | 16:4e76b53cdef2 | 158 | if (distLocalL >= 9.2) |
Hellkite85 | 7:52c4be3bfc1b | 159 | { |
Hellkite85 | 7:52c4be3bfc1b | 160 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 161 | drive.move(0, ((5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 162 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 163 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 164 | { |
j_j205 | 14:d327852dafd1 | 165 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 166 | } |
Hellkite85 | 7:52c4be3bfc1b | 167 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 168 | } |
j_j205 | 16:4e76b53cdef2 | 169 | else if (distLocalL <= 8.2) |
Hellkite85 | 7:52c4be3bfc1b | 170 | { |
Hellkite85 | 7:52c4be3bfc1b | 171 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 172 | drive.move(0, ((-5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 173 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 174 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 175 | { |
j_j205 | 14:d327852dafd1 | 176 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 177 | } |
Hellkite85 | 7:52c4be3bfc1b | 178 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 179 | } |
j_j205 | 16:4e76b53cdef2 | 180 | |
j_j205 | 16:4e76b53cdef2 | 181 | // wait for move to complete |
j_j205 | 16:4e76b53cdef2 | 182 | while(!drive.isMoveDone()) |
j_j205 | 16:4e76b53cdef2 | 183 | { |
j_j205 | 16:4e76b53cdef2 | 184 | wait(1e-6); |
j_j205 | 16:4e76b53cdef2 | 185 | } |
j_j205 | 16:4e76b53cdef2 | 186 | pitEnable = 1; |
j_j205 | 6:5e24ff86b743 | 187 | } |
j_j205 | 6:5e24ff86b743 | 188 | |
j_j205 | 6:5e24ff86b743 | 189 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 190 | // scan() |
j_j205 | 0:999bb8fcd0b2 | 191 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 192 | // None |
j_j205 | 0:999bb8fcd0b2 | 193 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 194 | // None |
j_j205 | 0:999bb8fcd0b2 | 195 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 196 | // Ticker function to make servos scan either the inward-facing |
j_j205 | 2:b281034eda86 | 197 | // 90 degrees or the outward-facing 90 degrees. Distance |
j_j205 | 2:b281034eda86 | 198 | // measures are obtained from both the short range and long |
j_j205 | 2:b281034eda86 | 199 | // range sensors at 15 degree intervals. |
j_j205 | 0:999bb8fcd0b2 | 200 | void Scanner::scan() |
j_j205 | 0:999bb8fcd0b2 | 201 | { |
j_j205 | 2:b281034eda86 | 202 | if (pitEnable == 1) |
j_j205 | 2:b281034eda86 | 203 | { |
j_j205 | 13:8767be1c5cc2 | 204 | if (localizeRightFlag == 1) |
j_j205 | 13:8767be1c5cc2 | 205 | localizeRight(); |
j_j205 | 13:8767be1c5cc2 | 206 | |
j_j205 | 14:d327852dafd1 | 207 | /*if (localizeLeftFlag == 1) |
j_j205 | 14:d327852dafd1 | 208 | localizeLeft();*/ |
j_j205 | 2:b281034eda86 | 209 | } |
j_j205 | 3:992558a021d7 | 210 | } |