strategy.c

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

/******************************************************************************\
*
* File:          Strategy.C
* Creation date: June 23, 2008 10:30
* Latest update: July 09, 2008 16:39
* Author:        ClassBuilder
*                XXXX
* Purpose:       Method implementations of class 'Strategy'
* CB version:    ClassBuilder Version 2.9 (PR523)
*
* Modifications: @INSERT_MODIFICATIONS(* )
* July 09, 2008 16:35 Chijinliang
*     Deleted method 'GetMaxLeftAngle'
*     Deleted method 'GetMaxRightAngle'
*     Deleted method 'OnTargetDetected'
*     Deleted method 'OnTargetNotDetected'
*     Deleted member 'm_targetColor'
*     Deleted member 'm_trackColor'
*     Deleted method 'Strategy'
*     Added member 'm_maxColor'
*     Added member 'm_minColor'
*     Updated interface of method 'Strategy'
*     Updated member 'm_steeringAngle'
*     Updated member 'm_maxRightAngle'
*     Updated member 'm_maxLeftAngle'
*     Updated member 'm_previousColor'
* July 08, 2008 10:25 Chijinliang
*     Added method 'Strategy'
* July 05, 2008 21:03 Chijinliang
*     Updated code of method 'Strategy'
* July 05, 2008 20:49 Chijinliang
*     Updated code of method 'Strategy'
* July 05, 2008 20:28 Chijinliang
*     Deleted method 'JudgeYY'
*     Updated code of method 'OnTargetNotDetected'
*     Updated code of method 'OnTargetDetected'
*     Updated code of method 'Judge'
* July 05, 2008 14:29 Chijinliang
*     Deleted method 'Add'
*     Deleted method 'InsertAfter'
*     Deleted method 'InsertBefore'
*     Deleted member 'm_next'
*     Deleted member 'm_previous'
*     Deleted member 'm_priority'
*     Deleted member 'm_activeTrackType'
*     Deleted member 'm_targetSpeed'
*     Deleted member 'm_targetSteeringAngle'
*     Added method 'GetMarkerDetected'
*     Added method 'GetFinished'
*     Added method 'Judge'
*     Added member 'm_command'
*     Added member 'm_finished'
*     Added member 'm_markerDetected'
*     Updated interface of method 'SetTrackColor'
*     Updated code of method 'GetTrackColor'
*     Updated interface of method 'SetTargetColor'
*     Updated code of method 'GetTargetColor'
*     Updated interface of method 'SetPreviousColor'
*     Updated code of method 'GetPreviousColor'
*     Updated code of method 'GetCurrentColor'
*     Updated code of method 'OnTargetNotDetected'
*     Updated code of method 'OnTargetDetected'
*     Updated interface of method 'JudgeYY'
*     Updated interface of method 'Strategy'
*     Updated member 'm_currentColor'
*     Updated member 'm_trackColor'
*     Updated member 'm_targetColor'
*     Updated member 'm_previousColor'
*     Updated return type of method 'JudgeX'
* June 23, 2008 13:42 Chijinliang
*     Updated code of method 'Judge'
*     Updated code of method 'Strategy'
* June 23, 2008 10:30 Chijinliang
*     Added method 'DestructorInclude'
*     Added method 'ConstructorInclude'
*     Added method 'SetTrackType'
*     Added method 'GetTrackType'
*     Added method 'SetTrackColor'
*     Added method 'GetTrackColor'
*     Added method 'SetTargetSteeringAngle'
*     Added method 'GetTargetSteeringAngle'
*     Added method 'SetTargetSpeed'
*     Added method 'GetTargetSpeed'
*     Added method 'SetTargetColor'
*     Added method 'GetTargetColor'
*     Added method 'GetSteeringAngle'
*     Added method 'SetPriority'
*     Added method 'GetPriority'
*     Added method 'SetPreviousColor'
*     Added method 'GetPreviousColor'
*     Added method 'GetPrevious'
*     Added method 'GetNext'
*     Added method 'GetMaxRightAngle'
*     Added method 'GetMaxLeftAngle'
*     Added method 'GetCurrentColor'
*     Added method 'OnTargetNotDetected'
*     Added method 'OnTargetDetected'
*     Added method 'Judge'
*     Added method 'InsertBefore'
*     Added method 'InsertAfter'
*     Added method 'Add'
*     Added method '~Strategy'
*     Added method 'Strategy'
*     Added member 'm_activeTrackType'
*     Added member 'm_steeringAngle'
*     Added member 'm_currentColor'
*     Added member 'm_trackColor'
*     Added member 'm_maxRightAngle'
*     Added member 'm_maxLeftAngle'
*     Added member 'm_targetColor'
*     Added member 'm_previousColor'
*     Added member 'm_trackType'
*     Added member 'm_targetSpeed'
*     Added member 'm_targetSteeringAngle'
*     Added member 'm_previous'
*     Added member 'm_next'
*     Added member 'm_priority'
*     Updated return type of method 'Judge'
*     Updated return type of method 'OnTargetNotDetected'
*     Updated return type of method 'OnTargetDetected'
*     Updated return type of method 'OnTargetNotDetected'
*
* 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.
*/
Strategy::Strategy(TrackType trackType, unsigned int minColor,
                   unsigned int maxColor)

