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@10:6a630acaeadb, 2016-03-26 (annotated)

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