steeringcontroller.c

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

/******************************************************************************\
*
* File:          SteeringController.C
* Creation date: June 23, 2008 10:30
* Latest update: July 09, 2008 16:39
* Author:        ClassBuilder
*                XXXX
* Purpose:       Method implementations of class 'SteeringController'
* CB version:    ClassBuilder Version 2.9 (PR523)
*
* Modifications: @INSERT_MODIFICATIONS(* )
* July 09, 2008 15:48 Chijinliang
*     Added member 'm_newCommand'
*     Updated code of method 'SteeringController'
* July 09, 2008 15:46 Chijinliang
*     Added method 'UpdateTargetTime'
*     Updated code of method 'GetMode'
*     Updated code of method 'UpdateCommand'
*     Updated code of method 'Execute'
*     Updated code of method 'ControlMotor'
*     Updated member 'm_mode'
* July 09, 2008 15:22 Chijinliang
*     Added method 'UpdateCommand'
*     Added member 'm_command'
*     Updated code of method 'Execute'
*     Updated code of method 'ControlMotor'
*     Updated code of method 'Control'
*     Updated code of method 'CalculateSteeringAngle'
*     Updated code of method 'SteeringController'
* July 09, 2008 12:22 Chijinliang
*     Deleted member 'm_steeringSpeed'
* July 09, 2008 12:19 Chijinliang
*     Updated code of method 'ControlMotor'
*     Updated code of method 'Control'
* July 09, 2008 12:16 Chijinliang
*     Updated code of method 'SteeringController'
* July 09, 2008 12:13 Chijinliang
*     Deleted method 'CalculateDeltaAngle'
*     Deleted member 'm_deltaAngleSum'
*     Added member 'm_deltaAngle0'
*     Added member 'm_pastTime'
*     Updated code of method 'Execute'
*     Updated code of method 'ControlMotor'
*     Updated code of method 'CalculateSteeringAngle'
*     Updated member 'm_targetTime'
* July 08, 2008 10:25 Chijinliang
*     Updated code of method 'Execute'
*     Updated code of method 'ControlMotor'
*     Updated code of method 'CalculateSteeringAngle'
*     Updated code of method 'CalculateDeltaAngle'
* July 05, 2008 21:33 Chijinliang
*     Deleted method 'SetParameters'
*     Added method 'Execute'
*     Updated member 'm_motorState'
* July 05, 2008 21:03 Chijinliang
*     Updated code of method 'SetTargetSteeringAngle'
*     Updated code of method 'SetSteeringSpeed'
*     Updated code of method 'SetParameters'
* July 05, 2008 20:28 Chijinliang
*     Deleted method 'UpdateTargetDeltaAngle'
*     Added method 'CalculateSteeringAngle'
*     Added method 'CalculateDeltaAngle'
*     Added member 'm_motorState'
*     Added member 'm_steeringAngle0'
*     Added member 'm_motorSpeed'
*     Added member 'm_deltaAngleSum'
*     Updated code of method 'ZeroRevise'
*     Updated code of method 'Stop'
*     Updated code of method 'SetParameters'
*     Updated code of method 'ControlMotor'
*     Updated code of method 'Control'
*     Updated code of method 'SteeringController'
* July 05, 2008 14:29 Chijinliang
*     Added method 'GetMode'
*     Added method 'GetMinAngle'
*     Added method 'GetMaxAngle'
*     Added method 'SetParameters'
*     Added member 'm_maxAngle'
*     Added member 'm_minAngle'
*     Added member 'm_mode'
*     Updated interface of method 'SetTargetSteeringAngle'
*     Updated code of method 'GetTargetSteeringAngle'
*     Updated interface of method 'SetSteeringSpeed'
*     Updated code of method 'GetSteeringSpeed'
*     Updated member 'm_steeringSpeed'
*     Updated member 'm_targetSteeringAngle'
* June 25, 2008 19:10 Chijinliang
*     Added method 'UpdateTargetDeltaAngle'
*     Added member 'm_targetDeltaAngle'
*     Updated code of method 'SetTargetSteeringAngle'
*     Updated code of method 'SetSteeringSpeed'
*     Updated code of method 'ControlMotor'
*     Updated code of method 'Control'
* June 24, 2008 19:51 Chijinliang
*     Updated code of method 'SteeringController'
* June 24, 2008 19:44 Chijinliang
*     Added method 'Stop'
*     Added member 'm_deltaAngle'
*     Updated code of method 'SetSteeringSpeed'
*     Updated code of method 'Control'
*     Updated code of method 'SteeringController'
* June 23, 2008 17:39 Chijinliang
*     Updated code of method 'SteeringController'
* June 23, 2008 14:03 Chijinliang
*     Updated code of method 'ZeroRevise'
*     Updated code of method 'Control'
*     Updated code of method 'SteeringController'
* June 23, 2008 13:42 Chijinliang
*     Updated code of method 'SetSteeringSpeed'
*     Updated code of method 'ZeroRevise'
*     Updated code of method '~SteeringController'
*     Updated member 'm_zeroIndicator'
* June 23, 2008 10:30 Chijinliang
*     Added method 'DestructorInclude'
*     Added method 'ConstructorInclude'
*     Added method 'SetTargetSteeringAngle'
*     Added method 'GetTargetSteeringAngle'
*     Added method 'SetSteeringSpeed'
*     Added method 'GetSteeringSpeed'
*     Added method 'GetSteeringAngle'
*     Added method 'ZeroRevise'
*     Added method 'ControlMotor'
*     Added method 'Control'
*     Added method '~SteeringController'
*     Added method 'SteeringController'
*     Added member 'm_steeringSpeed'
*     Added member 'm_targetSteeringAngle'
*     Added member 'm_steeringAngle'
*     Added member 'm_steeringMotor'
*     Added member 'm_zeroIndicator'
*
* 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.
*/
SteeringController::SteeringController()
{
    ConstructorInclude();

    m_pastTime = 0;
    m_deltaAngle = 0;
    m_deltaAngle0 = 0;
    m_minAngle = 0;
    m_maxAngle = 0;
    m_mode = TargetAngle;
    m_newCommand = false;
    
    m_motorSpeed = 255;
    m_motorState = 0;
    m_steeringAngle = 0;
    m_steeringAngle0 = 0;
    m_targetTime = 0;
    m_targetSteeringAngle = 0;
    m_pressed = true;
    m_leftside = 0;

    m_steeringMotor.Brake();
    m_steeringMotor.SetSpeed(255);
}


