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@21:bbadc0b8ed9f, 2017-02-27 (annotated)

Committer:
j_j205
Date:
Mon Feb 27 11:56:43 2017 +0000
Revision:
21:bbadc0b8ed9f
Parent:
19:795968997140 
uncommented code section

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 21:bbadc0b8ed9f 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 18:f5c46fb2cd31 21 LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period, ColorSensor &_colorSensor) : pc(pc1), drive(_drive), servoL(_servoL),
j_j205 2:b281034eda86 22 servoR(_servoR), shortRangeL(_shortRangeL),
j_j205 2:b281034eda86 23 shortRangeR(_shortRangeR), longRangeL(_longRangeL),
Hellkite85 18:f5c46fb2cd31 24 longRangeR(_longRangeR), robotGrip(_robotGrip), period(_period), colorSensor(_colorSensor)
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 {
j_j205 17:ec04d60f38bd 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 18:f5c46fb2cd31 237 // This code to enable the gripper to grip the peg.
j_j205 12:514544a4014f 238 robotGrip.grip();
Hellkite85 9:bae63bc84829 239
Hellkite85 18:f5c46fb2cd31 240 // This code is used to determine the color of the peg
Hellkite85 18:f5c46fb2cd31 241 // and the bool value will be used to control the
Hellkite85 18:f5c46fb2cd31 242 // navigation of the robot.
Hellkite85 18:f5c46fb2cd31 243 bool isPegColorRed;
Hellkite85 18:f5c46fb2cd31 244
Hellkite85 18:f5c46fb2cd31 245 int colorValue;
Hellkite85 18:f5c46fb2cd31 246 colorValue =
Hellkite85 18:f5c46fb2cd31 247
Hellkite85 18:f5c46fb2cd31 248 if(colorValue > 150)
Hellkite85 18:f5c46fb2cd31 249 isPegColorRed = true;
Hellkite85 18:f5c46fb2cd31 250 else
Hellkite85 18:f5c46fb2cd31 251 isPegColorRed = false;
Hellkite85 9:bae63bc84829 252
Hellkite85 9:bae63bc84829 253 // This is the code to enable the gripper to lift the peg.
Hellkite85 9:bae63bc84829 254 robotGrip.lift();
Hellkite85 9:bae63bc84829 255
Hellkite85 9:bae63bc84829 256 // This code uses the stack to reverse all the movement
Hellkite85 9:bae63bc84829 257 // of the robot during this function.
Hellkite85 9:bae63bc84829 258 while(!myStack.empty())
Hellkite85 9:bae63bc84829 259 {
Hellkite85 9:bae63bc84829 260 myStack.top();
Hellkite85 9:bae63bc84829 261 myStack.pop();
Hellkite85 9:bae63bc84829 262
Hellkite85 15:3967cb2d93f5 263 drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));
Hellkite85 15:3967cb2d93f5 264 while(!drive.isMoveDone())
Hellkite85 15:3967cb2d93f5 265 {
Hellkite85 15:3967cb2d93f5 266 wait(1e-6);
Hellkite85 15:3967cb2d93f5 267 }
Hellkite85 9:bae63bc84829 268 }
Hellkite85 7:52c4be3bfc1b 269 }
j_j205 0:999bb8fcd0b2 270
j_j205 0:999bb8fcd0b2 271 // FUNCTION:
j_j205 0:999bb8fcd0b2 272 // localize()
j_j205 0:999bb8fcd0b2 273 // IN-PARAMETERS:
j_j205 2:b281034eda86 274 // None
j_j205 0:999bb8fcd0b2 275 // OUT-PARAMETERS:
j_j205 2:b281034eda86 276 // None
j_j205 0:999bb8fcd0b2 277 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 278 // Checks localization points.
j_j205 3:992558a021d7 279 void Scanner::localize()
j_j205 0:999bb8fcd0b2 280 {
j_j205 3:992558a021d7 281 pitEnable = 0;
j_j205 3:992558a021d7 282 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 283 dutyR = MAX_DUTY;
j_j205 3:992558a021d7 284 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 285 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 286 wait(0.1);
j_j205 5:4d5601a9dffe 287 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 288 distRight = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 289 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 290 dutyR = MIN_DUTY;
j_j205 3:992558a021d7 291 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 292 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 293 wait(0.1);
j_j205 6:5e24ff86b743 294 distForwardL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 295 distForwardR = longRangeR.distInchesR();
j_j205 3:992558a021d7 296 pitEnable = 1;
j_j205 0:999bb8fcd0b2 297 }
j_j205 0:999bb8fcd0b2 298
j_j205 0:999bb8fcd0b2 299 // FUNCTION:
j_j205 14:d327852dafd1 300 // void localizeLeftMode()
j_j205 14:d327852dafd1 301 // IN-PARAMETERS:
j_j205 14:d327852dafd1 302 // None
j_j205 14:d327852dafd1 303 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 304 // None
j_j205 14:d327852dafd1 305 // DESCRIPTION:
j_j205 14:d327852dafd1 306 // This method initializes to localizeLeft()
j_j205 14:d327852dafd1 307 void Scanner::localizeLeftMode()
j_j205 14:d327852dafd1 308 {
j_j205 14:d327852dafd1 309 localizeRightFlag = 0;
j_j205 14:d327852dafd1 310
j_j205 14:d327852dafd1 311 dutyL = MAX_DUTY;
j_j205 14:d327852dafd1 312 dutyR = MAX_DUTY;
j_j205 14:d327852dafd1 313 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 314 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 315 wait(0.2);
j_j205 14:d327852dafd1 316
j_j205 14:d327852dafd1 317 localizeLeftFlag = 1;
j_j205 14:d327852dafd1 318 }
j_j205 14:d327852dafd1 319
j_j205 14:d327852dafd1 320 // FUNCTION:
j_j205 6:5e24ff86b743 321 // void localizeLeft()
j_j205 6:5e24ff86b743 322 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 323 // None
j_j205 6:5e24ff86b743 324 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 325 // None
j_j205 6:5e24ff86b743 326 // DESCRIPTION:
j_j205 6:5e24ff86b743 327 // Using sensor longRangeR this method will localize to the wall
j_j205 6:5e24ff86b743 328 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 329 // should remain xxx distance from the left wall.
j_j205 6:5e24ff86b743 330 void Scanner::localizeLeft()
Hellkite85 7:52c4be3bfc1b 331 {
j_j205 17:ec04d60f38bd 332 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 333 float distLocalR = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 334
j_j205 17:ec04d60f38bd 335 if (distLocalR >= 9.2)
Hellkite85 7:52c4be3bfc1b 336 {
Hellkite85 7:52c4be3bfc1b 337 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 338 drive.move(0.0, ((-5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 339 // wait for move to complete
j_j205 14:d327852dafd1 340 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 341 {
j_j205 14:d327852dafd1 342 wait(1e-6);
j_j205 14:d327852dafd1 343 }
Hellkite85 7:52c4be3bfc1b 344 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 345 }
j_j205 17:ec04d60f38bd 346 else if (distLocalR <= 8.2)
Hellkite85 7:52c4be3bfc1b 347 {
Hellkite85 7:52c4be3bfc1b 348 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 349 drive.move(0.0, ((5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 350 // wait for move to complete
j_j205 14:d327852dafd1 351 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 352 {
j_j205 14:d327852dafd1 353 wait(1e-6);
j_j205 14:d327852dafd1 354 }
Hellkite85 7:52c4be3bfc1b 355 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 356 }
j_j205 17:ec04d60f38bd 357
j_j205 17:ec04d60f38bd 358 // wait for move to complete
j_j205 17:ec04d60f38bd 359 while(!drive.isMoveDone())
j_j205 17:ec04d60f38bd 360 {
j_j205 17:ec04d60f38bd 361 wait(1e-6);
j_j205 17:ec04d60f38bd 362 }
j_j205 17:ec04d60f38bd 363 pitEnable = 1;
j_j205 6:5e24ff86b743 364 }
j_j205 6:5e24ff86b743 365
j_j205 6:5e24ff86b743 366 // FUNCTION:
j_j205 14:d327852dafd1 367 // void localizeRightMode()
j_j205 14:d327852dafd1 368 // IN-PARAMETERS:
j_j205 14:d327852dafd1 369 // None
j_j205 14:d327852dafd1 370 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 371 // None
j_j205 14:d327852dafd1 372 // DESCRIPTION:
j_j205 14:d327852dafd1 373 // Initializes to localizeRight()
Hellkite85 15:3967cb2d93f5 374 void Scanner::localizeRightMode()
j_j205 14:d327852dafd1 375 {
j_j205 14:d327852dafd1 376 localizeLeftFlag = 0;
j_j205 14:d327852dafd1 377
j_j205 14:d327852dafd1 378 dutyL = MIN_DUTY;
j_j205 14:d327852dafd1 379 dutyR = MIN_DUTY;
j_j205 14:d327852dafd1 380 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 381 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 382 wait(0.2);
j_j205 14:d327852dafd1 383
j_j205 14:d327852dafd1 384 localizeRightFlag = 1;
j_j205 14:d327852dafd1 385 }
j_j205 14:d327852dafd1 386
j_j205 14:d327852dafd1 387 // FUNCTION:
j_j205 6:5e24ff86b743 388 // void localizeRight()
j_j205 6:5e24ff86b743 389 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 390 // None
j_j205 6:5e24ff86b743 391 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 392 // None
j_j205 6:5e24ff86b743 393 // DESCRIPTION:
j_j205 6:5e24ff86b743 394 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 395 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 396 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 397 // will be called from scan when the localizeRightFlag is set.
Hellkite85 7:52c4be3bfc1b 398 void Scanner::localizeRight()
j_j205 6:5e24ff86b743 399 {
j_j205 17:ec04d60f38bd 400 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 401 float distLocalL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 402
j_j205 17:ec04d60f38bd 403 if (distLocalL >= 9.2)
Hellkite85 7:52c4be3bfc1b 404 {
Hellkite85 7:52c4be3bfc1b 405 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 406 drive.move(0, ((5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 407 // wait for move to complete
j_j205 14:d327852dafd1 408 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 409 {
j_j205 14:d327852dafd1 410 wait(1e-6);
j_j205 14:d327852dafd1 411 }
Hellkite85 7:52c4be3bfc1b 412 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 413 }
j_j205 17:ec04d60f38bd 414 else if (distLocalL <= 8.2)
Hellkite85 7:52c4be3bfc1b 415 {
Hellkite85 7:52c4be3bfc1b 416 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 417 drive.move(0, ((-5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 418 // wait for move to complete
j_j205 14:d327852dafd1 419 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 420 {
j_j205 14:d327852dafd1 421 wait(1e-6);
j_j205 14:d327852dafd1 422 }
Hellkite85 7:52c4be3bfc1b 423 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 424 }
j_j205 17:ec04d60f38bd 425
j_j205 17:ec04d60f38bd 426 // wait for move to complete
j_j205 17:ec04d60f38bd 427 while(!drive.isMoveDone())
j_j205 17:ec04d60f38bd 428 {
j_j205 17:ec04d60f38bd 429 wait(1e-6);
j_j205 17:ec04d60f38bd 430 }
j_j205 17:ec04d60f38bd 431 pitEnable = 1;
j_j205 6:5e24ff86b743 432 }
j_j205 6:5e24ff86b743 433
j_j205 6:5e24ff86b743 434 // FUNCTION:
j_j205 0:999bb8fcd0b2 435 // scan()
j_j205 0:999bb8fcd0b2 436 // IN-PARAMETERS:
j_j205 2:b281034eda86 437 // None
j_j205 0:999bb8fcd0b2 438 // OUT-PARAMETERS:
j_j205 2:b281034eda86 439 // None
j_j205 0:999bb8fcd0b2 440 // DESCRIPTION:
j_j205 2:b281034eda86 441 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 442 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 443 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 444 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 445 void Scanner::scan()
j_j205 0:999bb8fcd0b2 446 {
j_j205 2:b281034eda86 447 if (pitEnable == 1)
j_j205 2:b281034eda86 448 {
j_j205 13:8767be1c5cc2 449 if (localizeRightFlag == 1)
j_j205 13:8767be1c5cc2 450 localizeRight();
j_j205 13:8767be1c5cc2 451
j_j205 21:bbadc0b8ed9f 452 if (localizeLeftFlag == 1)
j_j205 19:795968997140 453 localizeLeft();
j_j205 2:b281034eda86 454 }
j_j205 21:bbadc0b8ed9f 455 }