File content as of revision 21:bbadc0b8ed9f:

#include "scanner.h"
#include "mbed.h"
#include "LongRangeSensor.h"
#include "ShortRangeSensor.h"
#include "StepperDrive.h"
#include "Gripper.h"
#include <stack>

// FUNCTION:
//      Scanner()
// IN-PARAMETERS:
//      Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
//      VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
//      Gp2x &_longRangeR, float _period = 10e-3
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Constructor.
Scanner::Scanner(Serial &pc1, StepperDrive &_drive, PinName _servoL, PinName _servoR,
    ShortRangeSensor &_shortRangeL, ShortRangeSensor &_shortRangeR, LongRangeSensor &_longRangeL,
    LongRangeSensor &_longRangeR, Gripper &_robotGrip, float _period, ColorSensor &_colorSensor) : pc(pc1), drive(_drive), servoL(_servoL),
    servoR(_servoR), shortRangeL(_shortRangeL),
    shortRangeR(_shortRangeR), longRangeL(_longRangeL),
    longRangeR(_longRangeR), robotGrip(_robotGrip), period(_period), colorSensor(_colorSensor)
{
    servoL.period(1.0/50.0);
    servoR.period(1.0/50.0);

    float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925,
    0.1013, 0.1100}; // sub-micro

    for (int i = 0; i < 7; i++)
        DUTY[i] = temp[i];

    obstacle = 0;
    distLeft = 0.0;
    distRight = 0.0;
    distForwardL = 0.0;
    distForwardR = 0.0;

    // initializing to localizeRight status
    avoidFlag = 0;
    huntFlag = 0;
    localizeRightFlag = 1;
    localizeLeftFlag = 0;
    invertL = -1;
    invertR = 1;
    dutyL = MIN_DUTY;
    dutyR = MIN_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.2);
    pitEnable = 1;
    scanPit.attach(this, &Scanner::scan, period);
pc.printf("Scanner created\n");
}

// FUNCTION:
//      huntMode()
// IN-PARAMETERS:
//      None
//      None
// DESCRIPTION:
//      Hunts for victim.
void Scanner::huntMode()
{
    pitEnable = 0;
    avoidFlag = 0;
    huntFlag = 1;
    invertL = -1;
    invertR = 1;
    dutyL = MAX_DUTY;
    dutyR = MIN_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.1);
    pitEnable = 1;
}

// FUNCTION:
//      avoidMode()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Scans to avoid obstacles.
void Scanner::avoidMode()
{
    pitEnable = 0;
    avoidFlag = 1;
    huntFlag = 0;
    invertL = -1;
    invertR = 1;
    dutyL = MAX_DUTY;
    dutyR = MIN_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.1);
    pitEnable = 1;
}
// FUNCTION:
//      
void Scanner::hunt()
{
    // This creates an instance of a structure
    // call huntMove. This structure is used to
    // store the distance value and angle value
    // when implementing the Navigation drive 
    // code.
    huntMove moveRobot;
    
    // Used to store temporary distance 
    // recorded from VL6180x short range
    // sensor.
    float tempDistShortL;
    float tempDistShortR;
    
    float distShortL;
    float distShortR;

    // Used to temporary store the number 
    // of stepper motor increments.
    int tempStepCount = 0;
    
    // This variable is half of the available 
    // tolerance of the gripper with regards 
    // to the peg.
    float tolerance = 0.1875;
    
    // These variables represent the tolerance
    // and plus/minus half the diameter of the
    // peg measured in inches.
    float toleranceAHP = 0.9375;
    float toleranceSHP = 0.5625;
    
    pitEnable = 0;
    
    // This positions both of the servos to 
    // be 60 degrees inwards.
    servoL.write(DUTY[4]);
    servoR.write(DUTY[2]);
    
    // This initializes the temporary distance variables
    // with the data collected from the VL6180X sensors.
    tempDistShortL = shortRangeL.getRange();
    tempDistShortR = shortRangeR.getRange();
    
    // This code converts the measurements from the short
    // range sensor from millimeters to inches. 
    distShortL = (tempDistShortL / 25.0);
    distShortR = (tempDistShortR / 25.0);
    
    // This while loop statement moves the robot forward in order
    // to continue the hunt of the peg. The equals to comparsion
    // statements need to be updated to reflect what the short
    // range sensors output when there is nothing within the 
    // range of the short distance sensors.
    while(distShortL == 0.0 && distShortR == 0.0 && tempStepCount < 34)
    {
        drive.move(0.25, 0.0);
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
        
        moveRobot.distance = 0.25;
        moveRobot.angle = 0.0;
        
        myStack.push(moveRobot);
    }  
    
    // Adjust the comparsion for floats with
    // regards to the sensor data. This
    // while loop ideally checks the short range
    // sensors and if they are not equal to 
    // each other, the code will center the
    // robot according to the short range sensor.
    while((distShortL <= distShortR + tolerance) && (distShortL >= distShortR + tolerance))
    {            
        // Adjust this comparsion for floats
        // with regards to the sensor data.
        if(distShortL > (distShortR + tolerance))
        {
            drive.move(0.0, (-5.0)*(3.14159 / 180.0));
            while(!drive.isMoveDone())
            {
                wait(1e-6);
            }
            
            moveRobot.distance = 0.0;
            moveRobot.angle = ((-5.0)*(3.14159 / 180.0));
        
            myStack.push(moveRobot);
        }
        else
        {
            drive.move(0.0, (5.0*(3.14159 / 180.0)));
            while(!drive.isMoveDone())
            {
                wait(1e-6);
            }
            
            moveRobot.distance = 0.0;
            moveRobot.angle = ((5.0)*(3.14159 / 180.0));
            
            myStack.push(moveRobot);
        }
    }
        
    servoL.write(DUTY[dutyR]);
    servoR.write(DUTY[dutyL]);
    
    // This code reinitializes the short distance 
    // variable now that the short distance sensors
    // are pointing inward. 
    distShortL = shortRangeL.getRange();
    distShortR = shortRangeR.getRange();
    
    // This while loop is used to position the robot
    // at the correct location vertically with regards 
    // to the peg in order to pick up the peg. The tolerance
    while(((distShortL + toleranceSHP) <= distShortR) && ((distShortL + toleranceAHP) >= distShortR)) 
    {
        drive.move(0.125, 0.0);
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
        
        moveRobot.distance = 0.125;
        moveRobot.angle = 0.0;
        
        myStack.push(moveRobot);
    }
    
    // This code to enable the gripper to grip the peg. 
    robotGrip.grip();
    
    // This code is used to determine the color of the peg 
    // and the bool value will be used to control the 
    // navigation of the robot.
    bool isPegColorRed;
    
    int colorValue;
    colorValue = 
    
    if(colorValue > 150)
        isPegColorRed = true;
    else
        isPegColorRed = false;        
    
    // This is the code to enable the gripper to lift the peg.
    robotGrip.lift();
    
    // This code uses the stack to reverse all the movement
    // of the robot during this function.
    while(!myStack.empty())
    {   
        myStack.top();
        myStack.pop();
        
        drive.move((-(moveRobot.distance)),(-(moveRobot.angle)));
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
}

// FUNCTION:
//      localize()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Checks localization points.
void Scanner::localize()
{
    pitEnable = 0;
    dutyL = MIN_DUTY;
    dutyR = MAX_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.1);
    distLeft = longRangeL.distInchesL();
    distRight = longRangeR.distInchesR();
    dutyL = MAX_DUTY;
    dutyR = MIN_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.1);
    distForwardL = longRangeL.distInchesL();
    distForwardR = longRangeR.distInchesR();
    pitEnable = 1;
}

