calibration.c

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

/******************************************************************************\
*
* File:          Calibration.C
* Creation date: June 23, 2008 10:30
* Latest update: July 09, 2008 16:39
* Author:        ClassBuilder
*                XXXX
* Purpose:       Method implementations of class 'Calibration'
* CB version:    ClassBuilder Version 2.9 (PR523)
*
* Modifications: @INSERT_MODIFICATIONS(* )
* July 09, 2008 16:35 Chijinliang
*     Added method 'GetWhiteGray'
*     Added method 'GetBlackGray'
*     Updated interface of method 'SetWhite'
*     Updated code of method 'GetWhite'
*     Updated interface of method 'SetGray'
*     Updated code of method 'GetGray'
*     Updated interface of method 'SetBlack'
*     Updated code of method 'GetBlack'
*     Updated member 'm_gray'
*     Updated member 'm_black'
*     Updated member 'm_white'
* July 09, 2008 15:48 Chijinliang
*     Deleted member 'm_newCommand'
*     Updated code of method 'Calibration'
* July 09, 2008 15:46 Chijinliang
*     Added member 'm_newCommand'
* July 09, 2008 15:22 Chijinliang
*     Updated code of method 'CalTime'
* July 09, 2008 12:13 Chijinliang
*     Deleted method 'CalculateMotorSpeed'
*     Deleted member 'm_steeringAngleGain'
*     Added method 'CalTime'
*     Updated interface of method 'CalAngle'
*     Updated member 'm_steeringAngles'
*     Updated member 'm_times'
* July 05, 2008 14:29 Chijinliang
*     Added method 'SetSteeringAngleGain'
*     Added method 'GetSteeringAngleGain'
*     Added member 'm_steeringAngleGain'
*     Updated code of method 'DestructorInclude'
*     Updated code of method 'ConstructorInclude'
*     Updated interface of method 'SetThresholdWhite'
*     Updated code of method 'GetThresholdWhite'
*     Updated interface of method 'SetThresholdGray'
*     Updated code of method 'GetThresholdGray'
*     Updated interface of method 'SetThresholdBlack'
*     Updated code of method 'GetThresholdBlack'
*     Updated interface of method 'CalculateSteeringSpeed'
*     Updated interface of method 'CalculateMotorSpeed'
*     Updated code of method '~Calibration'
*     Updated code of method 'Calibration'
*     Updated member 'm_steeringSpeeds'
*     Updated member 'm_motorSpeeds'
*     Updated member 'm_thresholdGray'
*     Updated member 'm_thresholdBlack'
*     Updated member 'm_thresholdWhite'
* June 24, 2008 19:51 Chijinliang
*     Updated code of method 'Calibration'
* June 24, 2008 19:44 Chijinliang
*     Updated code of method 'CalculateSteeringSpeed'
*     Updated member 'm_steeringSpeeds'
* June 23, 2008 17:39 Chijinliang
*     Updated interface of method 'SetThresholdBlack'
*     Updated code of method 'GetThresholdBlack'
*     Updated code of method 'CalculateSteeringSpeed'
*     Updated code of method 'CalculateMotorSpeed'
*     Updated code of method 'Calibration'
*     Updated member 'm_steeringSpeeds'
*     Updated member 'm_motorSpeeds'
*     Updated member 'm_thresholdBlack'
* June 23, 2008 14:03 Chijinliang
*     Updated code of method 'CalculateSteeringSpeed'
*     Updated code of method 'CalculateMotorSpeed'
* June 23, 2008 13:46 Chijinliang
*     Added method 'Calibration'
*     Updated interface of method 'SetThresholdWhite'
*     Updated code of method 'GetThresholdWhite'
*     Updated interface of method 'SetThresholdGray'
*     Updated code of method 'GetThresholdGray'
*     Updated interface of method 'SetThresholdBlack'
*     Updated code of method 'GetThresholdBlack'
*     Updated member 'm_thresholdGray'
*     Updated member 'm_thresholdBlack'
*     Updated member 'm_thresholdWhite'
* June 23, 2008 10:30 Chijinliang
*     Added method 'DestructorInclude'
*     Added method 'ConstructorInclude'
*     Added method 'SetThresholdWhite'
*     Added method 'GetThresholdWhite'
*     Added method 'SetThresholdGray'
*     Added method 'GetThresholdGray'
*     Added method 'SetThresholdBlack'
*     Added method 'GetThresholdBlack'
*     Added method 'CalculateSteeringSpeed'
*     Added method 'CalculateMotorSpeed'
*     Added method '~Calibration'
*     Added member 'm_steeringSpeeds'
*     Added member 'm_motorSpeeds'
*     Added member 'm_thresholdGray'
*     Added member 'm_thresholdBlack'
*     Added member 'm_thresholdWhite'
*     Updated return type of method 'CalculateMotorSpeed'
*
* 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;


// Static members
byte Calibration::m_times[MAX_MAPITEM] = {2, 3, 4, 5, 6, 7, 12, 22, 52, 102, 202, 255};
int Calibration::m_steeringAngles[MAX_MAPITEM] = {0, 41, 134, 206, 275, 356, 768, 1665, 4500, 9374, 19196, 24631}/*LSB=0.003deg*/;


