File content as of revision 6:5e24ff86b743:

#include "scanner.h"
#include "mbed.h"
#include "LongRangeSensor.h"
#include "VL6180x.h"

// FUNCTION:
//      Scanner()
// IN-PARAMETERS:
//      Serial &pc1, 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, PinName _servoL, PinName _servoR,
    VL6180x &_shortRangeL, VL6180x &_shortRangeR, LongRangeSensor &_longRangeL,
    LongRangeSensor &_longRangeR, float _period) : pc(pc1), 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:
//      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()
{
    /****** pseudocode *****
    
    dist = longRangeR dist measure
    
    if (dist > xxx dist)
        pitEnable = 0
        pause current route
        adjust angle to left
        unpause route
        pitEnable = 1
        
        
    else if (dist < xxx dist)
        pitEnable = 0
        pause current route
        adjust angle to right
        unpause route
        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 localizeRight()
{
    /****** pseudocode *****
    
    dist = longRangeL dist measure
    
    if (dist > xxx dist)
        pitEnable = 0
        pause current route
        adjust angle to right
        unpause route
        pitEnable = 1
        
    else if (dist < xxx dist)
        pitEnable = 0
        pause current route
        adjust angle to left
        unpause route
        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;
        }
    }
}