car.c

eaayarm101自制小车源代码 周立功公司原创

//
//*****************************************************************************
static ZERO_INIT tCarMode g_eMode;

//*****************************************************************************
//
//! A delay count used for timing some events (such as reversing the direction
//! of the motors in diagnostic modes).
//
//*****************************************************************************
static ZERO_INIT unsigned char g_ucDelay;

//*****************************************************************************
//
//! A counter used for flashing the lights in diagnostic modes and for counting
//! off 1/10th of a second in running mode.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usCount;

//*****************************************************************************
//
//! The current reading from the left sensor, in the fixed point inches
//! representation.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usLeftSensor;

//*****************************************************************************
//
//! The change in the left sensor reading, in the fixed point inches
//! representation.
//
//*****************************************************************************
static ZERO_INIT short g_sLeftDelta;

//*****************************************************************************
//
//! The current reading from the right sensor, in the fixed point inches
//! representation.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usRightSensor;

//*****************************************************************************
//
//! The change in the right sensor reading, in the fixed point inches
//! representation.
//
//*****************************************************************************
static ZERO_INIT short g_sRightDelta;

//*****************************************************************************
//
//! The current reading from the front sensor, in the fixed point inches
//! representation.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usFrontSensor;

//*****************************************************************************
//
//! The change in the front sensor reading, in the fixed point inches
//! representation.
//
//*****************************************************************************
static ZERO_INIT short g_sFrontDelta;

//*****************************************************************************
//
//! The current speed of the left motor.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usMotorLeft;

//*****************************************************************************
//
//! The current speed of the right motor.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usMotorRight;

//*****************************************************************************
//
//! The target speed of the left motor.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usTargetLeft;

//*****************************************************************************
//
//! The target speed of the right motor.
//
//*****************************************************************************
static ZERO_INIT unsigned short g_usTargetRight;

//*****************************************************************************
//
//! This table converts sensor readings into distances in inches.  The sensors
//! are not linear, so this table is the invert of the sensor map.  This data
//! was collected by direct sampling of one sensor; other sensors may not be
//! exactly the same (in fact probably aren't) but this should be close enough.
//
//*****************************************************************************
static const unsigned short g_pusMap[23] =
{
    817,                                    // 4 in (10.1 cm)
    683,                                    // 5 in (12.7 cm)
    586,                                    // 6 in (15.2 cm)
    514,                                    // 7 in (17.8 cm)
    454,                                    // 8 in (20.3 cm)
    407,                                    // 9 in (22.9 cm)
    370,                                    // 10 in (25.4 cm)
    340,                                    // 11 in (27.9 cm)
    312,                                    // 12 in (30.5 cm)
    292,                                    // 13 in (33 cm)
    269,                                    // 14 in (35.6 cm)
    250,                                    // 15 in (38.1 cm)
    236,                                    // 16 in (40.6 cm)
    222,                                    // 17 in (43.2 cm)
    212,                                    // 18 in (45.7 cm)
    202,                                    // 19 in (48.3 cm)
    188,                                    // 20 in (50.8 cm)
    177,                                    // 21 in (53.3 cm)
    167,                                    // 22 in (55.9 cm)
    160,                                    // 23 in (58.4 cm)
    155,                                    // 24 in (61 cm)
    150,                                    // 25 in (63.5 cm)
    146                                     // 26 in (66 cm)
};

//*****************************************************************************
//
//! Converts a sensor reading into a distance in inches.
//!
//! \param ulValue is the sensor reading from the ADC.
//!
//! The sensors produce a voltage that is non-linear and inversely proportional
//! to the distance to the object, which therefore means that the ADC reading
//! has these same properties.  This function effectively provides an inverse
//! transform of the transfer function of the sensor; it makes the reading
//! linear and directly proportional to the distance, and converts the distance
//! into inches.
//!
//! The conversion is not 100% accurate.  There is a table (in #g_pusMap) that
//! contains the distance for several points along the sensor curve;
//! intermediary points are converted using linear interpolation.  This linear
//! interpolation introduces some inaccuracy, though it is small enough to not
//! matter in this application.
//!
//! \return Returns the distance in the fixed point inches representation.
//
//*****************************************************************************
static long
CarConvertSensorReading(unsigned long ulValue)
{
    unsigned long ulIdx;

    //
    // Return 4in if the sensor value is greater than the 4in entry in the
    // sensor map.
    //
    if(ulValue > g_pusMap[0])
    {
        return(INCHES(4, 0));
    }

    //
    // Return "infinity" if the sensor value is less than the 26in entry in the
    // sensor map.
    //
    if(ulValue < g_pusMap[22])
    {
        return(INFINITY);
    }

    //
    // Loop through the sensor map to find the first entry that is less than
    // the sensor value.
    //
    for(ulIdx = 1; ulIdx < 23; ulIdx++)
    {
        if(g_pusMap[ulIdx] <= ulValue)
        {
            break;
        }
    }

    //
    // Return the distance.  Use linear interpolation to provide accuracy
    // better than 1 inch (granted the sensor readings aren't linear, but this
    // is fast and "close enough").
    //
    return(INCHES(4, 0) + (ulIdx * INCHES(1, 0)) -
           (((ulValue - g_pusMap[ulIdx]) * INCHES(1, 0)) /
            (g_pusMap[ulIdx - 1] - g_pusMap[ulIdx])));
}

