lanetracker.c

为参加日本的ET机器人比赛写的LEGO机器人控制程序

/******************************************************************************\
*
* File:          LaneTracker.C
* Creation date: June 23, 2008 10:30
* Latest update: July 09, 2008 16:39
* Author:        ClassBuilder
*                XXXX
* Purpose:       Method implementations of class 'LaneTracker'
* CB version:    ClassBuilder Version 2.9 (PR523)
*
* Modifications: @INSERT_MODIFICATIONS(* )
* July 09, 2008 16:39 Chijinliang
*     Updated code of method 'Initialize'
*     Updated code of method 'LaneTracker'
* July 09, 2008 16:06 Chijinliang
*     Added method 'FreeStrategies'
*     Updated code of method 'Track'
*     Updated code of method 'Initialize'
*     Updated code of method 'LaneTracker'
* July 09, 2008 15:55 Chijinliang
*     Updated code of method 'Initialize'
* July 09, 2008 15:22 Chijinliang
*     Updated code of method 'CaptureCurrent'
* July 05, 2008 21:07 Chijinliang
*     Updated code of method 'Stop'
*     Updated code of method 'Start'
*     Updated code of method 'Initialize'
* July 05, 2008 21:03 Chijinliang
*     Deleted method 'Track'
*     Updated code of method 'Track'
*     Updated code of method 'LaneTracker'
* July 05, 2008 20:28 Chijinliang
*     Updated code of method 'Track'
* July 05, 2008 14:29 Chijinliang
*     Added method 'UpdateCurrentLaneType'
*     Added method 'Track'
*     Added method 'GetActiveStrategy'
*     Added member 'm_strategies'
*     Added member 'm_currentLaneType'
*     Updated code of method 'Track'
*     Updated interface of method 'Initialize'
* June 25, 2008 19:10 Chijinliang
*     Updated code of method 'Track'
*     Updated code of method 'Stop'
*     Updated code of method 'Start'
*     Updated code of method 'Initialize'
* June 24, 2008 19:51 Chijinliang
*     Added method 'Initialize'
* June 24, 2008 19:44 Chijinliang
*     Added method 'Stop'
*     Added method 'Start'
*     Updated code of method 'GetSteeringController'
*     Updated code of method 'Track'
* June 23, 2008 17:39 Chijinliang
*     Added method 'GetSteeringController'
*     Updated member 'm_steeringController'
* June 23, 2008 14:03 Chijinliang
*     Updated code of method 'CaptureCurrent'
* June 23, 2008 13:46 Chijinliang
*     Added method 'LaneTracker'
* June 23, 2008 13:42 Chijinliang
*     Updated code of method 'Track'
* June 23, 2008 10:30 Chijinliang
*     Added method 'DestructorInclude'
*     Added method 'ConstructorInclude'
*     Added method 'Track'
*     Added method 'CaptureCurrent'
*     Added method 'CalibrationWhite'
*     Added method 'CalibrationGray'
*     Added method 'CalibrationBlack'
*     Added method '~LaneTracker'
*     Added member 'm_calibration'
*     Added member 'm_lightSensor'
*     Added member 'm_steeringController'
*     Added member 'm_driveMotor'
*     Updated return type of method 'Track'
*
* Copyright 2008, XXXXX
* All rights are reserved. Reproduction in whole or part is prohibited
* without the written consent of the copyright owner.
*
\******************************************************************************/
// Master include file
#include "JASTInTime.h"


using namespace JASTInTime;




/*
Constructor method.
*/
LaneTracker::LaneTracker()

  
{
    ConstructorInclude();

    m_currentLaneType = Straight;
    //m_strategies[0] = (Strategy*)NULL;
    //m_strategies[1] = (Strategy*)NULL;
    //m_strategies[2] = (Strategy*)NULL;
    //m_strategies[3] = (Strategy*)NULL;
    //m_strategies[4] = (Strategy*)NULL;
    //m_strategies[5] = (Strategy*)NULL;
    
    m_trackType = TrackLeftEdge;
    m_minColor = 0;
    m_prevColor = 0;
    m_grayCount = 0;
    m_outCourse = true;
    m_bFirstLoop = true;
    m_bMarker = false;
    m_markerCount = 0;
    m_bChecked = false;
    m_markerIndex = 0;
    m_bMarker = false;
    m_bMarker0 = false;
}


