File content as of revision 1:2714d60284ef:

#include "scanner.h"
#include "mbed.h"
#include "Gp2x.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, Gp2x &_longRangeL,
    Gp2x &_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);
    
    for (int i = 0; i < 13; i++)
        DUTY[i] = 0.0300 + (i * 0.0075); // for radioshack servo
    
    /*for (int i = 0; i < 13; i++)
        DUTY[i] = 0.0500 + (i * DELTA_DUTY); // 0.05 = min duty cycle
        // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708,
        // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958, 
        // 0.1000};*/
    
    // initializing to avoid status
    invertL = 1;
    invertR = -1;
    dutyL = MIN_DUTY;
    dutyR = MAX_DUTY;
    servoL.write(DUTY[dutyL]);
    servoR.write(DUTY[dutyR]);
    wait(0.2);
    pitEnable = 1;
    pit.attach(this, &Scanner::scan, period);
}

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

// FUNCTION:
//      avoid()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Scans to avoid obstacles.
void Scanner::avoid()
{
    pitEnable = 0;
    invertL = 1;
    invertR = -1;
    dutyL = MIN_DUTY;
    dutyR = MAX_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.
int Scanner::localize()
{
    /* need to implement */
    return 0;
}

// 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
        dutyL += invertL;
        dutyR += invertR;
        servoL.write(DUTY[dutyL]);
        servoR.write(DUTY[dutyR]);
    
        if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) || 
            ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
            ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
        {
            invertL *= -1;
            invertR *= -1;
        }
    }
}