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 }