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

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