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() );
+    }
+}