/*
Destructor method.
*/
SteeringController::~SteeringController()
{
    

    DestructorInclude();
}


void SteeringController::CalculateSteeringAngle()
{
    #define ZEROOFFSET 50
    m_steeringAngle0 = m_steeringAngle;
    
    m_deltaAngle  = Calibration::CalAngle(m_pastTime);
    if (m_motorState == 1)
    {
        m_steeringAngle -= m_deltaAngle - m_deltaAngle0;
    }
    else if (m_motorState == 2)
    {
        m_steeringAngle += m_deltaAngle - m_deltaAngle0;
    }
    else
    {
        //cputw(m_pastTime); // 0 ?
    }
    
    m_deltaAngle0 = m_deltaAngle;
    
    if (m_zeroIndicator.Pressed())
    {
        m_pressed = true;
        m_leftside = 0;
        cputw(m_steeringAngle);
        if (m_steeringAngle > ZEROOFFSET)
        {
            m_steeringAngle = ZEROOFFSET;
            m_deltaAngle0 = 0;
            m_deltaAngle = 0;
            m_pastTime = 2;
            UpdateTargetTime();
        }
        else if (m_steeringAngle < -ZEROOFFSET)
        {
            m_steeringAngle = -ZEROOFFSET;
            m_deltaAngle0 = 0;
            m_deltaAngle = 0;
            m_pastTime = 2;
            UpdateTargetTime();
        }
    }
    else
    {
        if (m_pressed)
        {
            if (m_motorState == 1)
            {
                m_leftside = -1;
            }
            else if (m_motorState == 2)
            {
                m_leftside = 1;
            }
            else
            {
                m_leftside = 0;
            }
        }
        
        m_pressed = false;
    }

    if (m_motorState == 1)
    {
        if ((m_leftside == 1) && (m_steeringAngle < -ZEROOFFSET))
        {
            m_steeringAngle = 0;
            m_deltaAngle0 = 0;
            m_deltaAngle = 0;
            m_pastTime = 2;
            UpdateTargetTime();
        }
    }
    else if (m_motorState == 2)
    {
        if ((m_leftside == -1) && (m_steeringAngle > ZEROOFFSET))
        {
            m_steeringAngle = 0;
            m_deltaAngle0 = 0;
            m_deltaAngle = 0;
            m_pastTime = 2;
            UpdateTargetTime();
        }
    }

#undef  ZEROOFFSET
    //cputw(m_steeringAngle);
}


void SteeringController::Control()
{
    CalculateSteeringAngle();
    
    UpdateCommand();
    
    ControlMotor();
}


