3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp@15:3967cb2d93f5, 2016-04-01 (annotated)
- Committer:
- Hellkite85
- Date:
- Fri Apr 01 17:25:02 2016 +0000
- Revision:
- 15:3967cb2d93f5
- Parent:
- 14:d327852dafd1
- Child:
- 16:4e76b53cdef2
- Child:
- 17:ec04d60f38bd
Updated hunt function 4-1-2016
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 | 8:32a445ae1d72 | 4 | #include "ShortRangeSensor.h" |
Hellkite85 | 7:52c4be3bfc1b | 5 | #include "StepperDrive.h" |
Hellkite85 | 9:bae63bc84829 | 6 | #include "Gripper.h" |
Hellkite85 | 7:52c4be3bfc1b | 7 | #include <stack> |
j_j205 | 0:999bb8fcd0b2 | 8 | |
j_j205 | 0:999bb8fcd0b2 | 9 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 10 | // Scanner() |
j_j205 | 0:999bb8fcd0b2 | 11 | // IN-PARAMETERS: |
Hellkite85 | 7:52c4be3bfc1b | 12 | // Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, |
j_j205 | 2:b281034eda86 | 13 | // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, |
j_j205 | 2:b281034eda86 | 14 | // Gp2x &_longRangeR, float _period = 10e-3 |
j_j205 | 0:999bb8fcd0b2 | 15 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 16 | // None |
j_j205 | 0:999bb8fcd0b2 | 17 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 18 | // Constructor. |
Hellkite85 | 7:52c4be3bfc1b | 19 | Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, |
j_j205 | 8:32a445ae1d72 | 20 | ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL, |
Hellkite85 | 9:bae63bc84829 | 21 | LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period) : pc(pc1), drive(_drive), servoL(_servoL), |
j_j205 | 2:b281034eda86 | 22 | servoR(_servoR), shortRangeL(_shortRangeL), |
j_j205 | 2:b281034eda86 | 23 | shortRangeR(_shortRangeR), longRangeL(_longRangeL), |
j_j205 | 12:514544a4014f | 24 | longRangeR(_longRangeR), robotGrip(_robotGrip), period(_period) |
j_j205 | 0:999bb8fcd0b2 | 25 | { |
j_j205 | 0:999bb8fcd0b2 | 26 | servoL.period(1.0/50.0); |
j_j205 | 0:999bb8fcd0b2 | 27 | servoR.period(1.0/50.0); |
j_j205 | 2:b281034eda86 | 28 | |
j_j205 | 6:5e24ff86b743 | 29 | float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925, |
j_j205 | 6:5e24ff86b743 | 30 | 0.1013, 0.1100}; // sub-micro |
j_j205 | 2:b281034eda86 | 31 | |
j_j205 | 6:5e24ff86b743 | 32 | for (int i = 0; i < 7; i++) |
j_j205 | 3:992558a021d7 | 33 | DUTY[i] = temp[i]; |
j_j205 | 3:992558a021d7 | 34 | |
j_j205 | 3:992558a021d7 | 35 | obstacle = 0; |
j_j205 | 3:992558a021d7 | 36 | distLeft = 0.0; |
j_j205 | 3:992558a021d7 | 37 | distRight = 0.0; |
j_j205 | 6:5e24ff86b743 | 38 | distForwardL = 0.0; |
j_j205 | 6:5e24ff86b743 | 39 | distForwardR = 0.0; |
j_j205 | 2:b281034eda86 | 40 | |
j_j205 | 14:d327852dafd1 | 41 | // initializing to localizeRight status |
j_j205 | 14:d327852dafd1 | 42 | avoidFlag = 0; |
j_j205 | 3:992558a021d7 | 43 | huntFlag = 0; |
j_j205 | 14:d327852dafd1 | 44 | localizeRightFlag = 1; |
j_j205 | 14:d327852dafd1 | 45 | localizeLeftFlag = 0; |
j_j205 | 3:992558a021d7 | 46 | invertL = -1; |
j_j205 | 3:992558a021d7 | 47 | invertR = 1; |
j_j205 | 14:d327852dafd1 | 48 | dutyL = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 49 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 50 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 51 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 52 | wait(0.2); |
j_j205 | 2:b281034eda86 | 53 | pitEnable = 1; |
j_j205 | 3:992558a021d7 | 54 | scanPit.attach(this, &Scanner::scan, period); |
j_j205 | 6:5e24ff86b743 | 55 | pc.printf("Scanner created\n"); |
j_j205 | 0:999bb8fcd0b2 | 56 | } |
j_j205 | 0:999bb8fcd0b2 | 57 | |
j_j205 | 0:999bb8fcd0b2 | 58 | // FUNCTION: |
j_j205 | 3:992558a021d7 | 59 | // huntMode() |
j_j205 | 0:999bb8fcd0b2 | 60 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 61 | // None |
j_j205 | 2:b281034eda86 | 62 | // None |
j_j205 | 0:999bb8fcd0b2 | 63 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 64 | // Hunts for victim. |
j_j205 | 3:992558a021d7 | 65 | void Scanner::huntMode() |
j_j205 | 0:999bb8fcd0b2 | 66 | { |
j_j205 | 2:b281034eda86 | 67 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 68 | avoidFlag = 0; |
j_j205 | 3:992558a021d7 | 69 | huntFlag = 1; |
j_j205 | 3:992558a021d7 | 70 | invertL = -1; |
j_j205 | 3:992558a021d7 | 71 | invertR = 1; |
j_j205 | 6:5e24ff86b743 | 72 | dutyL = MAX_DUTY; |
j_j205 | 6:5e24ff86b743 | 73 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 74 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 75 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 76 | wait(0.1); |
j_j205 | 2:b281034eda86 | 77 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 78 | } |
j_j205 | 0:999bb8fcd0b2 | 79 | |
j_j205 | 0:999bb8fcd0b2 | 80 | // FUNCTION: |
j_j205 | 3:992558a021d7 | 81 | // avoidMode() |
j_j205 | 0:999bb8fcd0b2 | 82 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 83 | // None |
j_j205 | 0:999bb8fcd0b2 | 84 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 85 | // None |
j_j205 | 0:999bb8fcd0b2 | 86 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 87 | // Scans to avoid obstacles. |
j_j205 | 3:992558a021d7 | 88 | void Scanner::avoidMode() |
j_j205 | 0:999bb8fcd0b2 | 89 | { |
j_j205 | 2:b281034eda86 | 90 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 91 | avoidFlag = 1; |
j_j205 | 3:992558a021d7 | 92 | huntFlag = 0; |
j_j205 | 3:992558a021d7 | 93 | invertL = -1; |
j_j205 | 3:992558a021d7 | 94 | invertR = 1; |
j_j205 | 3:992558a021d7 | 95 | dutyL = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 96 | dutyR = MIN_DUTY; |
j_j205 | 2:b281034eda86 | 97 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 98 | servoR.write(DUTY[dutyR]); |
j_j205 | 2:b281034eda86 | 99 | wait(0.1); |
j_j205 | 2:b281034eda86 | 100 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 101 | } |
Hellkite85 | 7:52c4be3bfc1b | 102 | // FUNCTION: |
Hellkite85 | 7:52c4be3bfc1b | 103 | // |
Hellkite85 | 7:52c4be3bfc1b | 104 | void Scanner::hunt() |
Hellkite85 | 7:52c4be3bfc1b | 105 | { |
Hellkite85 | 9:bae63bc84829 | 106 | // This creates an instance of a structure |
Hellkite85 | 9:bae63bc84829 | 107 | // call huntMove. This structure is used to |
Hellkite85 | 9:bae63bc84829 | 108 | // store the distance value and angle value |
Hellkite85 | 9:bae63bc84829 | 109 | // when implementing the Navigation drive |
Hellkite85 | 9:bae63bc84829 | 110 | // code. |
Hellkite85 | 9:bae63bc84829 | 111 | huntMove moveRobot; |
Hellkite85 | 9:bae63bc84829 | 112 | |
Hellkite85 | 7:52c4be3bfc1b | 113 | // Used to store temporary distance |
Hellkite85 | 7:52c4be3bfc1b | 114 | // recorded from VL6180x short range |
Hellkite85 | 7:52c4be3bfc1b | 115 | // sensor. |
Hellkite85 | 7:52c4be3bfc1b | 116 | float tempDistShortL; |
Hellkite85 | 7:52c4be3bfc1b | 117 | float tempDistShortR; |
Hellkite85 | 9:bae63bc84829 | 118 | |
Hellkite85 | 9:bae63bc84829 | 119 | float distShortL; |
Hellkite85 | 9:bae63bc84829 | 120 | float distShortR; |
Hellkite85 | 7:52c4be3bfc1b | 121 | |
Hellkite85 | 7:52c4be3bfc1b | 122 | // Used to temporary store the number |
Hellkite85 | 7:52c4be3bfc1b | 123 | // of stepper motor increments. |
Hellkite85 | 7:52c4be3bfc1b | 124 | int tempStepCount = 0; |
Hellkite85 | 7:52c4be3bfc1b | 125 | |
Hellkite85 | 9:bae63bc84829 | 126 | // This variable is half of the available |
Hellkite85 | 9:bae63bc84829 | 127 | // tolerance of the gripper with regards |
Hellkite85 | 9:bae63bc84829 | 128 | // to the peg. |
Hellkite85 | 9:bae63bc84829 | 129 | float tolerance = 0.1875; |
Hellkite85 | 9:bae63bc84829 | 130 | |
Hellkite85 | 9:bae63bc84829 | 131 | // These variables represent the tolerance |
Hellkite85 | 9:bae63bc84829 | 132 | // and plus/minus half the diameter of the |
Hellkite85 | 9:bae63bc84829 | 133 | // peg measured in inches. |
Hellkite85 | 9:bae63bc84829 | 134 | float toleranceAHP = 0.9375; |
Hellkite85 | 9:bae63bc84829 | 135 | float toleranceSHP = 0.5625; |
Hellkite85 | 9:bae63bc84829 | 136 | |
Hellkite85 | 7:52c4be3bfc1b | 137 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 138 | |
Hellkite85 | 9:bae63bc84829 | 139 | // This positions both of the servos to |
Hellkite85 | 9:bae63bc84829 | 140 | // be 60 degrees inwards. |
Hellkite85 | 7:52c4be3bfc1b | 141 | servoL.write(DUTY[4]); |
Hellkite85 | 7:52c4be3bfc1b | 142 | servoR.write(DUTY[2]); |
Hellkite85 | 7:52c4be3bfc1b | 143 | |
Hellkite85 | 9:bae63bc84829 | 144 | // This initializes the temporary distance variables |
Hellkite85 | 9:bae63bc84829 | 145 | // with the data collected from the VL6180X sensors. |
Hellkite85 | 9:bae63bc84829 | 146 | tempDistShortL = shortRangeL.getRange(); |
Hellkite85 | 9:bae63bc84829 | 147 | tempDistShortR = shortRangeR.getRange(); |
Hellkite85 | 7:52c4be3bfc1b | 148 | |
Hellkite85 | 9:bae63bc84829 | 149 | // This code converts the measurements from the short |
Hellkite85 | 9:bae63bc84829 | 150 | // range sensor from millimeters to inches. |
Hellkite85 | 9:bae63bc84829 | 151 | distShortL = (tempDistShortL / 25.0); |
Hellkite85 | 9:bae63bc84829 | 152 | distShortR = (tempDistShortR / 25.0); |
Hellkite85 | 9:bae63bc84829 | 153 | |
Hellkite85 | 9:bae63bc84829 | 154 | // This while loop statement moves the robot forward in order |
Hellkite85 | 9:bae63bc84829 | 155 | // to continue the hunt of the peg. The equals to comparsion |
Hellkite85 | 9:bae63bc84829 | 156 | // statements need to be updated to reflect what the short |
Hellkite85 | 9:bae63bc84829 | 157 | // range sensors output when there is nothing within the |
Hellkite85 | 9:bae63bc84829 | 158 | // range of the short distance sensors. |
Hellkite85 | 9:bae63bc84829 | 159 | while(distShortL == 0.0 && distShortR == 0.0 && tempStepCount < 34) |
Hellkite85 | 7:52c4be3bfc1b | 160 | { |
Hellkite85 | 7:52c4be3bfc1b | 161 | drive.move(0.25, 0.0); |
Hellkite85 | 15:3967cb2d93f5 | 162 | while(!drive.isMoveDone()) |
Hellkite85 | 15:3967cb2d93f5 | 163 | { |
Hellkite85 | 15:3967cb2d93f5 | 164 | wait(1e-6); |
Hellkite85 | 15:3967cb2d93f5 | 165 | } |
Hellkite85 | 9:bae63bc84829 | 166 | |
Hellkite85 | 9:bae63bc84829 | 167 | moveRobot.distance = 0.25; |
Hellkite85 | 9:bae63bc84829 | 168 | moveRobot.angle = 0.0; |
Hellkite85 | 9:bae63bc84829 | 169 | |
Hellkite85 | 9:bae63bc84829 | 170 | myStack.push(moveRobot); |
Hellkite85 | 9:bae63bc84829 | 171 | } |
Hellkite85 | 7:52c4be3bfc1b | 172 | |
Hellkite85 | 7:52c4be3bfc1b | 173 | // Adjust the comparsion for floats with |
Hellkite85 | 9:bae63bc84829 | 174 | // regards to the sensor data. This |
Hellkite85 | 9:bae63bc84829 | 175 | // while loop ideally checks the short range |
Hellkite85 | 7:52c4be3bfc1b | 176 | // sensors and if they are not equal to |
Hellkite85 | 7:52c4be3bfc1b | 177 | // each other, the code will center the |
Hellkite85 | 9:bae63bc84829 | 178 | // robot according to the short range sensor. |
Hellkite85 | 9:bae63bc84829 | 179 | while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance)) |
Hellkite85 | 7:52c4be3bfc1b | 180 | { |
Hellkite85 | 7:52c4be3bfc1b | 181 | // Adjust this comparsion for floats |
Hellkite85 | 7:52c4be3bfc1b | 182 | // with regards to the sensor data. |
Hellkite85 | 9:bae63bc84829 | 183 | if(distShortL > (distShortR + tolerance)) |
Hellkite85 | 9:bae63bc84829 | 184 | { |
Hellkite85 | 15:3967cb2d93f5 | 185 | drive.move(0.0, (-5.0)*(3.14159 / 180.0)); |
Hellkite85 | 15:3967cb2d93f5 | 186 | while(!drive.isMoveDone()) |
Hellkite85 | 15:3967cb2d93f5 | 187 | { |
Hellkite85 | 15:3967cb2d93f5 | 188 | wait(1e-6); |
Hellkite85 | 15:3967cb2d93f5 | 189 | } |
Hellkite85 | 9:bae63bc84829 | 190 | |
Hellkite85 | 9:bae63bc84829 | 191 | moveRobot.distance = 0.0; |
Hellkite85 | 15:3967cb2d93f5 | 192 | moveRobot.angle = ((-5.0)*(3.14159 / 180.0)); |
Hellkite85 | 9:bae63bc84829 | 193 | |
Hellkite85 | 9:bae63bc84829 | 194 | myStack.push(moveRobot); |
Hellkite85 | 9:bae63bc84829 | 195 | } |
Hellkite85 | 7:52c4be3bfc1b | 196 | else |
Hellkite85 | 9:bae63bc84829 | 197 | { |
Hellkite85 | 15:3967cb2d93f5 | 198 | drive.move(0.0, (5.0*(3.14159 / 180.0)); |
Hellkite85 | 15:3967cb2d93f5 | 199 | while(!drive.isMoveDone()) |
Hellkite85 | 15:3967cb2d93f5 | 200 | { |
Hellkite85 | 15:3967cb2d93f5 | 201 | wait(1e-6); |
Hellkite85 | 15:3967cb2d93f5 | 202 | } |
Hellkite85 | 9:bae63bc84829 | 203 | |
Hellkite85 | 9:bae63bc84829 | 204 | moveRobot.distance = 0.0; |
Hellkite85 | 15:3967cb2d93f5 | 205 | moveRobot.angle = ((5.0)*(3.14159 / 180.0)); |
Hellkite85 | 9:bae63bc84829 | 206 | |
Hellkite85 | 9:bae63bc84829 | 207 | myStack.push(moveRobot); |
Hellkite85 | 9:bae63bc84829 | 208 | } |
Hellkite85 | 7:52c4be3bfc1b | 209 | } |
Hellkite85 | 7:52c4be3bfc1b | 210 | |
Hellkite85 | 7:52c4be3bfc1b | 211 | servoL.write(DUTY[dutyR]); |
Hellkite85 | 7:52c4be3bfc1b | 212 | servoR.write(DUTY[dutyL]); |
Hellkite85 | 7:52c4be3bfc1b | 213 | |
Hellkite85 | 9:bae63bc84829 | 214 | // This code reinitializes the short distance |
Hellkite85 | 9:bae63bc84829 | 215 | // variable now that the short distance sensors |
Hellkite85 | 9:bae63bc84829 | 216 | // are pointing inward. |
Hellkite85 | 9:bae63bc84829 | 217 | distShortL = shortRangeL.getRange(); |
Hellkite85 | 9:bae63bc84829 | 218 | distShortR = shortRangeR.getRange(); |
Hellkite85 | 7:52c4be3bfc1b | 219 | |
Hellkite85 | 7:52c4be3bfc1b | 220 | // This while loop is used to position the robot |
Hellkite85 | 9:bae63bc84829 | 221 | // at the correct location vertically with regards |
Hellkite85 | 9:bae63bc84829 | 222 | // to the peg in order to pick up the peg. The tolerance |
Hellkite85 | 9:bae63bc84829 | 223 | while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) |
Hellkite85 | 7:52c4be3bfc1b | 224 | { |
Hellkite85 | 9:bae63bc84829 | 225 | drive.move(0.125, 0.0); |
Hellkite85 | 15:3967cb2d93f5 | 226 | while(!drive.isMoveDone()) |
Hellkite85 | 15:3967cb2d93f5 | 227 | { |
Hellkite85 | 15:3967cb2d93f5 | 228 | wait(1e-6); |
Hellkite85 | 15:3967cb2d93f5 | 229 | } |
Hellkite85 | 9:bae63bc84829 | 230 | |
Hellkite85 | 9:bae63bc84829 | 231 | moveRobot.distance = 0.125; |
Hellkite85 | 9:bae63bc84829 | 232 | moveRobot.angle = 0.0; |
Hellkite85 | 9:bae63bc84829 | 233 | |
Hellkite85 | 9:bae63bc84829 | 234 | myStack.push(moveRobot); |
Hellkite85 | 9:bae63bc84829 | 235 | } |
Hellkite85 | 9:bae63bc84829 | 236 | |
Hellkite85 | 9:bae63bc84829 | 237 | // This is the code to enable the gripper to grip the peg. |
j_j205 | 12:514544a4014f | 238 | robotGrip.grip(); |
Hellkite85 | 9:bae63bc84829 | 239 | |
Hellkite85 | 9:bae63bc84829 | 240 | /* pseudocode |
Hellkite85 | 9:bae63bc84829 | 241 | implement the code for reading a value from the color sensor |
Hellkite85 | 9:bae63bc84829 | 242 | store the value that is returned from the color sensor |
Hellkite85 | 9:bae63bc84829 | 243 | */ |
Hellkite85 | 9:bae63bc84829 | 244 | |
Hellkite85 | 9:bae63bc84829 | 245 | // This is the code to enable the gripper to lift the peg. |
Hellkite85 | 9:bae63bc84829 | 246 | robotGrip.lift(); |
Hellkite85 | 9:bae63bc84829 | 247 | |
Hellkite85 | 9:bae63bc84829 | 248 | // This code uses the stack to reverse all the movement |
Hellkite85 | 9:bae63bc84829 | 249 | // of the robot during this function. |
Hellkite85 | 9:bae63bc84829 | 250 | while(!myStack.empty()) |
Hellkite85 | 9:bae63bc84829 | 251 | { |
Hellkite85 | 9:bae63bc84829 | 252 | myStack.top(); |
Hellkite85 | 9:bae63bc84829 | 253 | myStack.pop(); |
Hellkite85 | 9:bae63bc84829 | 254 | |
Hellkite85 | 15:3967cb2d93f5 | 255 | drive.move((-(moveRobot.distance)),(-(moveRobot.angle))); |
Hellkite85 | 15:3967cb2d93f5 | 256 | while(!drive.isMoveDone()) |
Hellkite85 | 15:3967cb2d93f5 | 257 | { |
Hellkite85 | 15:3967cb2d93f5 | 258 | wait(1e-6); |
Hellkite85 | 15:3967cb2d93f5 | 259 | } |
Hellkite85 | 9:bae63bc84829 | 260 | } |
Hellkite85 | 7:52c4be3bfc1b | 261 | } |
j_j205 | 0:999bb8fcd0b2 | 262 | |
j_j205 | 0:999bb8fcd0b2 | 263 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 264 | // localize() |
j_j205 | 0:999bb8fcd0b2 | 265 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 266 | // None |
j_j205 | 0:999bb8fcd0b2 | 267 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 268 | // None |
j_j205 | 0:999bb8fcd0b2 | 269 | // DESCRIPTION: |
j_j205 | 0:999bb8fcd0b2 | 270 | // Checks localization points. |
j_j205 | 3:992558a021d7 | 271 | void Scanner::localize() |
j_j205 | 0:999bb8fcd0b2 | 272 | { |
j_j205 | 3:992558a021d7 | 273 | pitEnable = 0; |
j_j205 | 3:992558a021d7 | 274 | dutyL = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 275 | dutyR = MAX_DUTY; |
j_j205 | 3:992558a021d7 | 276 | servoL.write(DUTY[dutyL]); |
j_j205 | 3:992558a021d7 | 277 | servoR.write(DUTY[dutyR]); |
j_j205 | 3:992558a021d7 | 278 | wait(0.1); |
j_j205 | 5:4d5601a9dffe | 279 | distLeft = longRangeL.distInchesL(); |
j_j205 | 5:4d5601a9dffe | 280 | distRight = longRangeR.distInchesR(); |
j_j205 | 6:5e24ff86b743 | 281 | dutyL = MAX_DUTY; |
j_j205 | 6:5e24ff86b743 | 282 | dutyR = MIN_DUTY; |
j_j205 | 3:992558a021d7 | 283 | servoL.write(DUTY[dutyL]); |
j_j205 | 3:992558a021d7 | 284 | servoR.write(DUTY[dutyR]); |
j_j205 | 3:992558a021d7 | 285 | wait(0.1); |
j_j205 | 6:5e24ff86b743 | 286 | distForwardL = longRangeL.distInchesL(); |
j_j205 | 6:5e24ff86b743 | 287 | distForwardR = longRangeR.distInchesR(); |
j_j205 | 3:992558a021d7 | 288 | pitEnable = 1; |
j_j205 | 0:999bb8fcd0b2 | 289 | } |
j_j205 | 0:999bb8fcd0b2 | 290 | |
j_j205 | 0:999bb8fcd0b2 | 291 | // FUNCTION: |
j_j205 | 14:d327852dafd1 | 292 | // void localizeLeftMode() |
j_j205 | 14:d327852dafd1 | 293 | // IN-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 294 | // None |
j_j205 | 14:d327852dafd1 | 295 | // OUT-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 296 | // None |
j_j205 | 14:d327852dafd1 | 297 | // DESCRIPTION: |
j_j205 | 14:d327852dafd1 | 298 | // This method initializes to localizeLeft() |
j_j205 | 14:d327852dafd1 | 299 | void Scanner::localizeLeftMode() |
j_j205 | 14:d327852dafd1 | 300 | { |
j_j205 | 14:d327852dafd1 | 301 | localizeRightFlag = 0; |
j_j205 | 14:d327852dafd1 | 302 | |
j_j205 | 14:d327852dafd1 | 303 | dutyL = MAX_DUTY; |
j_j205 | 14:d327852dafd1 | 304 | dutyR = MAX_DUTY; |
j_j205 | 14:d327852dafd1 | 305 | servoL.write(DUTY[dutyL]); |
j_j205 | 14:d327852dafd1 | 306 | servoR.write(DUTY[dutyR]); |
j_j205 | 14:d327852dafd1 | 307 | wait(0.2); |
j_j205 | 14:d327852dafd1 | 308 | |
j_j205 | 14:d327852dafd1 | 309 | localizeLeftFlag = 1; |
j_j205 | 14:d327852dafd1 | 310 | } |
j_j205 | 14:d327852dafd1 | 311 | |
j_j205 | 14:d327852dafd1 | 312 | // FUNCTION: |
j_j205 | 6:5e24ff86b743 | 313 | // void localizeLeft() |
j_j205 | 6:5e24ff86b743 | 314 | // IN-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 315 | // None |
j_j205 | 6:5e24ff86b743 | 316 | // OUT-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 317 | // None |
j_j205 | 6:5e24ff86b743 | 318 | // DESCRIPTION: |
j_j205 | 6:5e24ff86b743 | 319 | // Using sensor longRangeR this method will localize to the wall |
j_j205 | 6:5e24ff86b743 | 320 | // to the left during stretches of forward motion where robot |
j_j205 | 6:5e24ff86b743 | 321 | // should remain xxx distance from the left wall. |
j_j205 | 6:5e24ff86b743 | 322 | void Scanner::localizeLeft() |
Hellkite85 | 7:52c4be3bfc1b | 323 | { |
Hellkite85 | 7:52c4be3bfc1b | 324 | float distLocalR = longRangeR.distInchesR(); |
j_j205 | 6:5e24ff86b743 | 325 | |
j_j205 | 14:d327852dafd1 | 326 | if (distLocalR >= 8.5) |
Hellkite85 | 7:52c4be3bfc1b | 327 | { |
Hellkite85 | 7:52c4be3bfc1b | 328 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 329 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 330 | drive.move(0.0, ((-5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 331 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 332 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 333 | { |
j_j205 | 14:d327852dafd1 | 334 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 335 | } |
Hellkite85 | 7:52c4be3bfc1b | 336 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 337 | pitEnable = 1; |
Hellkite85 | 7:52c4be3bfc1b | 338 | } |
j_j205 | 14:d327852dafd1 | 339 | else if (distLocalR <= 7.5) |
Hellkite85 | 7:52c4be3bfc1b | 340 | { |
Hellkite85 | 7:52c4be3bfc1b | 341 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 342 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 343 | drive.move(0.0, ((5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 344 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 345 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 346 | { |
j_j205 | 14:d327852dafd1 | 347 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 348 | } |
Hellkite85 | 7:52c4be3bfc1b | 349 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 350 | pitEnable = 1; |
Hellkite85 | 7:52c4be3bfc1b | 351 | } |
j_j205 | 6:5e24ff86b743 | 352 | } |
j_j205 | 6:5e24ff86b743 | 353 | |
j_j205 | 6:5e24ff86b743 | 354 | // FUNCTION: |
j_j205 | 14:d327852dafd1 | 355 | // void localizeRightMode() |
j_j205 | 14:d327852dafd1 | 356 | // IN-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 357 | // None |
j_j205 | 14:d327852dafd1 | 358 | // OUT-PARAMETERS: |
j_j205 | 14:d327852dafd1 | 359 | // None |
j_j205 | 14:d327852dafd1 | 360 | // DESCRIPTION: |
j_j205 | 14:d327852dafd1 | 361 | // Initializes to localizeRight() |
Hellkite85 | 15:3967cb2d93f5 | 362 | void Scanner::localizeRightMode() |
j_j205 | 14:d327852dafd1 | 363 | { |
j_j205 | 14:d327852dafd1 | 364 | localizeLeftFlag = 0; |
j_j205 | 14:d327852dafd1 | 365 | |
j_j205 | 14:d327852dafd1 | 366 | dutyL = MIN_DUTY; |
j_j205 | 14:d327852dafd1 | 367 | dutyR = MIN_DUTY; |
j_j205 | 14:d327852dafd1 | 368 | servoL.write(DUTY[dutyL]); |
j_j205 | 14:d327852dafd1 | 369 | servoR.write(DUTY[dutyR]); |
j_j205 | 14:d327852dafd1 | 370 | wait(0.2); |
j_j205 | 14:d327852dafd1 | 371 | |
j_j205 | 14:d327852dafd1 | 372 | localizeRightFlag = 1; |
j_j205 | 14:d327852dafd1 | 373 | } |
j_j205 | 14:d327852dafd1 | 374 | |
j_j205 | 14:d327852dafd1 | 375 | // FUNCTION: |
j_j205 | 6:5e24ff86b743 | 376 | // void localizeRight() |
j_j205 | 6:5e24ff86b743 | 377 | // IN-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 378 | // None |
j_j205 | 6:5e24ff86b743 | 379 | // OUT-PARAMETERS: |
j_j205 | 6:5e24ff86b743 | 380 | // None |
j_j205 | 6:5e24ff86b743 | 381 | // DESCRIPTION: |
j_j205 | 6:5e24ff86b743 | 382 | // Using sensor longRangeL this method will localize to the wall |
j_j205 | 6:5e24ff86b743 | 383 | // to the left during stretches of forward motion where robot |
j_j205 | 6:5e24ff86b743 | 384 | // should remain xxx distance from the left wall. This function |
j_j205 | 6:5e24ff86b743 | 385 | // will be called from scan when the localizeRightFlag is set. |
Hellkite85 | 7:52c4be3bfc1b | 386 | void Scanner::localizeRight() |
j_j205 | 6:5e24ff86b743 | 387 | { |
j_j205 | 14:d327852dafd1 | 388 | dutyL = MIN_DUTY; |
j_j205 | 14:d327852dafd1 | 389 | dutyR = MIN_DUTY; |
j_j205 | 14:d327852dafd1 | 390 | servoL.write(DUTY[dutyL]); |
j_j205 | 14:d327852dafd1 | 391 | servoR.write(DUTY[dutyR]); |
Hellkite85 | 7:52c4be3bfc1b | 392 | float distLocalL = longRangeL.distInchesL(); |
j_j205 | 6:5e24ff86b743 | 393 | |
j_j205 | 14:d327852dafd1 | 394 | if (distLocalL >= 8.5) |
Hellkite85 | 7:52c4be3bfc1b | 395 | { |
Hellkite85 | 7:52c4be3bfc1b | 396 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 397 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 398 | drive.move(0, ((5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 399 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 400 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 401 | { |
j_j205 | 14:d327852dafd1 | 402 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 403 | } |
Hellkite85 | 7:52c4be3bfc1b | 404 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 405 | pitEnable = 1; |
Hellkite85 | 7:52c4be3bfc1b | 406 | } |
j_j205 | 14:d327852dafd1 | 407 | else if (distLocalL <= 7.5) |
Hellkite85 | 7:52c4be3bfc1b | 408 | { |
Hellkite85 | 7:52c4be3bfc1b | 409 | pitEnable = 0; |
Hellkite85 | 7:52c4be3bfc1b | 410 | drive.pauseMove(); |
Hellkite85 | 15:3967cb2d93f5 | 411 | drive.move(0, ((-5.0)*(3.14159 / 180.0))); |
j_j205 | 14:d327852dafd1 | 412 | // wait for move to complete |
j_j205 | 14:d327852dafd1 | 413 | while(!drive.isMoveDone()) |
j_j205 | 14:d327852dafd1 | 414 | { |
j_j205 | 14:d327852dafd1 | 415 | wait(1e-6); |
j_j205 | 14:d327852dafd1 | 416 | } |
Hellkite85 | 7:52c4be3bfc1b | 417 | drive.resumeMove(); |
Hellkite85 | 7:52c4be3bfc1b | 418 | pitEnable = 1; |
Hellkite85 | 7:52c4be3bfc1b | 419 | } |
j_j205 | 6:5e24ff86b743 | 420 | } |
j_j205 | 6:5e24ff86b743 | 421 | |
j_j205 | 14:d327852dafd1 | 422 | void Scanner |
j_j205 | 14:d327852dafd1 | 423 | |
j_j205 | 6:5e24ff86b743 | 424 | // FUNCTION: |
j_j205 | 0:999bb8fcd0b2 | 425 | // scan() |
j_j205 | 0:999bb8fcd0b2 | 426 | // IN-PARAMETERS: |
j_j205 | 2:b281034eda86 | 427 | // None |
j_j205 | 0:999bb8fcd0b2 | 428 | // OUT-PARAMETERS: |
j_j205 | 2:b281034eda86 | 429 | // None |
j_j205 | 0:999bb8fcd0b2 | 430 | // DESCRIPTION: |
j_j205 | 2:b281034eda86 | 431 | // Ticker function to make servos scan either the inward-facing |
j_j205 | 2:b281034eda86 | 432 | // 90 degrees or the outward-facing 90 degrees. Distance |
j_j205 | 2:b281034eda86 | 433 | // measures are obtained from both the short range and long |
j_j205 | 2:b281034eda86 | 434 | // range sensors at 15 degree intervals. |
j_j205 | 0:999bb8fcd0b2 | 435 | void Scanner::scan() |
j_j205 | 0:999bb8fcd0b2 | 436 | { |
j_j205 | 2:b281034eda86 | 437 | if (pitEnable == 1) |
j_j205 | 2:b281034eda86 | 438 | { |
j_j205 | 13:8767be1c5cc2 | 439 | if (localizeRightFlag == 1) |
j_j205 | 13:8767be1c5cc2 | 440 | localizeRight(); |
j_j205 | 13:8767be1c5cc2 | 441 | |
j_j205 | 14:d327852dafd1 | 442 | /*if (localizeLeftFlag == 1) |
j_j205 | 14:d327852dafd1 | 443 | localizeLeft();*/ |
j_j205 | 13:8767be1c5cc2 | 444 | |
j_j205 | 6:5e24ff86b743 | 445 | /*// obtain distance measures from sensors |
j_j205 | 5:4d5601a9dffe | 446 | distLeft = longRangeL.distInchesL(); |
j_j205 | 5:4d5601a9dffe | 447 | distRight = longRangeR.distInchesR(); |
j_j205 | 5:4d5601a9dffe | 448 | |
j_j205 | 6:5e24ff86b743 | 449 | //check localize flags and make necessary method calls |
j_j205 | 6:5e24ff86b743 | 450 | |
j_j205 | 3:992558a021d7 | 451 | // check avoid flag |
j_j205 | 5:4d5601a9dffe | 452 | if (avoidFlag == 1) |
j_j205 | 5:4d5601a9dffe | 453 | { |
j_j205 | 5:4d5601a9dffe | 454 | // add obstacle avoid code |
j_j205 | 5:4d5601a9dffe | 455 | } |
j_j205 | 5:4d5601a9dffe | 456 | |
j_j205 | 3:992558a021d7 | 457 | // check hunt flag |
j_j205 | 6:5e24ff86b743 | 458 | if (huntFlag == 1) |
j_j205 | 5:4d5601a9dffe | 459 | { |
j_j205 | 5:4d5601a9dffe | 460 | // add hunt code |
j_j205 | 6:5e24ff86b743 | 461 | }*/ |
j_j205 | 3:992558a021d7 | 462 | |
j_j205 | 13:8767be1c5cc2 | 463 | /* // increment/decrement servo position |
j_j205 | 2:b281034eda86 | 464 | dutyL += invertL; |
j_j205 | 2:b281034eda86 | 465 | dutyR += invertR; |
j_j205 | 2:b281034eda86 | 466 | servoL.write(DUTY[dutyL]); |
j_j205 | 2:b281034eda86 | 467 | servoR.write(DUTY[dutyR]); |
j_j205 | 6:5e24ff86b743 | 468 | if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) || |
j_j205 | 2:b281034eda86 | 469 | ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY))) |
j_j205 | 2:b281034eda86 | 470 | { |
j_j205 | 2:b281034eda86 | 471 | invertL *= -1; |
j_j205 | 2:b281034eda86 | 472 | invertR *= -1; |
j_j205 | 13:8767be1c5cc2 | 473 | }*/ |
j_j205 | 2:b281034eda86 | 474 | } |
j_j205 | 3:992558a021d7 | 475 | } |