File content as of revision 4:912ac0c2c52a:

#include "Steering.h"
#include "SteeringTire.h"
#include "I2CServo.h"
#include "Math.h"
#include "Command.h"
#include "ShootingSystem.h"
#include "Steering.h"

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

Steering::Steering( I2CMotor** t, I2CServo** s ) : mServo( s ){
    mActionType = STOP;
    mTire = new SteeringTire( t );
}

Steering::~Steering(){
    delete mTire;
    delete mServo;
}

void Steering::update( Command command ){
    mTire->update( mActionType, command );
    
    switch ( mActionType ){
        case MOVE:
            updateMove( command );
            break;
            
        case ROLL:
            updateRoll( command );
            break;
            
        case STOP:
            updateStop( command );
            break;
            
        case WAIT_SERVO:
            updateWaitServo( command );
            break;
        
        default:
            mActionType = STOP;
            updateStop( command );
            break;
    }
    
    updateActionType( command );
}

void Steering::updateActionType( Command command ){
    if ( command.getSteeringActionType() != mActionType ){
        if ( command.getSteeringActionType() == STOP ){
            mActionType = STOP;
            setServoPositionByActionType( mNextActionType, command );
            return;
        }
        
        // 現在と違う動作が求められたらサーボの向きを調整するためのシーケンス遷移をする
        mActionType     = WAIT_SERVO;
        mNextActionType = command.getSteeringActionType();
        setServoPositionByActionType( mNextActionType, command );
    }
}

void Steering::updateMove( Command command ){
    // モータの角度をサーボを使って調整（0度~180度）
    setServoPositionByActionType( MOVE, command );
    
    /////////////////////
    /*
    // 現在のサーボの目標角度と実際の角度の差で一番大きな値を求める
    float maxDiff = 0.0f;
    for ( int i = 0; i < mNumOfTire; ++i ){
        float diff = abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad );
        if ( diff > maxDiff ){
            maxDiff = diff;
        }
    }
    
    // 一定以上の移動方向変化なら一旦タイヤの向きを変えるのを待つために状態遷移する
    if ( maxDiff > mStoppingMoveAngle ){
        mActionType     = WAIT_SERVO;
        mNextActionType = MOVE;
        return;
    }
    ////////////////
    */
}

void Steering::updateRoll( Command command ){
}

void Steering::updateStop( Command command ){
}

void Steering::updateWaitServo( Command command ){
    if ( mNextActionType == STOP ){
        // もし次の状態が停止状態ならサーボの状態に関係なく止める
        // （もしサーボがI2Cバス上になかったらサーボ停止信号が受信できず止まらなくなるから）
        mActionType = STOP;
        return;
    }
    
    for ( int i = 0; i < mNumOfTire; ++i ){
        // 一つでも目標角度に達していないサーボがあったら次の状態には遷移させない
        if ( !mServo[ i ]->hasStopped() ){
            return;
        }
    }
    
    mActionType = mNextActionType;
}

void Steering::setServoPositionByActionType( ActionType action, Command command ){
    switch ( action ){
        case MOVE:
            setServoPositionWhenMove( command );
            break;
            
        case ROLL:
            setServoPositionWhenRoll();
            break;
            
        case STOP:
            setServoPositionWhenStop();
            break;
            
        case WAIT_SERVO:
            break;
            
        default:
            setServoPositionWhenStop();
            return;
    }
}

void adjust( float* angle_deg ){
    float x = cos( *angle_deg );
    float y = sin( *angle_deg );
    float adjustY = 1.0f;
    float p = 0.10f;
    
    y = p * adjustY + ( 1.0f - p ) * y;
    
    *angle_deg = atan2( y, x );
    
    if ( *angle_deg < 0.0f ){
        *angle_deg += gPI * 2.0f;
    }
}

void Steering::setServoPositionWhenMove( Command command ){
    float angle = command.getMoveDirection_rad();
    
    adjust( &angle );
    
    // 目標角度>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;
}

void Steering::setServoPositionWhenRoll(){
    // タイヤの向き（0.0~1.0）を更新
    for ( int i = 0; i < mNumOfTire; ++i ){
        mServo[ i ]->setTargetPosition( mRollTirePosition[ i ] );
    }
}

void Steering::setServoPositionWhenStop(){
    // サーボを止める
    for ( int i = 0; i < mNumOfTire; ++i ){
        //mServo[ i ]->stop(); 
    }
}