/*
Constructor method.
*/
Calibration::Calibration()
{
    ConstructorInclude();

    m_white = 50;
    m_gray = 43;
    m_black = 39;
}


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

    DestructorInclude();
}


int Calibration::CalAngle(byte pastTime)
{
    byte i           = 0;
    byte x_plus      = 0;
    byte x_minus     = 0;
    int steeringAngle  = 0;

    for( i = 1; i < MAX_MAPITEM; i++){
        if(pastTime <= *(m_times + i)){
            x_plus     = i;
            x_minus = i - 1;
            break;
        }
    }

    if(x_plus > 0){
        if(*(m_steeringAngles + x_plus) == *(m_steeringAngles + x_minus)){
            steeringAngle = *(m_steeringAngles + x_minus);
            
        }
        else if( *(m_steeringAngles + x_plus) > *(m_steeringAngles + x_minus)){
            steeringAngle = (((long)(*(m_steeringAngles + x_plus) - *(m_steeringAngles + x_minus)) * (pastTime - *(m_times + x_minus)))
                            + (long)*(m_steeringAngles + x_minus) * (*(m_times + x_plus) - *(m_times + x_minus)))
                            /(*(m_times + x_plus) - *(m_times + x_minus)) ;

        }
        else{
            steeringAngle = (((long)*(m_steeringAngles + x_minus) * (*(m_times + x_plus) - *(m_times + x_minus)))
                            - ((long)(*(m_steeringAngles + x_minus) - *(m_steeringAngles + x_plus)) * (pastTime - *(m_times + x_minus))))
                            /(*(m_times + x_plus) - *(m_times + x_minus));

        }
    }
    else if(x_plus == 0){
        steeringAngle = *(m_steeringAngles + MAX_MAPITEM - 1);
    }
    
    return(steeringAngle);
}


byte Calibration::CalTime(int steeringAngle)
{
    byte i           = 0;
    byte x_plus      = 0;
    byte x_minus     = 0;
    byte pastTime  = 0;

    for( i = 1; i < MAX_MAPITEM; i++){
        if(steeringAngle <= *(m_steeringAngles + i)){
            x_plus     = i;
            x_minus = i - 1;
            break;
        }
    }

    if(x_plus > 0){
        if(*(m_times + x_plus) == *(m_times + x_minus)){
            pastTime = *(m_times + x_minus);
            
        }
        else if( *(m_times + x_plus) > *(m_times + x_minus)){
            pastTime = (((long)(*(m_times + x_plus) - *(m_times + x_minus)) * (steeringAngle - *(m_steeringAngles + x_minus)))
                            + (long)*(m_times + x_minus) * (*(m_steeringAngles + x_plus) - *(m_steeringAngles + x_minus)))
                            /(*(m_steeringAngles + x_plus) - *(m_steeringAngles + x_minus)) ;

        }
        else{
            pastTime = (((long)*(m_times + x_minus) * (*(m_steeringAngles + x_plus) - *(m_steeringAngles + x_minus)))
                            - ((long)(*(m_times + x_minus) - *(m_times + x_plus)) * (steeringAngle - *(m_steeringAngles + x_minus))))
                            /(*(m_steeringAngles + x_plus) - *(m_steeringAngles + x_minus));

        }
    }
    else if(x_plus == 0){
        pastTime = *(m_times + MAX_MAPITEM - 1);
    }
    
    return(pastTime);
}


unsigned int Calibration::GetBlackGray() const
{
    return m_gray + 2;
}


unsigned int Calibration::GetWhiteGray() const
{
    return m_white - 2;
}


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


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


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