Diff: Steering.cpp
- Revision:
- 1:4b719f80e9c4
- Parent:
- 0:0bac1fed0273
- Child:
- 2:8be8699c5afd
--- a/Steering.cpp Sat Aug 15 13:31:39 2015 +0000 +++ b/Steering.cpp Fri Aug 21 04:52:10 2015 +0000 @@ -1,11 +1,10 @@ #include "Steering.h" #include "I2CMotor.h" #include "I2CServo.h" -#include "Vector2.h" #include "Math.h" #include "Command.h" -// +// Tire Number // 0 3 // // 1 2 @@ -31,8 +30,13 @@ } void Steering::update( Command command ){ - // もし現在と違う動作が求められたらサーボの向きを調整するためのシーケンス遷移をする - if ( command.getActionType() != mActionType ){ + 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 ); @@ -98,12 +102,17 @@ // 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; @@ -119,6 +128,7 @@ } void Steering::updateStop( Command command ){ + // 全駆動用モータがブレーキ for ( int i = 0; i < mNumOfTire; ++i ){ mTire[ i ]->setActionType( I2CMotor::BRAKE ); } @@ -142,38 +152,52 @@ 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 ); + case Command::MOVE: + setServoPositionWhenMove( command ); + break; - // タイヤの向き(0.0~1.0)を更新 - for ( int i = 0; i < mNumOfTire; ++i ){ - mServo[ i ]->setTargetPosition( pos ); - } - - mMoveDirection_rad = angle; - + case Command::ROLL: + setServoPositionWhenRoll(); break; - } - - case Command::ROLL: - // タイヤの向き(0.0~1.0)を更新 - for ( int i = 0; i < mNumOfTire; ++i ){ - mServo[ i ]->setTargetPosition( mRollTirePosition[ i ] ); - } + + case Command::STOP: + setServoPositionWhenStop(); break; default: - for ( int i = 0; i < mNumOfTire; ++i ){ - mServo[ i ]->setTargetPosition( mServo[ i ]->getPosition() ); - } + 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() ); + } +}