File content as of revision 8:32a445ae1d72:

#include "scanner.h"
#include "mbed.h"
#include "LongRangeSensor.h"
#include "ShortRangeSensor.h"
#include "StepperDrive.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, float _period) : pc(pc1), drive(_drive), servoL(_servoL),
    servoR(_servoR), shortRangeL(_shortRangeL),
    shortRangeR(_shortRangeR), longRangeL(_longRangeL),
    longRangeR(_longRangeR), period(_period)
{
    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 avoid status
    avoidFlag = 1;
    huntFlag = 0;
    invertL = -1;
    invertR = 1;
    dutyL = MAX_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()
{
    // Used to store temporary distance 
    // recorded from VL6180x short range
    // sensor.
    float tempDistShortL;
    float tempDistShortR;

    // Used to temporary store the number 
    // of stepper motor increments.
    int tempStepCount = 0;
    
    pitEnable = 0;
    
    servoL.write(DUTY[4]);
    servoR.write(DUTY[2]);
    
    // Update this code using the new VL6180x code
    // from Victor.
    // tempDistShortL = 
    // tempDistShortR =
    
    // This if statement moves the robot forward in order
    // to continue the hunt of the peg.
/*    while(distShortL == 0 && distShortR == 0 && tempStepCount < 34)
    {
        drive.move(0.25, 0.0);
    }*/  
    
    // Adjust the comparsion for floats with
    // regards to the sensor data.
    // This while loop checks the short range
    // sensors and if they are not equal to 
    // each other, the code will center the
    // robot.
    while(tempDistShortL != tempDistShortR)
    {            
        // Adjust this comparsion for floats
        // with regards to the sensor data.
        if(tempDistShortL > tempDistShortR)
            drive.move(0.0, -5.0);
        else
            drive.move(0.0, 5.0);
    }
        
    servoL.write(DUTY[dutyR]);
    servoR.write(DUTY[dutyL]);
    
    // Update this code with the new VL6180x sensor
    // code from Victor.
    // tempDistShortL =
    // tempDistShortR = 
    
    // This while loop is used to position the robot
    // at the correct location in order to pick up
    // the peg.
    while(tempDistShortL == tempDistShortR)
    {
        /* pseudocode
        implement the code for the gripper to pick up the peg
        implement the code for reading a value from the color sensor
        store the value that is returned from the color sensor
        implement the code for the gripper to angle up
        pop the stack and reverse the moves stored in the stack
        return a done value
        */   
    }    
}

// 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 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()
{   
    float distLocalR = longRangeR.distInchesR();
    
    if (distLocalR > 8.5)
    {
        pitEnable = 0;
        drive.pauseMove();
        drive.move(0.0, -1.0);
        drive.resumeMove();
        pitEnable = 1;
    }
    else if (distLocalR < 7.5)
    {
        pitEnable = 0;
        drive.pauseMove();
        drive.move(0.0, 1.0);
        drive.resumeMove();
        pitEnable = 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()
{
    float distLocalL = longRangeL.distInchesL();
    
    if (distLocalL > 8.5)
    {
        pitEnable = 0;
        drive.pauseMove();
        drive.move(0.0, 1.0);
        drive.resumeMove();
        pitEnable = 1;
    }
    else if (distLocalL < 7.5)
    {
        pitEnable = 0;
        drive.pauseMove();
        drive.move(0.0, -1.0);
        drive.resumeMove();
        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)
    {
        /*// obtain distance measures from sensors
        distLeft = longRangeL.distInchesL();
        distRight = longRangeR.distInchesR();
        
        //check localize flags and make necessary method calls
        
        // check avoid flag
        if (avoidFlag == 1)
        {
            // add obstacle avoid code
        }
        
        // check hunt flag
        if (huntFlag == 1)
        {
            // add hunt code
        }*/

        // increment/decrement servo position
        dutyL += invertL;
        dutyR += invertR;
        servoL.write(DUTY[dutyL]);
        servoR.write(DUTY[dutyR]);
        if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
            ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
        {
            invertL *= -1;
            invertR *= -1;
        }
    }
}