/*
Destructor method.
*/
LaneTracker::~LaneTracker()
{
    // Put in your own code

    DestructorInclude();
}


void LaneTracker::CalibrationBlack()
{
    m_calibration.SetBlack(CaptureCurrent(false));
}


void LaneTracker::CalibrationGray()
{
    m_calibration.SetGray(CaptureCurrent(false));

}


void LaneTracker::CalibrationWhite()
{
    m_calibration.SetWhite(CaptureCurrent(false));

}


unsigned int LaneTracker::CaptureCurrent(bool needConfirm) const
{
    //m_lightSensor.Active();
    unsigned int value = m_lightSensor.GetValue();
    for(int index = 1; index < 20; index++)
    {
        value += m_lightSensor.GetValue();
    }
    
    value /= 20;
    
    cputw(value);
    
    if (needConfirm)
    {
        // wait until View button pressed
        getchar();
    }
    
    //m_lightSensor.Passive();
    
    return value;
}


void LaneTracker::Execute(const Command* pCommand)
{
/*    if (pCommand->GetFlag() & 0x01)
    {
        m_steeringController.Execute(pCommand);
    }
    
    if (pCommand->GetFlag() & 0x02)
    {
        if (pCommand->GetSpeed() >= 0)
        {
            m_driveMotor.SetSpeed(pCommand->GetSpeed());
        }
        else
        {
            m_driveMotor.SetSpeed(-pCommand->GetSpeed());
        }
    }
*/
}


void LaneTracker::FreeStrategies()
{
    // TODO:: Enter code for method 'FreeStrategies'
}

/*
Strategy* LaneTracker::GetActiveStrategy() const
{
    return m_strategies[0];
}
*/

void LaneTracker::Initialize(bool outCourse)
{
    m_outCourse = outCourse;
    m_bFirstLoop = true;
    m_lightSensor.Initialize();
    m_driveMotor.Brake();
    m_driveMotor.SetSpeed(255);
    m_steeringMotor.Brake();
    m_steeringMotor.SetSpeed(255);
    /*
    FreeStrategies();
    
    if (outCourse)
    {
        m_strategies[0] = new Strategy(FollowLeftEdge, m_calibration.GetBlackGray(), m_calibration.GetWhiteGray());
        m_strategies[1] = (Strategy*)NULL;
        m_strategies[2] = (Strategy*)NULL;
        m_strategies[3] = (Strategy*)NULL;
        m_strategies[4] = (Strategy*)NULL;
        m_strategies[5] = (Strategy*)NULL;
    }
    else
    {
        m_strategies[0] = new Strategy(FollowRightEdge, m_calibration.GetBlackGray(), m_calibration.GetWhiteGray());
        m_strategies[1] = (Strategy*)NULL;
        m_strategies[2] = (Strategy*)NULL;
        m_strategies[3] = (Strategy*)NULL;
        m_strategies[4] = (Strategy*)NULL;
        m_strategies[5] = (Strategy*)NULL;
    }
    */
    m_currentLaneType = Straight;
    
    if (outCourse)
    {
        m_trackType = TrackRightEdge;
    }
    else
    {
        m_trackType = TrackLeftEdge;
    }
    
    m_markerIndex = 0;
    m_bMarker = false;
    m_bMarker0 = false;
}


void LaneTracker::Start()
{
    m_prevColor = m_lightSensor.GetCurrent();
    m_driveMotor.Forward();
}


void LaneTracker::Stop()
{
    m_lightSensor.Passive();
    m_driveMotor.SetSpeed(0);
    m_driveMotor.Stop();
    m_steeringMotor.Stop();
    //m_steeringController.Stop();
}