void SteeringController::ControlMotor()
{
#define ZEROOFFSET 50
    bool finished = false;
    long diffAngle = (long)m_targetSteeringAngle - m_steeringAngle;
    if ((diffAngle >= -ZEROOFFSET) && (diffAngle <= ZEROOFFSET))
    {
        finished = true;
    }
    
    if (finished)
    {
        if (m_mode == Swing)
        {
            // Left
            if (m_motorState == 1)
            {
                m_targetSteeringAngle = m_maxAngle;
                m_motorState = 2;
                m_deltaAngle0 = 0;
                m_deltaAngle = 0;
                m_pastTime = 0;
                m_steeringMotor.Right();
            }
            else if (m_motorState == 2)
            {
                m_targetSteeringAngle = m_minAngle;
                m_motorState = 1;
                m_deltaAngle0 = 0;
                m_deltaAngle = 0;
                m_pastTime = 0;
                m_steeringMotor.Left();
            }
            
            UpdateTargetTime();
            m_pastTime++;
        }
        else
        {
            m_deltaAngle0 = 0;
            m_deltaAngle = 0;
            m_targetTime = 0;
            m_pastTime = 0;
            m_motorState = 0;
            m_steeringMotor.Brake();
        }
    }
    else
    {
        if (m_steeringAngle > m_targetSteeringAngle)
        {
            // Left
            if (m_motorState != 1)
            {
                m_deltaAngle0 = 0;
                m_deltaAngle = 0;
                m_pastTime = 0;
                m_motorState = 1;
                m_steeringMotor.Left();
                
            }
        }
        else
        {
            // Right
            if (m_motorState != 2)
            {
                m_deltaAngle0 = 0;
                m_deltaAngle = 0;
                m_motorState = 2;
                m_pastTime = 0;
                m_steeringMotor.Right();
            }
        }
        
        m_pastTime++;
    }

    if ((m_leftside == -1) && (m_motorState == 1))
    {
        if (m_pastTime >= 200)
        {
            //m_deltaAngle0 = 0;
            //m_deltaAngle = 0;
            //m_motorState = 2;
            //m_pastTime = 0;
            //m_steeringMotor.Right();
            m_steeringMotor.Brake();
        }
    }
    else if ((m_leftside == 1) && (m_motorState == 2))
    {
        if (m_pastTime >= 200)
        {
            //m_deltaAngle0 = 0;
            //m_deltaAngle = 0;
            //m_pastTime = 0;
            //m_motorState = 1;
            //m_steeringMotor.Left();
            m_steeringMotor.Brake();
        }
    }
}


void SteeringController::Execute(const Command* pCommand)
{
/*    m_mode = pCommand->GetSteeringMode();
    m_targetSteeringAngle = pCommand->GetSteeringAngle();
    m_minAngle = pCommand->GetMinAngle();
    m_maxAngle = pCommand->GetMaxAngle();
    
    int deltaAngle = m_targetSteeringAngle - m_steeringAngle;
    if (deltaAngle > 0)
    {
        m_targetTime = Calibration::CalTime(deltaAngle);
    }
    else if (deltaAngle < 0)
    {
        m_targetTime = Calibration::CalTime(-deltaAngle);
    }
*/
    //m_command.Copy(*pCommand);
    m_command = *pCommand;
    m_newCommand = true;
}


void SteeringController::Stop()
{
    m_steeringMotor.Stop();
    m_motorState = 0;
    m_targetTime = 0;
}


void SteeringController::UpdateCommand()
{
    if (!m_newCommand)
    {
        return;
    }
    
    m_newCommand = false;
    m_mode = m_command.GetSteeringMode();
    if (m_mode == AbsTargetAngle)
    {
        m_targetSteeringAngle += m_command.GetSteeringAngle();
    }
    else
    {
        m_targetSteeringAngle = m_command.GetSteeringAngle();
    }
    
    m_minAngle = m_command.GetMinAngle();
    m_maxAngle = m_command.GetMaxAngle();
    
    UpdateTargetTime();
}


void SteeringController::UpdateTargetTime()
{
    int deltaAngle = m_targetSteeringAngle - m_steeringAngle;
    if (deltaAngle > 0)
    {
        m_targetTime = Calibration::CalTime(deltaAngle);
        if (m_motorState == 1)
        {
            //m_targetTime += 5;
        }
    }
    else if (deltaAngle < 0)
    {
        m_targetTime = Calibration::CalTime(-deltaAngle);
        if (m_motorState == 2)
        {
            //m_targetTime += 5;
        }
    }
    else
    {
        m_targetTime = 0;
    }
}


/*
Try to find and set steering angle to zero
*/
void SteeringController::ZeroRevise()
{
    // TODO:: Enter code for method 'ZeroRevise'
}


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


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


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