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

Committer:
Hellkite85
Date:
Sat Mar 26 05:09:29 2016 +0000
Revision:
7:52c4be3bfc1b
Parent:
6:5e24ff86b743
Child:
8:32a445ae1d72
3-26-2015   DSV

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 0:999bb8fcd0b2 4 #include "VL6180x.h"
Hellkite85 7:52c4be3bfc1b 5 #include "StepperDrive.h"
Hellkite85 7:52c4be3bfc1b 6 #include <stack>
j_j205 0:999bb8fcd0b2 7
j_j205 0:999bb8fcd0b2 8 // FUNCTION:
j_j205 0:999bb8fcd0b2 9 // Scanner()
j_j205 0:999bb8fcd0b2 10 // IN-PARAMETERS:
Hellkite85 7:52c4be3bfc1b 11 // Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
j_j205 2:b281034eda86 12 // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
j_j205 2:b281034eda86 13 // Gp2x &_longRangeR, float _period = 10e-3
j_j205 0:999bb8fcd0b2 14 // OUT-PARAMETERS:
j_j205 2:b281034eda86 15 // None
j_j205 0:999bb8fcd0b2 16 // DESCRIPTION:
j_j205 2:b281034eda86 17 // Constructor.
Hellkite85 7:52c4be3bfc1b 18 Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
j_j205 4:6a8d80954323 19 VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL,
Hellkite85 7:52c4be3bfc1b 20 LongRangeSensor &_longRangeR, float _period) : pc(pc1), drive(_drive), servoL(_servoL),
j_j205 2:b281034eda86 21 servoR(_servoR), shortRangeL(_shortRangeL),
j_j205 2:b281034eda86 22 shortRangeR(_shortRangeR), longRangeL(_longRangeL),
j_j205 0:999bb8fcd0b2 23 longRangeR(_longRangeR), period(_period)
j_j205 0:999bb8fcd0b2 24 {
j_j205 0:999bb8fcd0b2 25 servoL.period(1.0/50.0);
j_j205 0:999bb8fcd0b2 26 servoR.period(1.0/50.0);
j_j205 2:b281034eda86 27
j_j205 6:5e24ff86b743 28 float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925,
j_j205 6:5e24ff86b743 29 0.1013, 0.1100}; // sub-micro
j_j205 2:b281034eda86 30
j_j205 6:5e24ff86b743 31 for (int i = 0; i < 7; i++)
j_j205 3:992558a021d7 32 DUTY[i] = temp[i];
j_j205 3:992558a021d7 33
j_j205 3:992558a021d7 34 obstacle = 0;
j_j205 3:992558a021d7 35 distLeft = 0.0;
j_j205 3:992558a021d7 36 distRight = 0.0;
j_j205 6:5e24ff86b743 37 distForwardL = 0.0;
j_j205 6:5e24ff86b743 38 distForwardR = 0.0;
j_j205 2:b281034eda86 39
j_j205 2:b281034eda86 40 // initializing to avoid status
j_j205 3:992558a021d7 41 avoidFlag = 1;
j_j205 3:992558a021d7 42 huntFlag = 0;
j_j205 3:992558a021d7 43 invertL = -1;
j_j205 3:992558a021d7 44 invertR = 1;
j_j205 3:992558a021d7 45 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 46 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 47 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 48 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 49 wait(0.2);
j_j205 2:b281034eda86 50 pitEnable = 1;
j_j205 3:992558a021d7 51 scanPit.attach(this, &Scanner::scan, period);
j_j205 6:5e24ff86b743 52 pc.printf("Scanner created\n");
j_j205 0:999bb8fcd0b2 53 }
j_j205 0:999bb8fcd0b2 54
j_j205 0:999bb8fcd0b2 55 // FUNCTION:
j_j205 3:992558a021d7 56 // huntMode()
j_j205 0:999bb8fcd0b2 57 // IN-PARAMETERS:
j_j205 2:b281034eda86 58 // None
j_j205 2:b281034eda86 59 // None
j_j205 0:999bb8fcd0b2 60 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 61 // Hunts for victim.
j_j205 3:992558a021d7 62 void Scanner::huntMode()
j_j205 0:999bb8fcd0b2 63 {
j_j205 2:b281034eda86 64 pitEnable = 0;
j_j205 3:992558a021d7 65 avoidFlag = 0;
j_j205 3:992558a021d7 66 huntFlag = 1;
j_j205 3:992558a021d7 67 invertL = -1;
j_j205 3:992558a021d7 68 invertR = 1;
j_j205 6:5e24ff86b743 69 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 70 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 71 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 72 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 73 wait(0.1);
j_j205 2:b281034eda86 74 pitEnable = 1;
j_j205 0:999bb8fcd0b2 75 }
j_j205 0:999bb8fcd0b2 76
j_j205 0:999bb8fcd0b2 77 // FUNCTION:
j_j205 3:992558a021d7 78 // avoidMode()
j_j205 0:999bb8fcd0b2 79 // IN-PARAMETERS:
j_j205 2:b281034eda86 80 // None
j_j205 0:999bb8fcd0b2 81 // OUT-PARAMETERS:
j_j205 2:b281034eda86 82 // None
j_j205 0:999bb8fcd0b2 83 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 84 // Scans to avoid obstacles.
j_j205 3:992558a021d7 85 void Scanner::avoidMode()
j_j205 0:999bb8fcd0b2 86 {
j_j205 2:b281034eda86 87 pitEnable = 0;
j_j205 3:992558a021d7 88 avoidFlag = 1;
j_j205 3:992558a021d7 89 huntFlag = 0;
j_j205 3:992558a021d7 90 invertL = -1;
j_j205 3:992558a021d7 91 invertR = 1;
j_j205 3:992558a021d7 92 dutyL = MAX_DUTY;
j_j205 3:992558a021d7 93 dutyR = MIN_DUTY;
j_j205 2:b281034eda86 94 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 95 servoR.write(DUTY[dutyR]);
j_j205 2:b281034eda86 96 wait(0.1);
j_j205 2:b281034eda86 97 pitEnable = 1;
j_j205 0:999bb8fcd0b2 98 }
Hellkite85 7:52c4be3bfc1b 99 // FUNCTION:
Hellkite85 7:52c4be3bfc1b 100 //
Hellkite85 7:52c4be3bfc1b 101 void Scanner::hunt()
Hellkite85 7:52c4be3bfc1b 102 {
Hellkite85 7:52c4be3bfc1b 103 // Used to store temporary distance
Hellkite85 7:52c4be3bfc1b 104 // recorded from VL6180x short range
Hellkite85 7:52c4be3bfc1b 105 // sensor.
Hellkite85 7:52c4be3bfc1b 106 float tempDistShortL;
Hellkite85 7:52c4be3bfc1b 107 float tempDistShortR;
Hellkite85 7:52c4be3bfc1b 108
Hellkite85 7:52c4be3bfc1b 109 // Used to temporary store the number
Hellkite85 7:52c4be3bfc1b 110 // of stepper motor increments.
Hellkite85 7:52c4be3bfc1b 111 int tempStepCount = 0;
Hellkite85 7:52c4be3bfc1b 112
Hellkite85 7:52c4be3bfc1b 113 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 114
Hellkite85 7:52c4be3bfc1b 115 servoL.write(DUTY[4]);
Hellkite85 7:52c4be3bfc1b 116 servoR.write(DUTY[2]);
Hellkite85 7:52c4be3bfc1b 117
Hellkite85 7:52c4be3bfc1b 118 // Update this code using the new VL6180x code
Hellkite85 7:52c4be3bfc1b 119 // from Victor.
Hellkite85 7:52c4be3bfc1b 120 // tempDistShortL =
Hellkite85 7:52c4be3bfc1b 121 // tempDistShortR =
Hellkite85 7:52c4be3bfc1b 122
Hellkite85 7:52c4be3bfc1b 123 // This if statement moves the robot forward in order
Hellkite85 7:52c4be3bfc1b 124 // to continue the hunt of the peg.
Hellkite85 7:52c4be3bfc1b 125 while(distShortL == 0 && distShortR == 0 && tempStepCount < 34)
Hellkite85 7:52c4be3bfc1b 126 {
Hellkite85 7:52c4be3bfc1b 127 drive.move(0.25, 0.0);
Hellkite85 7:52c4be3bfc1b 128 }
Hellkite85 7:52c4be3bfc1b 129
Hellkite85 7:52c4be3bfc1b 130 // Adjust the comparsion for floats with
Hellkite85 7:52c4be3bfc1b 131 // regards to the sensor data.
Hellkite85 7:52c4be3bfc1b 132 // This while loop checks the short range
Hellkite85 7:52c4be3bfc1b 133 // sensors and if they are not equal to
Hellkite85 7:52c4be3bfc1b 134 // each other, the code will center the
Hellkite85 7:52c4be3bfc1b 135 // robot.
Hellkite85 7:52c4be3bfc1b 136 while(tempDistShortL != tempDistShortR)
Hellkite85 7:52c4be3bfc1b 137 {
Hellkite85 7:52c4be3bfc1b 138 // Adjust this comparsion for floats
Hellkite85 7:52c4be3bfc1b 139 // with regards to the sensor data.
Hellkite85 7:52c4be3bfc1b 140 if(tempDistShortL > tempDistShortR)
Hellkite85 7:52c4be3bfc1b 141 drive.move(0.0, -5.0);
Hellkite85 7:52c4be3bfc1b 142 else
Hellkite85 7:52c4be3bfc1b 143 drive.move(0.0, 5.0);
Hellkite85 7:52c4be3bfc1b 144 }
Hellkite85 7:52c4be3bfc1b 145
Hellkite85 7:52c4be3bfc1b 146 servoL.write(DUTY[dutyR]);
Hellkite85 7:52c4be3bfc1b 147 servoR.write(DUTY[dutyL]);
Hellkite85 7:52c4be3bfc1b 148
Hellkite85 7:52c4be3bfc1b 149 // Update this code with the new VL6180x sensor
Hellkite85 7:52c4be3bfc1b 150 // code from Victor.
Hellkite85 7:52c4be3bfc1b 151 // tempDistShortL =
Hellkite85 7:52c4be3bfc1b 152 // tempDistShortR =
Hellkite85 7:52c4be3bfc1b 153
Hellkite85 7:52c4be3bfc1b 154 // This while loop is used to position the robot
Hellkite85 7:52c4be3bfc1b 155 // at the correct location in order to pick up
Hellkite85 7:52c4be3bfc1b 156 // the peg.
Hellkite85 7:52c4be3bfc1b 157 while(tempDistShortL == tempDistShortR)
Hellkite85 7:52c4be3bfc1b 158 {
Hellkite85 7:52c4be3bfc1b 159 /* pseudocode
Hellkite85 7:52c4be3bfc1b 160 implement the code for the gripper to pick up the peg
Hellkite85 7:52c4be3bfc1b 161 implement the code for reading a value from the color sensor
Hellkite85 7:52c4be3bfc1b 162 store the value that is returned from the color sensor
Hellkite85 7:52c4be3bfc1b 163 implement the code for the gripper to angle up
Hellkite85 7:52c4be3bfc1b 164 pop the stack and reverse the moves stored in the stack
Hellkite85 7:52c4be3bfc1b 165 return a done value
Hellkite85 7:52c4be3bfc1b 166 */
Hellkite85 7:52c4be3bfc1b 167 }
Hellkite85 7:52c4be3bfc1b 168 }
j_j205 0:999bb8fcd0b2 169
j_j205 0:999bb8fcd0b2 170 // FUNCTION:
j_j205 0:999bb8fcd0b2 171 // localize()
j_j205 0:999bb8fcd0b2 172 // IN-PARAMETERS:
j_j205 2:b281034eda86 173 // None
j_j205 0:999bb8fcd0b2 174 // OUT-PARAMETERS:
j_j205 2:b281034eda86 175 // None
j_j205 0:999bb8fcd0b2 176 // DESCRIPTION:
j_j205 0:999bb8fcd0b2 177 // Checks localization points.
j_j205 3:992558a021d7 178 void Scanner::localize()
j_j205 0:999bb8fcd0b2 179 {
j_j205 3:992558a021d7 180 pitEnable = 0;
j_j205 3:992558a021d7 181 dutyL = MIN_DUTY;
j_j205 3:992558a021d7 182 dutyR = MAX_DUTY;
j_j205 3:992558a021d7 183 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 184 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 185 wait(0.1);
j_j205 5:4d5601a9dffe 186 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 187 distRight = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 188 dutyL = MAX_DUTY;
j_j205 6:5e24ff86b743 189 dutyR = MIN_DUTY;
j_j205 3:992558a021d7 190 servoL.write(DUTY[dutyL]);
j_j205 3:992558a021d7 191 servoR.write(DUTY[dutyR]);
j_j205 3:992558a021d7 192 wait(0.1);
j_j205 6:5e24ff86b743 193 distForwardL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 194 distForwardR = longRangeR.distInchesR();
j_j205 3:992558a021d7 195 pitEnable = 1;
j_j205 0:999bb8fcd0b2 196 }
j_j205 0:999bb8fcd0b2 197
j_j205 0:999bb8fcd0b2 198 // FUNCTION:
j_j205 6:5e24ff86b743 199 // void localizeLeft()
j_j205 6:5e24ff86b743 200 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 201 // None
j_j205 6:5e24ff86b743 202 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 203 // None
j_j205 6:5e24ff86b743 204 // DESCRIPTION:
j_j205 6:5e24ff86b743 205 // Using sensor longRangeR this method will localize to the wall
j_j205 6:5e24ff86b743 206 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 207 // should remain xxx distance from the left wall.
j_j205 6:5e24ff86b743 208 void Scanner::localizeLeft()
Hellkite85 7:52c4be3bfc1b 209 {
Hellkite85 7:52c4be3bfc1b 210 float distLocalR = longRangeR.distInchesR();
j_j205 6:5e24ff86b743 211
Hellkite85 7:52c4be3bfc1b 212 if (distLocalR > 8.5)
Hellkite85 7:52c4be3bfc1b 213 {
Hellkite85 7:52c4be3bfc1b 214 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 215 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 216 drive.move(0.0, -1.0);
Hellkite85 7:52c4be3bfc1b 217 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 218 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 219 }
Hellkite85 7:52c4be3bfc1b 220 else if (distLocalR < 7.5)
Hellkite85 7:52c4be3bfc1b 221 {
Hellkite85 7:52c4be3bfc1b 222 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 223 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 224 drive.move(0.0, 1.0);
Hellkite85 7:52c4be3bfc1b 225 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 226 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 227 }
j_j205 6:5e24ff86b743 228 }
j_j205 6:5e24ff86b743 229
j_j205 6:5e24ff86b743 230 // FUNCTION:
j_j205 6:5e24ff86b743 231 // void localizeRight()
j_j205 6:5e24ff86b743 232 // IN-PARAMETERS:
j_j205 6:5e24ff86b743 233 // None
j_j205 6:5e24ff86b743 234 // OUT-PARAMETERS:
j_j205 6:5e24ff86b743 235 // None
j_j205 6:5e24ff86b743 236 // DESCRIPTION:
j_j205 6:5e24ff86b743 237 // Using sensor longRangeL this method will localize to the wall
j_j205 6:5e24ff86b743 238 // to the left during stretches of forward motion where robot
j_j205 6:5e24ff86b743 239 // should remain xxx distance from the left wall. This function
j_j205 6:5e24ff86b743 240 // will be called from scan when the localizeRightFlag is set.
Hellkite85 7:52c4be3bfc1b 241 void Scanner::localizeRight()
j_j205 6:5e24ff86b743 242 {
Hellkite85 7:52c4be3bfc1b 243 float distLocalL = longRangeL.distInchesL();
j_j205 6:5e24ff86b743 244
Hellkite85 7:52c4be3bfc1b 245 if (distLocalL > 8.5)
Hellkite85 7:52c4be3bfc1b 246 {
Hellkite85 7:52c4be3bfc1b 247 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 248 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 249 drive.move(0.0, 1.0);
Hellkite85 7:52c4be3bfc1b 250 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 251 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 252 }
Hellkite85 7:52c4be3bfc1b 253 else if (distLocalL < 7.5)
Hellkite85 7:52c4be3bfc1b 254 {
Hellkite85 7:52c4be3bfc1b 255 pitEnable = 0;
Hellkite85 7:52c4be3bfc1b 256 drive.pauseMove();
Hellkite85 7:52c4be3bfc1b 257 drive.move(0.0, -1.0);
Hellkite85 7:52c4be3bfc1b 258 drive.resumeMove();
Hellkite85 7:52c4be3bfc1b 259 pitEnable = 1;
Hellkite85 7:52c4be3bfc1b 260 }
j_j205 6:5e24ff86b743 261 }
j_j205 6:5e24ff86b743 262
j_j205 6:5e24ff86b743 263 // FUNCTION:
j_j205 0:999bb8fcd0b2 264 // scan()
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 2:b281034eda86 270 // Ticker function to make servos scan either the inward-facing
j_j205 2:b281034eda86 271 // 90 degrees or the outward-facing 90 degrees. Distance
j_j205 2:b281034eda86 272 // measures are obtained from both the short range and long
j_j205 2:b281034eda86 273 // range sensors at 15 degree intervals.
j_j205 0:999bb8fcd0b2 274 void Scanner::scan()
j_j205 0:999bb8fcd0b2 275 {
j_j205 2:b281034eda86 276 if (pitEnable == 1)
j_j205 2:b281034eda86 277 {
j_j205 6:5e24ff86b743 278 /*// obtain distance measures from sensors
j_j205 5:4d5601a9dffe 279 distLeft = longRangeL.distInchesL();
j_j205 5:4d5601a9dffe 280 distRight = longRangeR.distInchesR();
j_j205 5:4d5601a9dffe 281
j_j205 6:5e24ff86b743 282 //check localize flags and make necessary method calls
j_j205 6:5e24ff86b743 283
j_j205 3:992558a021d7 284 // check avoid flag
j_j205 5:4d5601a9dffe 285 if (avoidFlag == 1)
j_j205 5:4d5601a9dffe 286 {
j_j205 5:4d5601a9dffe 287 // add obstacle avoid code
j_j205 5:4d5601a9dffe 288 }
j_j205 5:4d5601a9dffe 289
j_j205 3:992558a021d7 290 // check hunt flag
j_j205 6:5e24ff86b743 291 if (huntFlag == 1)
j_j205 5:4d5601a9dffe 292 {
j_j205 5:4d5601a9dffe 293 // add hunt code
j_j205 6:5e24ff86b743 294 }*/
j_j205 3:992558a021d7 295
j_j205 3:992558a021d7 296 // increment/decrement servo position
j_j205 2:b281034eda86 297 dutyL += invertL;
j_j205 2:b281034eda86 298 dutyR += invertR;
j_j205 2:b281034eda86 299 servoL.write(DUTY[dutyL]);
j_j205 2:b281034eda86 300 servoR.write(DUTY[dutyR]);
j_j205 6:5e24ff86b743 301 if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
j_j205 2:b281034eda86 302 ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
j_j205 2:b281034eda86 303 {
j_j205 2:b281034eda86 304 invertL *= -1;
j_j205 2:b281034eda86 305 invertR *= -1;
j_j205 2:b281034eda86 306 }
j_j205 2:b281034eda86 307 }
j_j205 3:992558a021d7 308 }