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@14:d327852dafd1, 2016-04-01 (annotated)

Committer:
j_j205
Date:
Fri Apr 01 15:55:52 2016 +0000
Revision:
14:d327852dafd1
Parent:
13:8767be1c5cc2
Child:
15:3967cb2d93f5
4/1/16 JJ

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 9:bae63bc84829 162
Hellkite85 9:bae63bc84829 163 moveRobot.distance = 0.25;
Hellkite85 9:bae63bc84829 164 moveRobot.angle = 0.0;
Hellkite85 9:bae63bc84829 165
Hellkite85 9:bae63bc84829 166 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 167 }
Hellkite85 7:52c4be3bfc1b 168
Hellkite85 7:52c4be3bfc1b 169 // Adjust the comparsion for floats with
Hellkite85 9:bae63bc84829 170 // regards to the sensor data. This
Hellkite85 9:bae63bc84829 171 // while loop ideally checks the short range
Hellkite85 7:52c4be3bfc1b 172 // sensors and if they are not equal to
Hellkite85 7:52c4be3bfc1b 173 // each other, the code will center the
Hellkite85 9:bae63bc84829 174 // robot according to the short range sensor.
Hellkite85 9:bae63bc84829 175 while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance))
Hellkite85 7:52c4be3bfc1b 176 {
Hellkite85 7:52c4be3bfc1b 177 // Adjust this comparsion for floats
Hellkite85 7:52c4be3bfc1b 178 // with regards to the sensor data.
Hellkite85 9:bae63bc84829 179 if(distShortL > (distShortR + tolerance))
Hellkite85 9:bae63bc84829 180 {
Hellkite85 7:52c4be3bfc1b 181 drive.move(0.0, -5.0);
Hellkite85 9:bae63bc84829 182
Hellkite85 9:bae63bc84829 183 moveRobot.distance = 0.0;
Hellkite85 9:bae63bc84829 184 moveRobot.angle = -5.0;
Hellkite85 9:bae63bc84829 185
Hellkite85 9:bae63bc84829 186 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 187 }
Hellkite85 7:52c4be3bfc1b 188 else
Hellkite85 9:bae63bc84829 189 {
Hellkite85 7:52c4be3bfc1b 190 drive.move(0.0, 5.0);
Hellkite85 9:bae63bc84829 191
Hellkite85 9:bae63bc84829 192 moveRobot.distance = 0.0;
Hellkite85 9:bae63bc84829 193 moveRobot.angle = 5.0;
Hellkite85 9:bae63bc84829 194
Hellkite85 9:bae63bc84829 195 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 196 }
Hellkite85 7:52c4be3bfc1b 197 }
Hellkite85 7:52c4be3bfc1b 198
Hellkite85 7:52c4be3bfc1b 199 servoL.write(DUTY[dutyR]);
Hellkite85 7:52c4be3bfc1b 200 servoR.write(DUTY[dutyL]);
Hellkite85 7:52c4be3bfc1b 201
Hellkite85 9:bae63bc84829 202 // This code reinitializes the short distance
Hellkite85 9:bae63bc84829 203 // variable now that the short distance sensors
Hellkite85 9:bae63bc84829 204 // are pointing inward.
Hellkite85 9:bae63bc84829 205 distShortL = shortRangeL.getRange();
Hellkite85 9:bae63bc84829 206 distShortR = shortRangeR.getRange();
Hellkite85 7:52c4be3bfc1b 207
Hellkite85 7:52c4be3bfc1b 208 // This while loop is used to position the robot
Hellkite85 9:bae63bc84829 209 // at the correct location vertically with regards
Hellkite85 9:bae63bc84829 210 // to the peg in order to pick up the peg. The tolerance
Hellkite85 9:bae63bc84829 211 while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR))
Hellkite85 7:52c4be3bfc1b 212 {
Hellkite85 9:bae63bc84829 213 drive.move(0.125, 0.0);
Hellkite85 9:bae63bc84829 214
Hellkite85 9:bae63bc84829 215 moveRobot.distance = 0.125;
Hellkite85 9:bae63bc84829 216 moveRobot.angle = 0.0;
Hellkite85 9:bae63bc84829 217
Hellkite85 9:bae63bc84829 218 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 219 }
Hellkite85 9:bae63bc84829 220
Hellkite85 9:bae63bc84829 221 // This is the code to enable the gripper to grip the peg.
j_j205 12:514544a4014f 222 robotGrip.grip();
Hellkite85 9:bae63bc84829 223
Hellkite85 9:bae63bc84829 224 /* pseudocode
Hellkite85 9:bae63bc84829 225 implement the code for reading a value from the color sensor
Hellkite85 9:bae63bc84829 226 store the value that is returned from the color sensor
Hellkite85 9:bae63bc84829 227 */
Hellkite85 9:bae63bc84829 228
Hellkite85 9:bae63bc84829 229 // This is the code to enable the gripper to lift the peg.
Hellkite85 9:bae63bc84829 230 robotGrip.lift();
Hellkite85 9:bae63bc84829 231
Hellkite85 9:bae63bc84829 232 // This code uses the stack to reverse all the movement
Hellkite85 9:bae63bc84829 233 // of the robot during this function.
Hellkite85 9:bae63bc84829 234 while(!myStack.empty())
Hellkite85 9:bae63bc84829 235 {
Hellkite85 9:bae63bc84829 236 myStack.top();
Hellkite85 9:bae63bc84829 237 myStack.pop();
Hellkite85 9:bae63bc84829 238
Hellkite85 9:bae63bc84829 239 drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));
Hellkite85 9:bae63bc84829 240 }
Hellkite85 7:52c4be3bfc1b 241 }
j_j205 0:999bb8fcd0b2 242
j_j205 0:999bb8fcd0b2 243 // FUNCTION:
j_j205 0:999bb8fcd0b2 244 // localize()
j_j205 0:999bb8fcd0b2 245 // IN-PARAMETERS:
j_j205 2:b281034eda86 246 // None
j_j205 0:999bb8fcd0b2 247 // OUT-PARAMETERS:
j_j205 2:b281034eda86 248 // None
j_j205 0:999bb8fcd0b2 249 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 250 // Checks localization points.
j_j205 3:992558a021d7 251 void Scanner::localize()
j_j205 0:999bb8fcd0b2 252 {
j_j205 3:992558a021d7 253 pitEnable = 0;
j_j205 3:992558a021d7 254 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 255 dutyR = MAX_DUTY;
j_j205 3:992558a021d7 256 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 257 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 258 wait(0.1);
j_j205 5:4d5601a9dffe 259 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 260 distRight = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 261 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 262 dutyR = MIN_DUTY;
j_j205 3:992558a021d7 263 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 264 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 265 wait(0.1);
j_j205 6:5e24ff86b743 266 distForwardL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 267 distForwardR = longRangeR.distInchesR();
j_j205 3:992558a021d7 268 pitEnable = 1;
j_j205 0:999bb8fcd0b2 269 }
j_j205 0:999bb8fcd0b2 270
j_j205 0:999bb8fcd0b2 271 // FUNCTION:
j_j205 14:d327852dafd1 272 // void localizeLeftMode()
j_j205 14:d327852dafd1 273 // IN-PARAMETERS:
j_j205 14:d327852dafd1 274 // None
j_j205 14:d327852dafd1 275 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 276 // None
j_j205 14:d327852dafd1 277 // DESCRIPTION:
j_j205 14:d327852dafd1 278 // This method initializes to localizeLeft()
j_j205 14:d327852dafd1 279 void Scanner::localizeLeftMode()
j_j205 14:d327852dafd1 280 {
j_j205 14:d327852dafd1 281 localizeRightFlag = 0;
j_j205 14:d327852dafd1 282
j_j205 14:d327852dafd1 283 dutyL = MAX_DUTY;
j_j205 14:d327852dafd1 284 dutyR = MAX_DUTY;
j_j205 14:d327852dafd1 285 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 286 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 287 wait(0.2);
j_j205 14:d327852dafd1 288
j_j205 14:d327852dafd1 289 localizeLeftFlag = 1;
j_j205 14:d327852dafd1 290 }
j_j205 14:d327852dafd1 291
j_j205 14:d327852dafd1 292 // FUNCTION:
j_j205 6:5e24ff86b743 293 // void localizeLeft()
j_j205 6:5e24ff86b743 294 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 295 // None
j_j205 6:5e24ff86b743 296 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 297 // None
j_j205 6:5e24ff86b743 298 // DESCRIPTION:
j_j205 6:5e24ff86b743 299 // Using sensor longRangeR this method will localize to the wall
j_j205 6:5e24ff86b743 300 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 301 // should remain xxx distance from the left wall.
j_j205 6:5e24ff86b743 302 void Scanner::localizeLeft()
Hellkite85 7:52c4be3bfc1b 303 {
Hellkite85 7:52c4be3bfc1b 304 float distLocalR = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 305
j_j205 14:d327852dafd1 306 if (distLocalR >= 8.5)
Hellkite85 7:52c4be3bfc1b 307 {
Hellkite85 7:52c4be3bfc1b 308 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 309 drive.pauseMove();
j_j205 14:d327852dafd1 310 drive.move(0.0, ((-0.5)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 311 // wait for move to complete
j_j205 14:d327852dafd1 312 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 313 {
j_j205 14:d327852dafd1 314 wait(1e-6);
j_j205 14:d327852dafd1 315 }
Hellkite85 7:52c4be3bfc1b 316 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 317 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 318 }
j_j205 14:d327852dafd1 319 else if (distLocalR <= 7.5)
Hellkite85 7:52c4be3bfc1b 320 {
Hellkite85 7:52c4be3bfc1b 321 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 322 drive.pauseMove();
j_j205 14:d327852dafd1 323 drive.move(0.0, ((0.5)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 324 // wait for move to complete
j_j205 14:d327852dafd1 325 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 326 {
j_j205 14:d327852dafd1 327 wait(1e-6);
j_j205 14:d327852dafd1 328 }
Hellkite85 7:52c4be3bfc1b 329 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 330 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 331 }
j_j205 6:5e24ff86b743 332 }
j_j205 6:5e24ff86b743 333
j_j205 6:5e24ff86b743 334 // FUNCTION:
j_j205 14:d327852dafd1 335 // void localizeRightMode()
j_j205 14:d327852dafd1 336 // IN-PARAMETERS:
j_j205 14:d327852dafd1 337 // None
j_j205 14:d327852dafd1 338 // OUT-PARAMETERS:
j_j205 14:d327852dafd1 339 // None
j_j205 14:d327852dafd1 340 // DESCRIPTION:
j_j205 14:d327852dafd1 341 // Initializes to localizeRight()
j_j205 14:d327852dafd1 342 void Scanner::localizeRight()
j_j205 14:d327852dafd1 343 {
j_j205 14:d327852dafd1 344 localizeLeftFlag = 0;
j_j205 14:d327852dafd1 345
j_j205 14:d327852dafd1 346 dutyL = MIN_DUTY;
j_j205 14:d327852dafd1 347 dutyR = MIN_DUTY;
j_j205 14:d327852dafd1 348 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 349 servoR.write(DUTY[dutyR]);
j_j205 14:d327852dafd1 350 wait(0.2);
j_j205 14:d327852dafd1 351
j_j205 14:d327852dafd1 352 localizeRightFlag = 1;
j_j205 14:d327852dafd1 353 }
j_j205 14:d327852dafd1 354
j_j205 14:d327852dafd1 355 // FUNCTION:
j_j205 6:5e24ff86b743 356 // void localizeRight()
j_j205 6:5e24ff86b743 357 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 358 // None
j_j205 6:5e24ff86b743 359 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 360 // None
j_j205 6:5e24ff86b743 361 // DESCRIPTION:
j_j205 6:5e24ff86b743 362 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 363 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 364 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 365 // will be called from scan when the localizeRightFlag is set.
Hellkite85 7:52c4be3bfc1b 366 void Scanner::localizeRight()
j_j205 6:5e24ff86b743 367 {
j_j205 14:d327852dafd1 368 dutyL = MIN_DUTY;
j_j205 14:d327852dafd1 369 dutyR = MIN_DUTY;
j_j205 14:d327852dafd1 370 servoL.write(DUTY[dutyL]);
j_j205 14:d327852dafd1 371 servoR.write(DUTY[dutyR]);
Hellkite85 7:52c4be3bfc1b 372 float distLocalL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 373
j_j205 14:d327852dafd1 374 if (distLocalL >= 8.5)
Hellkite85 7:52c4be3bfc1b 375 {
Hellkite85 7:52c4be3bfc1b 376 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 377 drive.pauseMove();
j_j205 14:d327852dafd1 378 drive.move(0, ((2.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 379 // wait for move to complete
j_j205 14:d327852dafd1 380 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 381 {
j_j205 14:d327852dafd1 382 wait(1e-6);
j_j205 14:d327852dafd1 383 }
Hellkite85 7:52c4be3bfc1b 384 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 385 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 386 }
j_j205 14:d327852dafd1 387 else if (distLocalL <= 7.5)
Hellkite85 7:52c4be3bfc1b 388 {
Hellkite85 7:52c4be3bfc1b 389 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 390 drive.pauseMove();
j_j205 14:d327852dafd1 391 drive.move(0, ((-2.0)*(3.14159 / 180.0)));
j_j205 14:d327852dafd1 392 // wait for move to complete
j_j205 14:d327852dafd1 393 while(!drive.isMoveDone())
j_j205 14:d327852dafd1 394 {
j_j205 14:d327852dafd1 395 wait(1e-6);
j_j205 14:d327852dafd1 396 }
Hellkite85 7:52c4be3bfc1b 397 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 398 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 399 }
j_j205 6:5e24ff86b743 400 }
j_j205 6:5e24ff86b743 401
j_j205 14:d327852dafd1 402 void Scanner
j_j205 14:d327852dafd1 403
j_j205 6:5e24ff86b743 404 // FUNCTION:
j_j205 0:999bb8fcd0b2 405 // scan()
j_j205 0:999bb8fcd0b2 406 // IN-PARAMETERS:
j_j205 2:b281034eda86 407 // None
j_j205 0:999bb8fcd0b2 408 // OUT-PARAMETERS:
j_j205 2:b281034eda86 409 // None
j_j205 0:999bb8fcd0b2 410 // DESCRIPTION:
j_j205 2:b281034eda86 411 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 412 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 413 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 414 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 415 void Scanner::scan()
j_j205 0:999bb8fcd0b2 416 {
j_j205 2:b281034eda86 417 if (pitEnable == 1)
j_j205 2:b281034eda86 418 {
j_j205 13:8767be1c5cc2 419 if (localizeRightFlag == 1)
j_j205 13:8767be1c5cc2 420 localizeRight();
j_j205 13:8767be1c5cc2 421
j_j205 14:d327852dafd1 422 /*if (localizeLeftFlag == 1)
j_j205 14:d327852dafd1 423 localizeLeft();*/
j_j205 13:8767be1c5cc2 424
j_j205 6:5e24ff86b743 425 /*// obtain distance measures from sensors
j_j205 5:4d5601a9dffe 426 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 427 distRight = longRangeR.distInchesR();
j_j205 5:4d5601a9dffe 428
j_j205 6:5e24ff86b743 429 //check localize flags and make necessary method calls
j_j205 6:5e24ff86b743 430
j_j205 3:992558a021d7 431 // check avoid flag
j_j205 5:4d5601a9dffe 432 if (avoidFlag == 1)
j_j205 5:4d5601a9dffe 433 {
j_j205 5:4d5601a9dffe 434 // add obstacle avoid code
j_j205 5:4d5601a9dffe 435 }
j_j205 5:4d5601a9dffe 436
j_j205 3:992558a021d7 437 // check hunt flag
j_j205 6:5e24ff86b743 438 if (huntFlag == 1)
j_j205 5:4d5601a9dffe 439 {
j_j205 5:4d5601a9dffe 440 // add hunt code
j_j205 6:5e24ff86b743 441 }*/
j_j205 3:992558a021d7 442
j_j205 13:8767be1c5cc2 443 /* // increment/decrement servo position
j_j205 2:b281034eda86 444 dutyL += invertL;
j_j205 2:b281034eda86 445 dutyR += invertR;
j_j205 2:b281034eda86 446 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 447 servoR.write(DUTY[dutyR]);
j_j205 6:5e24ff86b743 448 if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
j_j205 2:b281034eda86 449 ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
j_j205 2:b281034eda86 450 {
j_j205 2:b281034eda86 451 invertL *= -1;
j_j205 2:b281034eda86 452 invertR *= -1;
j_j205 13:8767be1c5cc2 453 }*/
j_j205 2:b281034eda86 454 }
j_j205 3:992558a021d7 455 }