[mikets]

Our line following algorithm uses PID control. Basically, we combined the readings of the three light sensors into a 3-bit value (0-7) and used it to index a table. If the line is right at the center, it maps to 0.0. If the line is slightly to the left, it maps to 1.0, more to the left maps to 2.0. If the line is slightly to the right, its maps to -1.0 and -2.0 if further right. 999.0 is just a number used to indicate no valid line is found. The number -2.0 to 2.0 is then used as the current input in PID control calculation and the setpoint is 0.0 (the center). The turning power is proportional on how far we are "off course". The speed driving forward is inversely proportional to the turning power. So the harder we turn, the slower we go. Our algorithm is actually more complicated than that but a simplified version is shown below.

UINT32
GetRawValue(
    void
    )
{
    UINT32 value = 0;
 
    value = m_leftLightSensor->Get() << 2;
    value |= m_centerLightSensor->Get() << 1;
    value |= m_rightLightSensor->Get();
 
    return value;
}   //GetRawValue
 
static float inputMap[8] =
{
    999.0, //000: n/a (No light)
    -2.0,   //001: Extreme right
    0.0,    //010: Center
    -1.0,   //011: Slight right
    2.0,    //100: Extreme left
    999.0, //101: n/a (At Y)
    1.0,    //110: Slight left
    999.0  //111: n/a (At T)
};
 
UINT32 sensorRawValue = GetRawValue();
if (sensorRawValue == 0)
{
    //
    // deal with losing the line.
    //
}
else if (sensorRawValue == 0x5)
{
    //
    // deal with the Y fork.
    //
}
else if (sensorRawValue == 0x7)
{
    //
    // deal with the T-end.
    //
}
else
{
    float sensorMappedValue = inputMap[sensorRawValue];
    float turnPower = CalcPIDOutput(sensorMappedValue);
    float drivePower = m_maxDrivePower*(1.0 - fabs(turnPower));
    ArcardeDrive(drivePower, turnPower);
}