File content as of revision 0:0bac1fed0273:

#include "Steering.h"
#include "I2CMotor.h"
#include "I2CServo.h"
#include "Vector2.h"
#include "Math.h"
#include "Command.h"

// 
// 0    3
// 
// 1    2
const int Steering::mNumOfTire = 4;
const float Steering::mAllowableError = 0.08f;
// 移動中にこれ以上大きな移動方向角度の変化があったらいったん止まる
const float Steering::mStoppingMoveAngle = gPI * 0.5f;
const float Steering::mRollTirePosition[] = {
    0.25f,
    0.75f,
    0.25f,
    0.75f
};

Steering::Steering( I2CMotor** t, I2CServo** s, XBee* xbee ) : 
mTire( t ),
mServo( s ),
mXBee( xbee ){
    mActionType = Command::STOP;
}

Steering::~Steering(){
}

void Steering::update( Command command ){
    // もし現在と違う動作が求められたらサーボの向きを調整するためのシーケンス遷移をする
    if ( command.getActionType() != mActionType ){
        mActionType     = Command::WAIT_SERVO;
        mNextActionType = command.getActionType();
        setServoPositionByActionType( mNextActionType, command );
    }
    
    switch ( mActionType ){
        case Command::MOVE:
            updateMove( command );
            break;
            
        case Command::ROLL:
            updateRoll( command );
            break;
            
        case Command::STOP:
            updateStop( command );
            break;
            
        case Command::WAIT_SERVO:
            updateWaitServo( command );
            break;
            
        default:
            mActionType = Command::STOP;
            updateStop( command );
            break;
    }
}

void Steering::updateMove( Command command ){
    I2CMotor::ActionType action = I2CMotor::FORWARD;
    
    // 目標角度>PI ならモータを逆に回すことでプラス角度で対応（角度の調整はここではやらない）
    if ( command.getMoveDirection_rad() > gPI ){
        action = I2CMotor::REVERSE;
    }
    
    // モータの角度をサーボを使って調整（0度~180度）
    setServoPositionByActionType( Command::MOVE, command );
    
    // 現在のサーボの目標角度と実際の角度の差で一番大きな値を求める
    float diff = 0.0f;
    for ( int i = 0; i < mNumOfTire; ++i ){
        if ( abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad ) > diff ){
            diff = abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad );
        }
    }
    
    // 一定以上の移動方向変化なら一旦タイヤの向きを変えるのを待つために状態遷移する
    if ( diff > mStoppingMoveAngle ){
        mActionType     = Command::WAIT_SERVO;
        mNextActionType = Command::MOVE;
    }
    
    // タイヤの回転方向（正転or逆転）と速さ（duty）を更新
    for ( int i = 0; i < mNumOfTire; ++i ){
        mTire[ i ]->setActionType( action );
        mTire[ i ]->setDuty( command.getMoveDuty() );
    }
}

void Steering::updateAbsRoll( Command command ){
    // todo : implement
}

void Steering::updateRoll( Command command ){
    float coeff = command.getRollCoeff();
    I2CMotor::ActionType action[] = { I2CMotor::FORWARD, I2CMotor::REVERSE };
    
    if ( coeff < 0.0f ){
        // 回転係数がマイナスなら逆回転
        I2CMotor::ActionType temp = action[ 0 ];
        action[ 0 ] = action[ 1 ];
        action[ 1 ] = temp;
        
        coeff *= -1.0f;
    }
    
    for ( int i = 0; i < mNumOfTire; ++i ){
        // 左半分と右半分のモータは逆に駆動する
        mTire[ i ]->setActionType( action[ i / 2 ] );
        mTire[ i ]->setDuty( coeff );
    }
}

void Steering::updateStop( Command command ){
    for ( int i = 0; i < mNumOfTire; ++i ){
        mTire[ i ]->setActionType( I2CMotor::BRAKE );
    }
}

void Steering::updateWaitServo( Command command ){
    if ( command.getActionType() != mNextActionType ){
        mNextActionType = command.getActionType();
        setServoPositionByActionType( mNextActionType, command );
    }
    
    for ( int i = 0; i < mNumOfTire; ++i ){
        // 一つでも目標角度に達していないサーボがあったら抜ける
        if ( mServo[ i ]->isStop() == false ){
            return;
        }
    }
    
    mActionType = mNextActionType;
}

void Steering::setServoPositionByActionType( Command::ActionType action, Command command ){
    switch ( action ){
        case Command::MOVE:{
            float angle = command.getMoveDirection_rad();
            
            // 目標角度>PI ならモータを逆に回すことで180度引いた角度で対応
            if ( angle > gPI ){
                angle -= gPI;
            }
                    
            // 角度（0~pi）からサーボの位置データ（0.0~1.0）に変換
            float pos = convertRange( angle, 0.0f, gPI, 0.0f, 1.0f );
            
            // タイヤの向き（0.0~1.0）を更新
            for ( int i = 0; i < mNumOfTire; ++i ){
                mServo[ i ]->setTargetPosition( pos );
            }
            
            mMoveDirection_rad = angle;
            
            break;
        }
        
        case Command::ROLL:
            // タイヤの向き（0.0~1.0）を更新
            for ( int i = 0; i < mNumOfTire; ++i ){
                mServo[ i ]->setTargetPosition( mRollTirePosition[ i ] );
            }
            break;
            
        default:
            for ( int i = 0; i < mNumOfTire; ++i ){
                mServo[ i ]->setTargetPosition( mServo[ i ]->getPosition() );
            }
            return;
    }
}