File content as of revision 3:56ad82f5f780:

#include "Command.h"
#include "Math.h"
#include "Shooter.h"
#include "Steering.h"
#include "XBee.h"

// angle to analog
const float Command::mAngleToAnalogCoeffA = -0.016122;
const float Command::mAngleToAnalogCoeffB =  0.800000;

const float Command::mShootingAngleTable[] = {
    22.0f - 6.0f,   // OWN_POLE
    36.0f - 6.0f,   // CENTER_MIDDLE_POLE
    30.0f - 6.0f,   // CENTER_SIDE_POLE
    16.0f - 6.0f    // ENEMYS_POLE
};
const float Command::mMaxAngle_deg = 46.0f;
const float Command::mMinAngle_deg =  2.0f;

Command::Command(){
    mSteeringActionType = Steering::STOP;
    mMoveDirection_rad  = gPI * 0.5f;
    mMoveDuty           = 0.5f;
    mRollCoeff          = 0.0f;
    mShootingAngle_deg  = 0.0f;
    mAngleAdjust_deg    = 0.0f;
    mShooterPosition    = 0.0f;
    mIsShooting         = false;
    mIsSupplying        = false;
    
    setAimStateAndAimParam( ShootingSystem::AIM_OWN_POLE );
}

Command::Command( Steering::ActionType steeringAction,
                  ShootingSystem::AimState aimState,
                  float moveDirection_rad,  float angleAdjust_deg,
                  float moveDuty, float roll, bool isSupplying ){
    
    mSteeringActionType     = steeringAction;
    mMoveDirection_rad      = moveDirection_rad;
    mShootingAngle_deg      = 0.0f;
    mAngleAdjust_deg        = angleAdjust_deg;
    mMoveDuty               = moveDuty;
    mRollCoeff              = roll;
    mIsShooting             = ( mSteeringActionType == Steering::SHOOT );
    mIsSupplying            = isSupplying;

    setAimStateAndAimParam( aimState );
}

void Command::setAimStateAndAimParam( ShootingSystem::AimState aimState ){
    mAimState  = aimState;
    
    switch ( mAimState ){
        case ShootingSystem::AIM_OWN_POLE:
            mShootingAngle_deg  = mShootingAngleTable[ OWN_POLE_ID ] + mAngleAdjust_deg;
            mShootVoltage       = Shooter::WORKING_8V;
            mShooterPosition    = 0.780392f;
            break;
        
        case ShootingSystem::AIM_CENTER_MIDDLE_POLE:
            mShootingAngle_deg  = mShootingAngleTable[ CENTER_MIDDLE_POLE_ID ] + mAngleAdjust_deg;
            mShootVoltage       = Shooter::WORKING_12V;
            mShooterPosition    = 0.305882f;
            break;
        
        case ShootingSystem::AIM_CENTER_SIDE_POLE:
            mShootingAngle_deg  = mShootingAngleTable[ CENTER_SIDE_POLE_ID ] + mAngleAdjust_deg;
            mShootVoltage       = Shooter::WORKING_12V;
            mShooterPosition    = 0.780392f;
            break;
            
        case ShootingSystem::AIM_ENEMYS_POLE:
            mShootingAngle_deg  = mShootingAngleTable[ ENEMYS_POLE_ID ] + mAngleAdjust_deg;
            mShootVoltage       = Shooter::WORKING_12V;
            mShooterPosition    = 0.305882f;
            break;
            
        default:
            break;
    }
    
    if ( mShootingAngle_deg > mMaxAngle_deg ){
        mShootingAngle_deg = mMaxAngle_deg;
        XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::INCREMENT ] );
    }
    if ( mShootingAngle_deg < mMinAngle_deg ){
        mShootingAngle_deg = mMinAngle_deg;
        XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::DECREMENT ] );
    }
}