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@9:bae63bc84829, 2016-03-26 (annotated)

Committer:
Hellkite85
Date:
Sat Mar 26 23:01:49 2016 +0000
Revision:
9:bae63bc84829
Parent:
8:32a445ae1d72
Child:
12:514544a4014f
Modified hunt function in scanner.cpp and scanner.h on 3-26-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),
Hellkite85 9:bae63bc84829 24 longRangeR(_longRangeR), grip(_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 2:b281034eda86 41 // initializing to avoid status
j_j205 3:992558a021d7 42 avoidFlag = 1;
j_j205 3:992558a021d7 43 huntFlag = 0;
j_j205 3:992558a021d7 44 invertL = -1;
j_j205 3:992558a021d7 45 invertR = 1;
j_j205 3:992558a021d7 46 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 47 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 48 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 49 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 50 wait(0.2);
j_j205 2:b281034eda86 51 pitEnable = 1;
j_j205 3:992558a021d7 52 scanPit.attach(this, &Scanner::scan, period);
j_j205 6:5e24ff86b743 53 pc.printf("Scanner created\n");
j_j205 0:999bb8fcd0b2 54 }
j_j205 0:999bb8fcd0b2 55
j_j205 0:999bb8fcd0b2 56 // FUNCTION:
j_j205 3:992558a021d7 57 // huntMode()
j_j205 0:999bb8fcd0b2 58 // IN-PARAMETERS:
j_j205 2:b281034eda86 59 // None
j_j205 2:b281034eda86 60 // None
j_j205 0:999bb8fcd0b2 61 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 62 // Hunts for victim.
j_j205 3:992558a021d7 63 void Scanner::huntMode()
j_j205 0:999bb8fcd0b2 64 {
j_j205 2:b281034eda86 65 pitEnable = 0;
j_j205 3:992558a021d7 66 avoidFlag = 0;
j_j205 3:992558a021d7 67 huntFlag = 1;
j_j205 3:992558a021d7 68 invertL = -1;
j_j205 3:992558a021d7 69 invertR = 1;
j_j205 6:5e24ff86b743 70 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 71 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 72 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 73 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 74 wait(0.1);
j_j205 2:b281034eda86 75 pitEnable = 1;
j_j205 0:999bb8fcd0b2 76 }
j_j205 0:999bb8fcd0b2 77
j_j205 0:999bb8fcd0b2 78 // FUNCTION:
j_j205 3:992558a021d7 79 // avoidMode()
j_j205 0:999bb8fcd0b2 80 // IN-PARAMETERS:
j_j205 2:b281034eda86 81 // None
j_j205 0:999bb8fcd0b2 82 // OUT-PARAMETERS:
j_j205 2:b281034eda86 83 // None
j_j205 0:999bb8fcd0b2 84 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 85 // Scans to avoid obstacles.
j_j205 3:992558a021d7 86 void Scanner::avoidMode()
j_j205 0:999bb8fcd0b2 87 {
j_j205 2:b281034eda86 88 pitEnable = 0;
j_j205 3:992558a021d7 89 avoidFlag = 1;
j_j205 3:992558a021d7 90 huntFlag = 0;
j_j205 3:992558a021d7 91 invertL = -1;
j_j205 3:992558a021d7 92 invertR = 1;
j_j205 3:992558a021d7 93 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 94 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 95 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 96 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 97 wait(0.1);
j_j205 2:b281034eda86 98 pitEnable = 1;
j_j205 0:999bb8fcd0b2 99 }
Hellkite85 7:52c4be3bfc1b 100 // FUNCTION:
Hellkite85 7:52c4be3bfc1b 101 //
Hellkite85 7:52c4be3bfc1b 102 void Scanner::hunt()
Hellkite85 7:52c4be3bfc1b 103 {
Hellkite85 9:bae63bc84829 104 // This creates an instance of a structure
Hellkite85 9:bae63bc84829 105 // call huntMove. This structure is used to
Hellkite85 9:bae63bc84829 106 // store the distance value and angle value
Hellkite85 9:bae63bc84829 107 // when implementing the Navigation drive
Hellkite85 9:bae63bc84829 108 // code.
Hellkite85 9:bae63bc84829 109 huntMove moveRobot;
Hellkite85 9:bae63bc84829 110
Hellkite85 7:52c4be3bfc1b 111 // Used to store temporary distance
Hellkite85 7:52c4be3bfc1b 112 // recorded from VL6180x short range
Hellkite85 7:52c4be3bfc1b 113 // sensor.
Hellkite85 7:52c4be3bfc1b 114 float tempDistShortL;
Hellkite85 7:52c4be3bfc1b 115 float tempDistShortR;
Hellkite85 9:bae63bc84829 116
Hellkite85 9:bae63bc84829 117 float distShortL;
Hellkite85 9:bae63bc84829 118 float distShortR;
Hellkite85 7:52c4be3bfc1b 119
Hellkite85 7:52c4be3bfc1b 120 // Used to temporary store the number
Hellkite85 7:52c4be3bfc1b 121 // of stepper motor increments.
Hellkite85 7:52c4be3bfc1b 122 int tempStepCount = 0;
Hellkite85 7:52c4be3bfc1b 123
Hellkite85 9:bae63bc84829 124 // This variable is half of the available
Hellkite85 9:bae63bc84829 125 // tolerance of the gripper with regards
Hellkite85 9:bae63bc84829 126 // to the peg.
Hellkite85 9:bae63bc84829 127 float tolerance = 0.1875;
Hellkite85 9:bae63bc84829 128
Hellkite85 9:bae63bc84829 129 // These variables represent the tolerance
Hellkite85 9:bae63bc84829 130 // and plus/minus half the diameter of the
Hellkite85 9:bae63bc84829 131 // peg measured in inches.
Hellkite85 9:bae63bc84829 132 float toleranceAHP = 0.9375;
Hellkite85 9:bae63bc84829 133 float toleranceSHP = 0.5625;
Hellkite85 9:bae63bc84829 134
Hellkite85 7:52c4be3bfc1b 135 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 136
Hellkite85 9:bae63bc84829 137 // This positions both of the servos to
Hellkite85 9:bae63bc84829 138 // be 60 degrees inwards.
Hellkite85 7:52c4be3bfc1b 139 servoL.write(DUTY[4]);
Hellkite85 7:52c4be3bfc1b 140 servoR.write(DUTY[2]);
Hellkite85 7:52c4be3bfc1b 141
Hellkite85 9:bae63bc84829 142 // This initializes the temporary distance variables
Hellkite85 9:bae63bc84829 143 // with the data collected from the VL6180X sensors.
Hellkite85 9:bae63bc84829 144 tempDistShortL = shortRangeL.getRange();
Hellkite85 9:bae63bc84829 145 tempDistShortR = shortRangeR.getRange();
Hellkite85 7:52c4be3bfc1b 146
Hellkite85 9:bae63bc84829 147 // This code converts the measurements from the short
Hellkite85 9:bae63bc84829 148 // range sensor from millimeters to inches.
Hellkite85 9:bae63bc84829 149 distShortL = (tempDistShortL / 25.0);
Hellkite85 9:bae63bc84829 150 distShortR = (tempDistShortR / 25.0);
Hellkite85 9:bae63bc84829 151
Hellkite85 9:bae63bc84829 152 // This while loop statement moves the robot forward in order
Hellkite85 9:bae63bc84829 153 // to continue the hunt of the peg. The equals to comparsion
Hellkite85 9:bae63bc84829 154 // statements need to be updated to reflect what the short
Hellkite85 9:bae63bc84829 155 // range sensors output when there is nothing within the
Hellkite85 9:bae63bc84829 156 // range of the short distance sensors.
Hellkite85 9:bae63bc84829 157 while(distShortL == 0.0 && distShortR == 0.0 && tempStepCount < 34)
Hellkite85 7:52c4be3bfc1b 158 {
Hellkite85 7:52c4be3bfc1b 159 drive.move(0.25, 0.0);
Hellkite85 9:bae63bc84829 160
Hellkite85 9:bae63bc84829 161 moveRobot.distance = 0.25;
Hellkite85 9:bae63bc84829 162 moveRobot.angle = 0.0;
Hellkite85 9:bae63bc84829 163
Hellkite85 9:bae63bc84829 164 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 165 }
Hellkite85 7:52c4be3bfc1b 166
Hellkite85 7:52c4be3bfc1b 167 // Adjust the comparsion for floats with
Hellkite85 9:bae63bc84829 168 // regards to the sensor data. This
Hellkite85 9:bae63bc84829 169 // while loop ideally checks the short range
Hellkite85 7:52c4be3bfc1b 170 // sensors and if they are not equal to
Hellkite85 7:52c4be3bfc1b 171 // each other, the code will center the
Hellkite85 9:bae63bc84829 172 // robot according to the short range sensor.
Hellkite85 9:bae63bc84829 173 while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance))
Hellkite85 7:52c4be3bfc1b 174 {
Hellkite85 7:52c4be3bfc1b 175 // Adjust this comparsion for floats
Hellkite85 7:52c4be3bfc1b 176 // with regards to the sensor data.
Hellkite85 9:bae63bc84829 177 if(distShortL > (distShortR + tolerance))
Hellkite85 9:bae63bc84829 178 {
Hellkite85 7:52c4be3bfc1b 179 drive.move(0.0, -5.0);
Hellkite85 9:bae63bc84829 180
Hellkite85 9:bae63bc84829 181 moveRobot.distance = 0.0;
Hellkite85 9:bae63bc84829 182 moveRobot.angle = -5.0;
Hellkite85 9:bae63bc84829 183
Hellkite85 9:bae63bc84829 184 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 185 }
Hellkite85 7:52c4be3bfc1b 186 else
Hellkite85 9:bae63bc84829 187 {
Hellkite85 7:52c4be3bfc1b 188 drive.move(0.0, 5.0);
Hellkite85 9:bae63bc84829 189
Hellkite85 9:bae63bc84829 190 moveRobot.distance = 0.0;
Hellkite85 9:bae63bc84829 191 moveRobot.angle = 5.0;
Hellkite85 9:bae63bc84829 192
Hellkite85 9:bae63bc84829 193 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 194 }
Hellkite85 7:52c4be3bfc1b 195 }
Hellkite85 7:52c4be3bfc1b 196
Hellkite85 7:52c4be3bfc1b 197 servoL.write(DUTY[dutyR]);
Hellkite85 7:52c4be3bfc1b 198 servoR.write(DUTY[dutyL]);
Hellkite85 7:52c4be3bfc1b 199
Hellkite85 9:bae63bc84829 200 // This code reinitializes the short distance
Hellkite85 9:bae63bc84829 201 // variable now that the short distance sensors
Hellkite85 9:bae63bc84829 202 // are pointing inward.
Hellkite85 9:bae63bc84829 203 distShortL = shortRangeL.getRange();
Hellkite85 9:bae63bc84829 204 distShortR = shortRangeR.getRange();
Hellkite85 7:52c4be3bfc1b 205
Hellkite85 7:52c4be3bfc1b 206 // This while loop is used to position the robot
Hellkite85 9:bae63bc84829 207 // at the correct location vertically with regards
Hellkite85 9:bae63bc84829 208 // to the peg in order to pick up the peg. The tolerance
Hellkite85 9:bae63bc84829 209 while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR))
Hellkite85 7:52c4be3bfc1b 210 {
Hellkite85 9:bae63bc84829 211 drive.move(0.125, 0.0);
Hellkite85 9:bae63bc84829 212
Hellkite85 9:bae63bc84829 213 moveRobot.distance = 0.125;
Hellkite85 9:bae63bc84829 214 moveRobot.angle = 0.0;
Hellkite85 9:bae63bc84829 215
Hellkite85 9:bae63bc84829 216 myStack.push(moveRobot);
Hellkite85 9:bae63bc84829 217 }
Hellkite85 9:bae63bc84829 218
Hellkite85 9:bae63bc84829 219 // This is the code to enable the gripper to grip the peg.
Hellkite85 9:bae63bc84829 220 robotGrip.grip()
Hellkite85 9:bae63bc84829 221
Hellkite85 9:bae63bc84829 222 /* pseudocode
Hellkite85 9:bae63bc84829 223 implement the code for reading a value from the color sensor
Hellkite85 9:bae63bc84829 224 store the value that is returned from the color sensor
Hellkite85 9:bae63bc84829 225 */
Hellkite85 9:bae63bc84829 226
Hellkite85 9:bae63bc84829 227 // This is the code to enable the gripper to lift the peg.
Hellkite85 9:bae63bc84829 228 robotGrip.lift();
Hellkite85 9:bae63bc84829 229
Hellkite85 9:bae63bc84829 230 // This code uses the stack to reverse all the movement
Hellkite85 9:bae63bc84829 231 // of the robot during this function.
Hellkite85 9:bae63bc84829 232 while(!myStack.empty())
Hellkite85 9:bae63bc84829 233 {
Hellkite85 9:bae63bc84829 234 myStack.top();
Hellkite85 9:bae63bc84829 235 myStack.pop();
Hellkite85 9:bae63bc84829 236
Hellkite85 9:bae63bc84829 237 drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));
Hellkite85 9:bae63bc84829 238 }
Hellkite85 7:52c4be3bfc1b 239 }
j_j205 0:999bb8fcd0b2 240
j_j205 0:999bb8fcd0b2 241 // FUNCTION:
j_j205 0:999bb8fcd0b2 242 // localize()
j_j205 0:999bb8fcd0b2 243 // IN-PARAMETERS:
j_j205 2:b281034eda86 244 // None
j_j205 0:999bb8fcd0b2 245 // OUT-PARAMETERS:
j_j205 2:b281034eda86 246 // None
j_j205 0:999bb8fcd0b2 247 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 248 // Checks localization points.
j_j205 3:992558a021d7 249 void Scanner::localize()
j_j205 0:999bb8fcd0b2 250 {
j_j205 3:992558a021d7 251 pitEnable = 0;
j_j205 3:992558a021d7 252 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 253 dutyR = MAX_DUTY;
j_j205 3:992558a021d7 254 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 255 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 256 wait(0.1);
j_j205 5:4d5601a9dffe 257 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 258 distRight = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 259 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 260 dutyR = MIN_DUTY;
j_j205 3:992558a021d7 261 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 262 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 263 wait(0.1);
j_j205 6:5e24ff86b743 264 distForwardL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 265 distForwardR = longRangeR.distInchesR();
j_j205 3:992558a021d7 266 pitEnable = 1;
j_j205 0:999bb8fcd0b2 267 }
j_j205 0:999bb8fcd0b2 268
j_j205 0:999bb8fcd0b2 269 // FUNCTION:
j_j205 6:5e24ff86b743 270 // void localizeLeft()
j_j205 6:5e24ff86b743 271 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 272 // None
j_j205 6:5e24ff86b743 273 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 274 // None
j_j205 6:5e24ff86b743 275 // DESCRIPTION:
j_j205 6:5e24ff86b743 276 // Using sensor longRangeR this method will localize to the wall
j_j205 6:5e24ff86b743 277 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 278 // should remain xxx distance from the left wall.
j_j205 6:5e24ff86b743 279 void Scanner::localizeLeft()
Hellkite85 7:52c4be3bfc1b 280 {
Hellkite85 7:52c4be3bfc1b 281 float distLocalR = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 282
Hellkite85 7:52c4be3bfc1b 283 if (distLocalR > 8.5)
Hellkite85 7:52c4be3bfc1b 284 {
Hellkite85 7:52c4be3bfc1b 285 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 286 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 287 drive.move(0.0, -1.0);
Hellkite85 7:52c4be3bfc1b 288 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 289 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 290 }
Hellkite85 7:52c4be3bfc1b 291 else if (distLocalR < 7.5)
Hellkite85 7:52c4be3bfc1b 292 {
Hellkite85 7:52c4be3bfc1b 293 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 294 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 295 drive.move(0.0, 1.0);
Hellkite85 7:52c4be3bfc1b 296 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 297 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 298 }
j_j205 6:5e24ff86b743 299 }
j_j205 6:5e24ff86b743 300
j_j205 6:5e24ff86b743 301 // FUNCTION:
j_j205 6:5e24ff86b743 302 // void localizeRight()
j_j205 6:5e24ff86b743 303 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 304 // None
j_j205 6:5e24ff86b743 305 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 306 // None
j_j205 6:5e24ff86b743 307 // DESCRIPTION:
j_j205 6:5e24ff86b743 308 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 309 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 310 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 311 // will be called from scan when the localizeRightFlag is set.
Hellkite85 7:52c4be3bfc1b 312 void Scanner::localizeRight()
j_j205 6:5e24ff86b743 313 {
Hellkite85 7:52c4be3bfc1b 314 float distLocalL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 315
Hellkite85 7:52c4be3bfc1b 316 if (distLocalL > 8.5)
Hellkite85 7:52c4be3bfc1b 317 {
Hellkite85 7:52c4be3bfc1b 318 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 319 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 320 drive.move(0.0, 1.0);
Hellkite85 7:52c4be3bfc1b 321 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 322 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 323 }
Hellkite85 7:52c4be3bfc1b 324 else if (distLocalL < 7.5)
Hellkite85 7:52c4be3bfc1b 325 {
Hellkite85 7:52c4be3bfc1b 326 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 327 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 328 drive.move(0.0, -1.0);
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 0:999bb8fcd0b2 335 // scan()
j_j205 0:999bb8fcd0b2 336 // IN-PARAMETERS:
j_j205 2:b281034eda86 337 // None
j_j205 0:999bb8fcd0b2 338 // OUT-PARAMETERS:
j_j205 2:b281034eda86 339 // None
j_j205 0:999bb8fcd0b2 340 // DESCRIPTION:
j_j205 2:b281034eda86 341 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 342 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 343 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 344 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 345 void Scanner::scan()
j_j205 0:999bb8fcd0b2 346 {
j_j205 2:b281034eda86 347 if (pitEnable == 1)
j_j205 2:b281034eda86 348 {
j_j205 6:5e24ff86b743 349 /*// obtain distance measures from sensors
j_j205 5:4d5601a9dffe 350 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 351 distRight = longRangeR.distInchesR();
j_j205 5:4d5601a9dffe 352
j_j205 6:5e24ff86b743 353 //check localize flags and make necessary method calls
j_j205 6:5e24ff86b743 354
j_j205 3:992558a021d7 355 // check avoid flag
j_j205 5:4d5601a9dffe 356 if (avoidFlag == 1)
j_j205 5:4d5601a9dffe 357 {
j_j205 5:4d5601a9dffe 358 // add obstacle avoid code
j_j205 5:4d5601a9dffe 359 }
j_j205 5:4d5601a9dffe 360
j_j205 3:992558a021d7 361 // check hunt flag
j_j205 6:5e24ff86b743 362 if (huntFlag == 1)
j_j205 5:4d5601a9dffe 363 {
j_j205 5:4d5601a9dffe 364 // add hunt code
j_j205 6:5e24ff86b743 365 }*/
j_j205 3:992558a021d7 366
j_j205 3:992558a021d7 367 // increment/decrement servo position
j_j205 2:b281034eda86 368 dutyL += invertL;
j_j205 2:b281034eda86 369 dutyR += invertR;
j_j205 2:b281034eda86 370 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 371 servoR.write(DUTY[dutyR]);
j_j205 6:5e24ff86b743 372 if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
j_j205 2:b281034eda86 373 ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
j_j205 2:b281034eda86 374 {
j_j205 2:b281034eda86 375 invertL *= -1;
j_j205 2:b281034eda86 376 invertR *= -1;
j_j205 2:b281034eda86 377 }
j_j205 2:b281034eda86 378 }
j_j205 3:992558a021d7 379 }