{
    ConstructorInclude();

    m_trackType = trackType;
    m_minColor = minColor;
    m_maxColor = maxColor;
    m_previousColor = minColor;
    m_maxLeftAngle = 0;
    m_maxRightAngle = 0;
    m_markerDetected = false;
    m_finished = false;
    m_steeringAngle = 0;
    m_maxCount = 0;
    m_minCount = 0;

}


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

    DestructorInclude();
}


/*
the return value is used to indicate if target speed or target steering angle was modified
*/
const Command* Strategy::Judge(unsigned int color, int steeringAngle)
{
#define ABSANGLE   134
    int offset = ABSANGLE;
    m_command.SetFlag(0);
    int targetAngle = 0;
    if (color <= m_minColor)
    {
        m_minCount++;
        if (m_minCount > 2)
        {
            offset = ABSANGLE * 2;
        }
        else if (m_minCount > 5) 
        {
            offset = ABSANGLE * 3;
        }
        
        if (m_trackType == FollowLeftEdge)
        {
            targetAngle = steeringAngle + offset;
        }
        else
        {
            targetAngle = steeringAngle - offset;
        }
        m_maxCount = 0;
    }
    else if (color >= m_maxColor)
    {
        m_maxCount++;
        if (m_maxCount > 2)
        {
            offset = ABSANGLE * 2;
        }
        else if (m_maxCount > 5)
        {
            offset = ABSANGLE * 3;
        }
        
        if (m_trackType == FollowLeftEdge)
        {
            targetAngle = steeringAngle - offset;
        } 
        else
        {
            targetAngle = steeringAngle + offset;
        }
        m_minCount = 0;
    }
    else
    {
        m_minCount = 0;
        m_maxCount = 0;
    }
    
    if (steeringAngle < m_maxLeftAngle)
    {
        m_maxLeftAngle = steeringAngle;
    }
    
    if (steeringAngle > m_maxRightAngle)
    {
        m_maxRightAngle = steeringAngle;
    }
    
    if (targetAngle > 15000)
    {
        targetAngle = 15000;
    }
    else if (targetAngle < -15000)
    {
        targetAngle = -15000;
    }
    
    m_command.SetSteeringMode(TargetAngle);
    m_command.SetSteeringAngle(targetAngle);
    
    if ((targetAngle > 1500) || (targetAngle < -1500))
    {
        m_command.SetSpeed(100);
    }
    else if ((targetAngle > 800) || (targetAngle < -800))
    {
        m_command.SetSpeed(150);
    }
    else if ((targetAngle > 500) || (targetAngle < -500))
    {
        m_command.SetSpeed(200);
    }
    else
    {
        m_command.SetSpeed(255);
    }
    
    //cputw(m_maxLeftAngle);
    m_steeringAngle = steeringAngle;
    return &m_command;
}


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


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


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