//*****************************************************************************
//
//! Initializes the internal state of the car.
//!
//! This function initializes the internal state of the car, preparing it for
//! normal operation.  This should be called before the car is allowed to start
//! roaming around.
//!
//! \return None.
//
//*****************************************************************************
void
CarInit(void)
{
    g_usLeftSensor = INFINITY;
    g_sLeftDelta = 0;
    g_usRightSensor = INFINITY;
    g_sRightDelta = 0;
    g_usFrontSensor = INFINITY;
    g_sFrontDelta = 0;
    g_usMotorLeft = SPEED(0);
    g_usMotorRight = SPEED(0);
}

//*****************************************************************************
//
//! Performs all the decision making for the car.
//!
//! This function is called every tick of the clock, allowing the car to make
//! decisions on a regular basis.  The majority of the car's state machine is
//! contained within this function, providing the car's reactions to the
//! surrounding environment.
//!
//! \return None.
//
//*****************************************************************************
void
CarTick(void)
{
    long lLeft, lRight, lDiff;

    //
    // Do some up front work in running modes.
    //
    if((g_eMode == MODE_IN_OPEN) ||
       (g_eMode == MODE_APPROACHING) ||
       (g_eMode == MODE_FOLLOWING) ||
       (g_eMode == MODE_TURN_LEFT) ||
       (g_eMode == MODE_TURN_RIGHT) ||
       (g_eMode == MODE_TURNING) ||
       (g_eMode == MODE_FORWARD) ||
       (g_eMode == MODE_RANDOM_LEFT) ||
       (g_eMode == MODE_RANDOM_RIGHT) ||
       (g_eMode == MODE_RANDOM_TURN) ||
       (g_eMode == MODE_RANDOM_FORWARD))
    {
        //
        // Compute the new speed for the motors.
        //
        if((g_usMotorLeft > g_usTargetLeft) &&
           (g_usMotorRight > g_usTargetRight))
        {
            g_usMotorLeft = (((g_usMotorLeft * 45575) +
                              (g_usTargetLeft * 19661)) / 65536);
            g_usMotorRight = (((g_usMotorRight * 45575) +
                               (g_usTargetRight * 19661)) / 65536);
        }
        else
        {
            g_usMotorLeft = (((g_usMotorLeft * 58982) +
                              (g_usTargetLeft * 6554)) / 65536);
            g_usMotorRight = (((g_usMotorRight * 58982) +
                               (g_usTargetRight * 6554)) / 65536);
        }

        //
        // Set the new motor speed.
        //
        MotorLeftSpeed((g_usMotorLeft + (SPEED(1) / 2)) / SPEED(1));
        MotorRightSpeed((g_usMotorRight + (SPEED(1) / 2)) / SPEED(1));

        //
        // Return without doing anything if 1/10th of a second has not passed.
        //
        if(++g_ucDelay != (SYSTICK_CLOCK / 10))
        {
            return;
        }

        //
        // Reset the counter.
        //
        g_ucDelay = 0;

        //
        // Grab the left sensor reading.
        //
        lDiff = CarConvertSensorReading(g_pusSamples[0]);

        //
        // Determine how to handle the sensor reading.
        //
        if((g_usLeftSensor == INFINITY) && (lDiff != INFINITY) &&
           (g_usFrontSensor == INFINITY))
        {
            //
            // The sensor reading just went from infinity to something
            // non-infinite, and there is nothing to the front, so set the
            // initial delta really high so that a quick response is provided
            // when approaching a block in the middle of the arena.
            //
            g_sLeftDelta = INCHES(31, 999);
            g_usLeftSensor = lDiff;
        }
        else if((g_usLeftSensor == INFINITY) || (lDiff == INFINITY))
        {
            //
            // The sensor reading is going to or coming from infinity, so set
            // the delta to zero.
            //
            g_sLeftDelta = 0;
            g_usLeftSensor = lDiff;
        }
        else
        {
            //
            // Low pass filter the delta and sensor reading.
            //
            g_sLeftDelta = ((3 * g_sLeftDelta) + (g_usLeftSensor - lDiff)) / 4;
            g_usLeftSensor = ((3 * g_usLeftSensor) + lDiff) / 4;
        }

        //
        // Grab the right sensor reading.
        //
        lDiff = CarConvertSensorReading(g_pusSamples[1]);

        //
        // Determine how to handle the sensor reading.
        //
        if((g_usRightSensor == INFINITY) && (lDiff != INFINITY) &&
           (g_usFrontSensor == INFINITY))
        {
            //
            // The sensor reading just went from infinity to something
            // non-infinite, and there is nothing to the front, so set the
            // initial delta really high so that a quick response is provided
            // when approaching a block in the middle of the arena.
            //
            g_sRightDelta = INCHES(31, 999);
            g_usRightSensor = lDiff;
        }
        else if((g_usRightSensor == INFINITY) || (lDiff == INFINITY))
        {
            //
            // The sensor reading is going to or coming from infinity, so set
            // the delta to zero.
            //
            g_sRightDelta = 0;
            g_usRightSensor = lDiff;
        }
        else
        {
            //
            // Low pass filter the delta and sensor reading.
            //
            g_sRightDelta = (((3 * g_sRightDelta) +
                              (g_usRightSensor - lDiff)) / 4);
            g_usRightSensor = ((3 * g_usRightSensor) + lDiff) / 4;
        }

        //
        // Grab the front sensor reading.
        //
        lDiff = CarConvertSensorReading(g_pusSamples[2]);

        //
        // Determine how to handle the sensor reading.
        //
        if((g_usFrontSensor == INFINITY) || (lDiff == INFINITY))
        {
            //
            // The sensor reading is going to or coming from infinity, so set
            // the delta to zero.
            //
            g_sFrontDelta = 0;
            g_usFrontSensor = lDiff;
        }
        else
        {
            //
            // Low pass filter the delta and sensor reading.