Texas State IEEE / scanner

3/26/16 12:25 am JJ

Dependents:   steppertest R5 2016 Robotics Team 1

Fork of scanner by David Vasquez

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?

UserRevisionLine numberNew 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 }