int LaneTracker::Track()
{
    unsigned int sensorValue = m_lightSensor.GetCurrent();
    //Color currentColor = ColorDetector::Detect(m_lightSensor.GetCurrent(), m_calibration);
    /*
    int steeringAngle = m_steeringController.GetSteeringAngle();
    
    Strategy* pStrategy = GetActiveStrategy();
    if (pStrategy == NULL)
    {
        return -1;
    }
    
    const Command* pCommand = pStrategy->Judge(sensorValue, steeringAngle);
    Execute(pCommand);
    */
    //if ((currentColor == WhiteGray) || (currentColor == White))
    //if (sensorValue >= (m_calibration.GetWhite() - 3))
    if (sensorValue >= (m_calibration.GetGray() + 2))
    {
        if (m_trackType == TrackLeftEdge)
        {
            m_steeringMotor.Right();
        }
        else
        {
            m_steeringMotor.Left();
        }
        
        m_grayCount = 0;
    }
    else if (sensorValue >= (m_calibration.GetGray() - 1))
    {
        m_grayCount++;
        if (m_grayCount >= 3)
        {
            if (m_trackType == TrackLeftEdge)
            {
                m_steeringMotor.Left();
            }
            else
            {
                m_steeringMotor.Right();
            }
            
            m_markerCount++;
        }
    }
    else 
    {
        m_markerCount = 0;
        m_grayCount = 0;
        if (m_trackType == TrackLeftEdge)
        {
            m_steeringMotor.Left();
        }
        else
        {
            m_steeringMotor.Right();
        }
    }
    //else if (sensorValue <= (m_calibration.GetGray() + 1))
    //else if (sensorValue <= (m_calibration.GetGray() + 1))
    //else if ((currentColor == BlackGray) || (currentColor == Black))
    /*{
        if (m_trackType == TrackLeftEdge)
        {
            m_steeringMotor.Left();
        }
        else
        {
            m_steeringMotor.Right();
        }
    }*/
    
    DetectMarker(sensorValue);
    
    if (m_bMarker)
    {
    }
    
    UpdateCurrentLaneType();

    m_prevColor = sensorValue;
    return 0;
}

void LaneTracker::DetectMarker(unsigned int sensorValue)
{
    if (m_markerCount >= 6)
    {
        m_bMarker = true;
        if (!m_bMarker0)
        {
            m_markerIndex++;
        }
    }
    else
    {
        m_bMarker = false;
    }
    
    cputw(m_markerIndex);
    m_bMarker0 = m_bMarker;
    /*
    if (m_bChecked)
    {
        if (sensorValue >= (m_calibration.GetGray() + 2))
        {
            m_minColor = sensorValue;
            m_bChecked = false;
        }
    }
    
    if (!m_bChecked)
    {
        if (sensorValue < m_minColor)
        {
            m_minColor = sensorValue;
        }
        
        if (sensorValue > (m_minColor + 1))
        {
            m_bChecked = true;
            if ((m_minColor >= (m_calibration.GetGray() - 1)) && (m_minColor <= (m_calibration.GetGray() + 1)))
            {
                m_grayCount++;
                cputw(m_grayCount);
            }
            else
            {
                m_grayCount = 0;
            }
            
            if (m_grayCount >= 3)
            {
                m_bMarker = true;
                cputs("11111");
            }
            else
            {
                m_bMarker = false;
            }
        }
    }
    */
    /*
    if ((!m_bChecked) && (sensorValue > (m_minColor + 1)))
    {
        m_bChecked = true;
        if ((m_minColor >= (m_calibration.GetGray() - 1)) && (m_minColor <= (m_calibration.GetGray() + 1)))
        {
            m_grayCount++;
            cputw(m_grayCount);
        }
        else
        {
            m_grayCount = 0;
        }
        
        if (m_grayCount >= 3)
        {
            m_bMarker = true;
            cputs("11111");
        }
        else
        {
            m_bMarker = false;
        }
        
        m_minColor = m_calibration.GetWhite() - 1;
    }*/
}

void LaneTracker::UpdateCurrentLaneType()
{
    // TODO:: Enter code for method 'UpdateCurrentLaneType'
}


/*
Method which must be called first in a constructor.
*/
void LaneTracker::ConstructorInclude()
{
}


/*
Method which must be called first in a destructor.
*/
void LaneTracker::DestructorInclude()
{
}


// Methods for the relation(s) of the class