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@17:ec04d60f38bd, 2016-04-06 (annotated)

Committer:
j_j205
Date:
Wed Apr 06 22:05:34 2016 +0000
Revision:
17:ec04d60f38bd
Parent:
15:3967cb2d93f5
Child:
18:f5c46fb2cd31
changes in localization

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 {
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 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 {
j_j205 17:ec04d60f38bd 324 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 325 float distLocalR = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 326
j_j205 17:ec04d60f38bd 327 if (distLocalR >= 9.2)
Hellkite85 7:52c4be3bfc1b 328 {
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 }
j_j205 17:ec04d60f38bd 338 else if (distLocalR <= 8.2)
Hellkite85 7:52c4be3bfc1b 339 {
Hellkite85 7:52c4be3bfc1b 340 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 341 drive.move(0.0, ((5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 342 // wait for move to complete
j_j205 14:d327852dafd1 343 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 344 {
j_j205 14:d327852dafd1 345 wait(1e-6);
j_j205 14:d327852dafd1 346 }
Hellkite85 7:52c4be3bfc1b 347 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 348 }
j_j205 17:ec04d60f38bd 349
j_j205 17:ec04d60f38bd 350 // wait for move to complete
j_j205 17:ec04d60f38bd 351 while(!drive.isMoveDone())
j_j205 17:ec04d60f38bd 352 {
j_j205 17:ec04d60f38bd 353 wait(1e-6);
j_j205 17:ec04d60f38bd 354 }
j_j205 17:ec04d60f38bd 355 pitEnable = 1;
j_j205 6:5e24ff86b743 356 }
j_j205 6:5e24ff86b743 357
j_j205 6:5e24ff86b743 358 // FUNCTION:
j_j205 14:d327852dafd1 359 // void localizeRightMode()
j_j205 14:d327852dafd1 360 // IN-PARAMETERS:
j_j205 14:d327852dafd1 361 // None
j_j205 14:d327852dafd1 362 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 363 // None
j_j205 14:d327852dafd1 364 // DESCRIPTION:
j_j205 14:d327852dafd1 365 // Initializes to localizeRight()
Hellkite85 15:3967cb2d93f5 366 void Scanner::localizeRightMode()
j_j205 14:d327852dafd1 367 {
j_j205 14:d327852dafd1 368 localizeLeftFlag = 0;
j_j205 14:d327852dafd1 369
j_j205 14:d327852dafd1 370 dutyL = MIN_DUTY;
j_j205 14:d327852dafd1 371 dutyR = MIN_DUTY;
j_j205 14:d327852dafd1 372 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 373 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 374 wait(0.2);
j_j205 14:d327852dafd1 375
j_j205 14:d327852dafd1 376 localizeRightFlag = 1;
j_j205 14:d327852dafd1 377 }
j_j205 14:d327852dafd1 378
j_j205 14:d327852dafd1 379 // FUNCTION:
j_j205 6:5e24ff86b743 380 // void localizeRight()
j_j205 6:5e24ff86b743 381 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 382 // None
j_j205 6:5e24ff86b743 383 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 384 // None
j_j205 6:5e24ff86b743 385 // DESCRIPTION:
j_j205 6:5e24ff86b743 386 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 387 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 388 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 389 // will be called from scan when the localizeRightFlag is set.
Hellkite85 7:52c4be3bfc1b 390 void Scanner::localizeRight()
j_j205 6:5e24ff86b743 391 {
j_j205 17:ec04d60f38bd 392 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 393 float distLocalL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 394
j_j205 17:ec04d60f38bd 395 if (distLocalL >= 9.2)
Hellkite85 7:52c4be3bfc1b 396 {
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 }
j_j205 17:ec04d60f38bd 406 else if (distLocalL <= 8.2)
Hellkite85 7:52c4be3bfc1b 407 {
Hellkite85 7:52c4be3bfc1b 408 drive.pauseMove();
Hellkite85 15:3967cb2d93f5 409 drive.move(0, ((-5.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 410 // wait for move to complete
j_j205 14:d327852dafd1 411 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 412 {
j_j205 14:d327852dafd1 413 wait(1e-6);
j_j205 14:d327852dafd1 414 }
Hellkite85 7:52c4be3bfc1b 415 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 416 }
j_j205 17:ec04d60f38bd 417
j_j205 17:ec04d60f38bd 418 // wait for move to complete
j_j205 17:ec04d60f38bd 419 while(!drive.isMoveDone())
j_j205 17:ec04d60f38bd 420 {
j_j205 17:ec04d60f38bd 421 wait(1e-6);
j_j205 17:ec04d60f38bd 422 }
j_j205 17:ec04d60f38bd 423 pitEnable = 1;
j_j205 6:5e24ff86b743 424 }
j_j205 6:5e24ff86b743 425
j_j205 6:5e24ff86b743 426 // FUNCTION:
j_j205 0:999bb8fcd0b2 427 // scan()
j_j205 0:999bb8fcd0b2 428 // IN-PARAMETERS:
j_j205 2:b281034eda86 429 // None
j_j205 0:999bb8fcd0b2 430 // OUT-PARAMETERS:
j_j205 2:b281034eda86 431 // None
j_j205 0:999bb8fcd0b2 432 // DESCRIPTION:
j_j205 2:b281034eda86 433 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 434 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 435 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 436 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 437 void Scanner::scan()
j_j205 0:999bb8fcd0b2 438 {
j_j205 2:b281034eda86 439 if (pitEnable == 1)
j_j205 2:b281034eda86 440 {
j_j205 13:8767be1c5cc2 441 if (localizeRightFlag == 1)
j_j205 13:8767be1c5cc2 442 localizeRight();
j_j205 13:8767be1c5cc2 443
j_j205 14:d327852dafd1 444 /*if (localizeLeftFlag == 1)
j_j205 14:d327852dafd1 445 localizeLeft();*/
j_j205 2:b281034eda86 446 }
j_j205 3:992558a021d7 447 }