Dependencies:
SteeringTire
Dependents:
OBROT_ALL
Diff: Steering.cpp
- Revision:
- 2:8be8699c5afd
- Parent:
- 1:4b719f80e9c4
- Child:
- 3:77264fa5d84b
--- a/Steering.cpp Fri Aug 21 04:52:10 2015 +0000
+++ b/Steering.cpp Wed Oct 14 03:55:16 2015 +0000
@@ -1,148 +1,119 @@
#include "Steering.h"
-#include "I2CMotor.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 = 4;
-const float Steering::mAllowableError = 0.08f;
+const int Steering::mNumOfTire = 3;
// 移動中にこれ以上大きな移動方向角度の変化があったらいったん止まる
const float Steering::mStoppingMoveAngle = gPI * 0.5f;
const float Steering::mRollTirePosition[] = {
- 0.25f,
- 0.75f,
- 0.25f,
- 0.75f
+ 0.1432678f,
+ 1.00f,
+ 0.8567322f
};
-Steering::Steering( I2CMotor** t, I2CServo** s, XBee* xbee ) :
-mTire( t ),
-mServo( s ),
-mXBee( xbee ){
- mActionType = Command::STOP;
+Steering::Steering( I2CMotor** t, I2CServo** s ) : mServo( s ){
+ mActionType = STOP;
+ mTire = new SteeringTire( t );
}
Steering::~Steering(){
+ delete mTire;
}
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 );
- }
+ mTire->update( mActionType, command );
switch ( mActionType ){
- case Command::MOVE:
+ case MOVE:
updateMove( command );
break;
- case Command::ROLL:
+ case ROLL:
updateRoll( command );
break;
- case Command::STOP:
+ case STOP:
updateStop( command );
break;
- case Command::WAIT_SERVO:
+ case WAIT_SERVO:
updateWaitServo( command );
break;
-
+
default:
- mActionType = Command::STOP;
+ 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 ){
- I2CMotor::ActionType action = I2CMotor::FORWARD;
-
- // 目標角度>PI ならモータを逆に回すことでプラス角度で対応（角度の調整はここではやらない）
- if ( command.getMoveDirection_rad() > gPI ){
- action = I2CMotor::REVERSE;
- }
+ // モータの角度をサーボを使って調整（0度~180度）
+ setServoPositionByActionType( MOVE, command );
- // モータの角度をサーボを使って調整（0度~180度）
- setServoPositionByActionType( Command::MOVE, command );
-
+ /////////////////////
+ /*
// 現在のサーボの目標角度と実際の角度の差で一番大きな値を求める
- float diff = 0.0f;
+ float maxDiff = 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 );
+ float diff = abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad );
+ if ( diff > maxDiff ){
+ maxDiff = diff;
}
}
// 一定以上の移動方向変化なら一旦タイヤの向きを変えるのを待つために状態遷移する
- if ( diff > mStoppingMoveAngle ){
- mActionType = Command::WAIT_SERVO;
- mNextActionType = Command::MOVE;
+ if ( maxDiff > mStoppingMoveAngle ){
+ mActionType = WAIT_SERVO;
+ mNextActionType = MOVE;
+ return;
}
-
- // タイヤの回転方向（正転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;
+}
+
+void Steering::updateStop( Command command ){
+}
+
+void Steering::updateWaitServo( Command command ){
+ if ( mNextActionType == STOP ){
+ // もし次の状態が停止状態ならサーボの状態に関係なく止める
+ // （もしサーボがI2Cバス上になかったらサーボ停止信号が受信できず止まらなくなるから）
+ mActionType = STOP;
+ return;
}
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 ){
+ // 一つでも目標角度に達していないサーボがあったら次の状態には遷移させない
+ if ( !mServo[ i ]->hasStopped() ){
return;
}
}
@@ -150,28 +121,49 @@
mActionType = mNextActionType;
}
-void Steering::setServoPositionByActionType( Command::ActionType action, Command command ){
+void Steering::setServoPositionByActionType( ActionType action, Command command ){
switch ( action ){
- case Command::MOVE:
+ case MOVE:
setServoPositionWhenMove( command );
break;
- case Command::ROLL:
+ case ROLL:
setServoPositionWhenRoll();
break;
- case Command::STOP:
+ 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;
@@ -196,8 +188,8 @@
}
void Steering::setServoPositionWhenStop(){
- // 現在の位置が目標位置であるとする
+ // サーボを止める
for ( int i = 0; i < mNumOfTire; ++i ){
- mServo[ i ]->setTargetPosition( mServo[ i ]->getPosition() );
+ //mServo[ i ]->stop();
}
}
tarou yamada