File content as of revision 3:115488d3ca1a:

#include "mbed.h"
#include "I2CDevice.h"
#include "XBee.h"
#include "Math.h"
#include "Steering.h"
#include "Command.h"
#include "ShootingSystem.h"

const size_t XBee::mNumOfDivDirection = 8;
const float XBee::mDutyTable[] = {
    0.05f, 0.90f
};
// 右回りがプラスで左回りがマイナス
const float XBee::mRollCoeffTable[] = {
    0.05f, 0.9f
};
const float XBee::mAngleAdjustTable[] = {
     0.0f,
     2.0f,
    -2.0f
};
float XBee::mAngleAdjust_deg    = 0.0f;
float XBee::mMoveDirection_rad  = 0.5f * gPI;

XBee::XBee( char address ) : I2CDevice( address ){
    mMoveDirection_rad      = 0;
    mMoveDuty               = 1.0f;
    mRollCoeff              = 0;
    mSteeringActionType     = Steering::STOP;
    mAimState               = ShootingSystem::AIM_OWN_POLE;
    mIsNotStop              = false;
    mSpeedMode              = HIGH_SPEED;
    mPrevAngleAdjustType    = NO_OPERATION;
}

int XBee::read(){
    char buf[ 4 ] = { 0 };
    
    // buf[ 0 ] : ActionType
    // buf[ 1 ] : AngleAdjust, MoveDirection, Speed
    // buf[ 2 ] : AimState
    // buf[ 3 ] : IsSupply
    int val = I2CDevice::read( buf, 4 );
    
    Steering::ActionType action = mSteeringActionType;
    mSteeringActionType = static_cast< Steering::ActionType >( buf[ 0 ] );
    
    ShootingSystem::AimState newAimState = static_cast< ShootingSystem::AimState >( buf[ 2 ] );
    if ( newAimState != mAimState ){
        mAngleAdjust_deg = 0.0f;
    }
    mAimState       = newAimState;
    
    mIsShooting     = false;
    mIsSupplying    = buf[ 3 ];
    
    if ( mSteeringActionType != Steering::STOP ){
        mPrevAngleAdjustType = NO_OPERATION;
    }
    
    switch ( mSteeringActionType ){
        case Steering::STOP:
            if ( ( buf[ 1 ] == LOW_SPEED ) || ( buf[ 1 ] == HIGH_SPEED )){
                mSpeedMode = static_cast< SpeedType >( buf[ 1 ] );
                mIsNotStop = true;
                mPrevAngleAdjustType = NO_OPERATION;
            } else if ( buf[ 1 ] == NO_OPERATION ){
                mIsNotStop = false;
                mPrevAngleAdjustType = NO_OPERATION;
            } else if ( mPrevAngleAdjustType != static_cast< AngleAdjustType >( buf[ 1 ] ) ){
                mAngleAdjust_deg += mAngleAdjustTable[ buf[ 1 ] ];
                mPrevAngleAdjustType = static_cast< AngleAdjustType >( buf[ 1 ] );
            }
            break;
            
        case Steering::MOVE:
            if ( mSpeedMode == LOW_SPEED ){
                mMoveDuty = mDutyTable[ 0 ];
            } else if ( mSpeedMode == HIGH_SPEED ){
                mMoveDuty = mDutyTable[ 1 ];
            }
            
            mMoveDirection_rad  = 2.0f * gPI / mNumOfDivDirection * buf[ 1 ];
            break;
            
        case Steering::ROLL:
            if ( mSpeedMode == LOW_SPEED ){
                mRollCoeff = mRollCoeffTable[ 0 ];
            } else if ( mSpeedMode == HIGH_SPEED ){
                mRollCoeff = mRollCoeffTable[ 1 ];
            }
            
            if ( buf[ 1 ] == CONTERCLOCKWISE ){
                mRollCoeff *= -1.0f;
            }
            break;
            
        case Steering::SHOOT:
            mIsShooting = true;
            break;
            
        default:
            mSteeringActionType = Steering::STOP;
            break;
    }
    
    if ( mIsNotStop ){
        mSteeringActionType = action;
    }
    
    return val;
}

Command XBee::createCommand(){
    return Command( mSteeringActionType, mAimState,
                    mMoveDirection_rad, mAngleAdjust_deg,
                    mMoveDuty, mRollCoeff, mIsSupplying );
}