// FUNCTION:
//      void localizeLeftMode()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      This method initializes to localizeLeft()
void Scanner::localizeLeftMode()
{   
    localizeRightFlag = 0;
    
    dutyL = MAX_DUTY;
    dutyR = MAX_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.2);
    
    localizeLeftFlag = 1;
}

// FUNCTION:
//      void localizeLeft()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Using sensor longRangeR this method will localize to the wall
//      to the left during stretches of forward motion where robot 
//      should remain xxx distance from the left wall. 
void Scanner::localizeLeft()
{   
    pitEnable = 0;
    float distLocalR = longRangeR.distInchesR();
    
    if (distLocalR >= 9.2)
    {
        drive.pauseMove();
        drive.move(0.0, ((-5.0)*(3.14159 / 180.0)));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
        drive.resumeMove();
    }
    else if (distLocalR <= 8.2)
    {
        drive.pauseMove();
        drive.move(0.0, ((5.0)*(3.14159 / 180.0)));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
        drive.resumeMove();
    }

    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pitEnable = 1;
}

// FUNCTION:
//      void localizeRightMode()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Initializes to localizeRight()
void Scanner::localizeRightMode()
{
    localizeLeftFlag = 0;
    
    dutyL = MIN_DUTY;
    dutyR = MIN_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.2);
    
    localizeRightFlag = 1;
}

// FUNCTION:
//      void localizeRight()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Using sensor longRangeL this method will localize to the wall
//      to the left during stretches of forward motion where robot 
//      should remain xxx distance from the left wall. This function
//      will be called from scan when the localizeRightFlag is set.
void Scanner::localizeRight()
{
    pitEnable = 0;
    float distLocalL = longRangeL.distInchesL();
    
    if (distLocalL >= 9.2)
    {
        drive.pauseMove();
        drive.move(0, ((5.0)*(3.14159 / 180.0)));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
        drive.resumeMove();
    }
    else if (distLocalL <= 8.2)
    {
        drive.pauseMove();
        drive.move(0, ((-5.0)*(3.14159 / 180.0)));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
        drive.resumeMove();
    }
    
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pitEnable = 1;
}

// FUNCTION:
//      scan()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Ticker function to make servos scan either the inward-facing
//      90 degrees or the outward-facing 90 degrees. Distance
//      measures are obtained from both the short range and long
//      range sensors at 15 degree intervals.
void Scanner::scan()
{
    if (pitEnable == 1)
    {
        if (localizeRightFlag == 1)
            localizeRight();
        
        if (localizeLeftFlag == 1)
            localizeLeft();
    }
}