Steering.cpp
- Committer:
- inst
- Date:
- 2015-08-21
- Revision:
- 1:4b719f80e9c4
- Parent:
- 0:0bac1fed0273
- Child:
- 2:8be8699c5afd
File content as of revision 1:4b719f80e9c4:
#include "Steering.h" #include "I2CMotor.h" #include "I2CServo.h" #include "Math.h" #include "Command.h" // Tire Number // 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 ){ Command::ActionType now = mActionType; if ( mActionType == Command::WAIT_SERVO ){ // もう既にサーボの動作完了待ちならサーボ完了後に移行予定の動作と比較する now = mNextActionType; } if ( command.getActionType() != now ){ // 現在と違う動作が求められたらサーボの向きを調整するためのシーケンス遷移をする 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 } // Tire Number // 0 3 // // 1 2 void Steering::updateRoll( Command command ){ float coeff = command.getRollCoeff(); // デフォルトでは0,1が正転で2,3が逆転1(時計回り) 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: setServoPositionWhenMove( command ); break; case Command::ROLL: setServoPositionWhenRoll(); break; case Command::STOP: setServoPositionWhenStop(); break; default: setServoPositionWhenStop(); return; } } void Steering::setServoPositionWhenMove( Command command ){ 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; } 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 ]->setTargetPosition( mServo[ i ]->getPosition() ); } }