Command.cpp

00001 #include "Command.h"
00002 #include "Math.h"
00003 #include "Shooter.h"
00004 #include "Steering.h"
00005 #include "XBee.h"
00006 
00007 // angle to analog
00008 const float Command::mAngleToAnalogCoeffA = -0.016122;
00009 const float Command::mAngleToAnalogCoeffB =  0.800000;
00010 
00011 const float Command::mShootingAngleTable[] = {
00012     22.0f - 6.0f,   // OWN_POLE
00013     36.0f - 6.0f,   // CENTER_MIDDLE_POLE
00014     30.0f - 6.0f,   // CENTER_SIDE_POLE
00015     16.0f - 6.0f    // ENEMYS_POLE
00016 };
00017 const float Command::mMaxAngle_deg = 46.0f;
00018 const float Command::mMinAngle_deg =  2.0f;
00019 
00020 Command::Command(){
00021     mSteeringActionType = Steering::STOP;
00022     mMoveDirection_rad  = gPI * 0.5f;
00023     mMoveDuty           = 0.5f;
00024     mRollCoeff          = 0.0f;
00025     mShootingAngle_deg  = 0.0f;
00026     mAngleAdjust_deg    = 0.0f;
00027     mShooterPosition    = 0.0f;
00028     mIsShooting         = false;
00029     mIsSupplying        = false;
00030     
00031     setAimStateAndAimParam( ShootingSystem::AIM_OWN_POLE );
00032 }
00033 
00034 Command::Command( Steering::ActionType steeringAction,
00035                   ShootingSystem::AimState aimState,
00036                   float moveDirection_rad,  float angleAdjust_deg,
00037                   float moveDuty, float roll, bool isSupplying ){
00038     
00039     mSteeringActionType     = steeringAction;
00040     mMoveDirection_rad      = moveDirection_rad;
00041     mShootingAngle_deg      = 0.0f;
00042     mAngleAdjust_deg        = angleAdjust_deg;
00043     mMoveDuty               = moveDuty;
00044     mRollCoeff              = roll;
00045     mIsShooting             = ( mSteeringActionType == Steering::SHOOT );
00046     mIsSupplying            = isSupplying;
00047 
00048     setAimStateAndAimParam( aimState );
00049 }
00050 
00051 void Command::setAimStateAndAimParam( ShootingSystem::AimState aimState ){
00052     mAimState  = aimState;
00053     
00054     switch ( mAimState ){
00055         case ShootingSystem::AIM_OWN_POLE:
00056             mShootingAngle_deg  = mShootingAngleTable[ OWN_POLE_ID ] + mAngleAdjust_deg;
00057             mShootVoltage       = Shooter::WORKING_8V;
00058             mShooterPosition    = 0.780392f;
00059             break;
00060         
00061         case ShootingSystem::AIM_CENTER_MIDDLE_POLE:
00062             mShootingAngle_deg  = mShootingAngleTable[ CENTER_MIDDLE_POLE_ID ] + mAngleAdjust_deg;
00063             mShootVoltage       = Shooter::WORKING_12V;
00064             mShooterPosition    = 0.305882f;
00065             break;
00066         
00067         case ShootingSystem::AIM_CENTER_SIDE_POLE:
00068             mShootingAngle_deg  = mShootingAngleTable[ CENTER_SIDE_POLE_ID ] + mAngleAdjust_deg;
00069             mShootVoltage       = Shooter::WORKING_12V;
00070             mShooterPosition    = 0.780392f;
00071             break;
00072             
00073         case ShootingSystem::AIM_ENEMYS_POLE:
00074             mShootingAngle_deg  = mShootingAngleTable[ ENEMYS_POLE_ID ] + mAngleAdjust_deg;
00075             mShootVoltage       = Shooter::WORKING_12V;
00076             mShooterPosition    = 0.305882f;
00077             break;
00078             
00079         default:
00080             break;
00081     }
00082     
00083     if ( mShootingAngle_deg > mMaxAngle_deg ){
00084         mShootingAngle_deg = mMaxAngle_deg;
00085         XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::INCREMENT ] );
00086     }
00087     if ( mShootingAngle_deg < mMinAngle_deg ){
00088         mShootingAngle_deg = mMinAngle_deg;
00089         XBee::setAngleAdjust_deg( mAngleAdjust_deg - XBee::mAngleAdjustTable[ XBee::DECREMENT ] );
00090     }
00091 }