3/26/16 12:25 am JJ
Dependents: steppertest R5 2016 Robotics Team 1
Fork of scanner by
scanner.cpp
00001 #include "scanner.h" 00002 #include "mbed.h" 00003 #include "LongRangeSensor.h" 00004 #include "ShortRangeSensor.h" 00005 #include "StepperDrive.h" 00006 #include "Gripper.h" 00007 #include <stack> 00008 00009 // FUNCTION: 00010 // Scanner() 00011 // IN-PARAMETERS: 00012 // Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, 00013 // VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL, 00014 // Gp2x &_longRangeR, float _period = 10e-3 00015 // OUT-PARAMETERS: 00016 // None 00017 // DESCRIPTION: 00018 // Constructor. 00019 Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR, 00020 ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL, 00021 LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period, ColorSensor &_colorSensor) : pc(pc1), drive(_drive), servoL(_servoL), 00022 servoR(_servoR), shortRangeL(_shortRangeL), 00023 shortRangeR(_shortRangeR), longRangeL(_longRangeL), 00024 longRangeR(_longRangeR), robotGrip(_robotGrip), period(_period), colorSensor(_colorSensor) 00025 { 00026 servoL.period(1.0/50.0); 00027 servoR.period(1.0/50.0); 00028 00029 float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925, 00030 0.1013, 0.1100}; // sub-micro 00031 00032 for (int i = 0; i < 7; i++) 00033 DUTY[i] = temp[i]; 00034 00035 obstacle = 0; 00036 distLeft = 0.0; 00037 distRight = 0.0; 00038 distForwardL = 0.0; 00039 distForwardR = 0.0; 00040 00041 // initializing to localizeRight status 00042 avoidFlag = 0; 00043 huntFlag = 0; 00044 localizeRightFlag = 1; 00045 localizeLeftFlag = 0; 00046 invertL = -1; 00047 invertR = 1; 00048 dutyL = MIN_DUTY; 00049 dutyR = MIN_DUTY; 00050 servoL.write(DUTY[dutyL]); 00051 servoR.write(DUTY[dutyR]); 00052 wait(0.2); 00053 pitEnable = 1; 00054 scanPit.attach(this, &Scanner::scan, period); 00055 pc.printf("Scanner created\n"); 00056 } 00057 00058 // FUNCTION: 00059 // huntMode() 00060 // IN-PARAMETERS: 00061 // None 00062 // None 00063 // DESCRIPTION: 00064 // Hunts for victim. 00065 void Scanner::huntMode() 00066 { 00067 pitEnable = 0; 00068 avoidFlag = 0; 00069 huntFlag = 1; 00070 invertL = -1; 00071 invertR = 1; 00072 dutyL = MAX_DUTY; 00073 dutyR = MIN_DUTY; 00074 servoL.write(DUTY[dutyL]); 00075 servoR.write(DUTY[dutyR]); 00076 wait(0.1); 00077 pitEnable = 1; 00078 } 00079 00080 // FUNCTION: 00081 // avoidMode() 00082 // IN-PARAMETERS: 00083 // None 00084 // OUT-PARAMETERS: 00085 // None 00086 // DESCRIPTION: 00087 // Scans to avoid obstacles. 00088 void Scanner::avoidMode() 00089 { 00090 pitEnable = 0; 00091 avoidFlag = 1; 00092 huntFlag = 0; 00093 invertL = -1; 00094 invertR = 1; 00095 dutyL = MAX_DUTY; 00096 dutyR = MIN_DUTY; 00097 servoL.write(DUTY[dutyL]); 00098 servoR.write(DUTY[dutyR]); 00099 wait(0.1); 00100 pitEnable = 1; 00101 } 00102 // FUNCTION: 00103 // 00104 void Scanner::hunt() 00105 { 00106 // This creates an instance of a structure 00107 // call huntMove. This structure is used to 00108 // store the distance value and angle value 00109 // when implementing the Navigation drive 00110 // code. 00111 huntMove moveRobot; 00112 00113 // Used to store temporary distance 00114 // recorded from VL6180x short range 00115 // sensor. 00116 float tempDistShortL; 00117 float tempDistShortR; 00118 00119 float distShortL; 00120 float distShortR; 00121 00122 // Used to temporary store the number 00123 // of stepper motor increments. 00124 int tempStepCount = 0; 00125 00126 // This variable is half of the available 00127 // tolerance of the gripper with regards 00128 // to the peg. 00129 float tolerance = 0.1875; 00130 00131 // These variables represent the tolerance 00132 // and plus/minus half the diameter of the 00133 // peg measured in inches. 00134 float toleranceAHP = 0.9375; 00135 float toleranceSHP = 0.5625; 00136 00137 pitEnable = 0; 00138 00139 // This positions both of the servos to 00140 // be 60 degrees inwards. 00141 servoL.write(DUTY[4]); 00142 servoR.write(DUTY[2]); 00143 00144 // This initializes the temporary distance variables 00145 // with the data collected from the VL6180X sensors. 00146 tempDistShortL = shortRangeL.getRange(); 00147 tempDistShortR = shortRangeR.getRange(); 00148 00149 // This code converts the measurements from the short 00150 // range sensor from millimeters to inches. 00151 distShortL = (tempDistShortL / 25.0); 00152 distShortR = (tempDistShortR / 25.0); 00153 00154 // This while loop statement moves the robot forward in order 00155 // to continue the hunt of the peg. The equals to comparsion 00156 // statements need to be updated to reflect what the short 00157 // range sensors output when there is nothing within the 00158 // range of the short distance sensors. 00159 while(distShortL == 0.0 && distShortR == 0.0 && tempStepCount < 34) 00160 { 00161 drive.move(0.25, 0.0); 00162 while(!drive.isMoveDone()) 00163 { 00164 wait(1e-6); 00165 } 00166 00167 moveRobot.distance = 0.25; 00168 moveRobot.angle = 0.0; 00169 00170 myStack.push(moveRobot); 00171 } 00172 00173 // Adjust the comparsion for floats with 00174 // regards to the sensor data. This 00175 // while loop ideally checks the short range 00176 // sensors and if they are not equal to 00177 // each other, the code will center the 00178 // robot according to the short range sensor. 00179 while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance)) 00180 { 00181 // Adjust this comparsion for floats 00182 // with regards to the sensor data. 00183 if(distShortL > (distShortR + tolerance)) 00184 { 00185 drive.move(0.0, (-5.0)*(3.14159 / 180.0)); 00186 while(!drive.isMoveDone()) 00187 { 00188 wait(1e-6); 00189 } 00190 00191 moveRobot.distance = 0.0; 00192 moveRobot.angle = ((-5.0)*(3.14159 / 180.0)); 00193 00194 myStack.push(moveRobot); 00195 } 00196 else 00197 { 00198 drive.move(0.0, (5.0*(3.14159 / 180.0))); 00199 while(!drive.isMoveDone()) 00200 { 00201 wait(1e-6); 00202 } 00203 00204 moveRobot.distance = 0.0; 00205 moveRobot.angle = ((5.0)*(3.14159 / 180.0)); 00206 00207 myStack.push(moveRobot); 00208 } 00209 } 00210 00211 servoL.write(DUTY[dutyR]); 00212 servoR.write(DUTY[dutyL]); 00213 00214 // This code reinitializes the short distance 00215 // variable now that the short distance sensors 00216 // are pointing inward. 00217 distShortL = shortRangeL.getRange(); 00218 distShortR = shortRangeR.getRange(); 00219 00220 // This while loop is used to position the robot 00221 // at the correct location vertically with regards 00222 // to the peg in order to pick up the peg. The tolerance 00223 while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) 00224 { 00225 drive.move(0.125, 0.0); 00226 while(!drive.isMoveDone()) 00227 { 00228 wait(1e-6); 00229 } 00230 00231 moveRobot.distance = 0.125; 00232 moveRobot.angle = 0.0; 00233 00234 myStack.push(moveRobot); 00235 } 00236 00237 // This code to enable the gripper to grip the peg. 00238 robotGrip.grip(); 00239 00240 // This code is used to determine the color of the peg 00241 // and the bool value will be used to control the 00242 // navigation of the robot. 00243 bool isPegColorRed; 00244 00245 int colorValue; 00246 colorValue = 00247 00248 if(colorValue > 150) 00249 isPegColorRed = true; 00250 else 00251 isPegColorRed = false; 00252 00253 // This is the code to enable the gripper to lift the peg. 00254 robotGrip.lift(); 00255 00256 // This code uses the stack to reverse all the movement 00257 // of the robot during this function. 00258 while(!myStack.empty()) 00259 { 00260 myStack.top(); 00261 myStack.pop(); 00262 00263 drive.move((-(moveRobot.distance)),(-(moveRobot.angle))); 00264 while(!drive.isMoveDone()) 00265 { 00266 wait(1e-6); 00267 } 00268 } 00269 } 00270 00271 // FUNCTION: 00272 // localize() 00273 // IN-PARAMETERS: 00274 // None 00275 // OUT-PARAMETERS: 00276 // None 00277 // DESCRIPTION: 00278 // Checks localization points. 00279 void Scanner::localize() 00280 { 00281 pitEnable = 0; 00282 dutyL = MIN_DUTY; 00283 dutyR = MAX_DUTY; 00284 servoL.write(DUTY[dutyL]); 00285 servoR.write(DUTY[dutyR]); 00286 wait(0.1); 00287 distLeft = longRangeL.distInchesL(); 00288 distRight = longRangeR.distInchesR(); 00289 dutyL = MAX_DUTY; 00290 dutyR = MIN_DUTY; 00291 servoL.write(DUTY[dutyL]); 00292 servoR.write(DUTY[dutyR]); 00293 wait(0.1); 00294 distForwardL = longRangeL.distInchesL(); 00295 distForwardR = longRangeR.distInchesR(); 00296 pitEnable = 1; 00297 } 00298 00299 // FUNCTION: 00300 // void localizeLeftMode() 00301 // IN-PARAMETERS: 00302 // None 00303 // OUT-PARAMETERS: 00304 // None 00305 // DESCRIPTION: 00306 // This method initializes to localizeLeft() 00307 void Scanner::localizeLeftMode() 00308 { 00309 localizeRightFlag = 0; 00310 00311 dutyL = MAX_DUTY; 00312 dutyR = MAX_DUTY; 00313 servoL.write(DUTY[dutyL]); 00314 servoR.write(DUTY[dutyR]); 00315 wait(0.2); 00316 00317 localizeLeftFlag = 1; 00318 } 00319 00320 // FUNCTION: 00321 // void localizeLeft() 00322 // IN-PARAMETERS: 00323 // None 00324 // OUT-PARAMETERS: 00325 // None 00326 // DESCRIPTION: 00327 // Using sensor longRangeR this method will localize to the wall 00328 // to the left during stretches of forward motion where robot 00329 // should remain xxx distance from the left wall. 00330 void Scanner::localizeLeft() 00331 { 00332 pitEnable = 0; 00333 float distLocalR = longRangeR.distInchesR(); 00334 00335 if (distLocalR >= 9.2) 00336 { 00337 drive.pauseMove(); 00338 drive.move(0.0, ((-5.0)*(3.14159 / 180.0))); 00339 // wait for move to complete 00340 while(!drive.isMoveDone()) 00341 { 00342 wait(1e-6); 00343 } 00344 drive.resumeMove(); 00345 } 00346 else if (distLocalR <= 8.2) 00347 { 00348 drive.pauseMove(); 00349 drive.move(0.0, ((5.0)*(3.14159 / 180.0))); 00350 // wait for move to complete 00351 while(!drive.isMoveDone()) 00352 { 00353 wait(1e-6); 00354 } 00355 drive.resumeMove(); 00356 } 00357 00358 // wait for move to complete 00359 while(!drive.isMoveDone()) 00360 { 00361 wait(1e-6); 00362 } 00363 pitEnable = 1; 00364 } 00365 00366 // FUNCTION: 00367 // void localizeRightMode() 00368 // IN-PARAMETERS: 00369 // None 00370 // OUT-PARAMETERS: 00371 // None 00372 // DESCRIPTION: 00373 // Initializes to localizeRight() 00374 void Scanner::localizeRightMode() 00375 { 00376 localizeLeftFlag = 0; 00377 00378 dutyL = MIN_DUTY; 00379 dutyR = MIN_DUTY; 00380 servoL.write(DUTY[dutyL]); 00381 servoR.write(DUTY[dutyR]); 00382 wait(0.2); 00383 00384 localizeRightFlag = 1; 00385 } 00386 00387 // FUNCTION: 00388 // void localizeRight() 00389 // IN-PARAMETERS: 00390 // None 00391 // OUT-PARAMETERS: 00392 // None 00393 // DESCRIPTION: 00394 // Using sensor longRangeL this method will localize to the wall 00395 // to the left during stretches of forward motion where robot 00396 // should remain xxx distance from the left wall. This function 00397 // will be called from scan when the localizeRightFlag is set. 00398 void Scanner::localizeRight() 00399 { 00400 pitEnable = 0; 00401 float distLocalL = longRangeL.distInchesL(); 00402 00403 if (distLocalL >= 9.2) 00404 { 00405 drive.pauseMove(); 00406 drive.move(0, ((5.0)*(3.14159 / 180.0))); 00407 // wait for move to complete 00408 while(!drive.isMoveDone()) 00409 { 00410 wait(1e-6); 00411 } 00412 drive.resumeMove(); 00413 } 00414 else if (distLocalL <= 8.2) 00415 { 00416 drive.pauseMove(); 00417 drive.move(0, ((-5.0)*(3.14159 / 180.0))); 00418 // wait for move to complete 00419 while(!drive.isMoveDone()) 00420 { 00421 wait(1e-6); 00422 } 00423 drive.resumeMove(); 00424 } 00425 00426 // wait for move to complete 00427 while(!drive.isMoveDone()) 00428 { 00429 wait(1e-6); 00430 } 00431 pitEnable = 1; 00432 } 00433 00434 // FUNCTION: 00435 // scan() 00436 // IN-PARAMETERS: 00437 // None 00438 // OUT-PARAMETERS: 00439 // None 00440 // DESCRIPTION: 00441 // Ticker function to make servos scan either the inward-facing 00442 // 90 degrees or the outward-facing 90 degrees. Distance 00443 // measures are obtained from both the short range and long 00444 // range sensors at 15 degree intervals. 00445 void Scanner::scan() 00446 { 00447 if (pitEnable == 1) 00448 { 00449 if (localizeRightFlag == 1) 00450 localizeRight(); 00451 00452 if (localizeLeftFlag == 1) 00453 localizeLeft(); 00454 } 00455 }
Generated on Fri Jul 15 2022 05:45:51 by 1.7.2