Diff: SteeringTire.cpp

Revision:

0:0cd6b505ba45

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SteeringTire.cpp	Thu Oct 15 08:46:09 2015 +0000
@@ -0,0 +1,127 @@
+#include "SteeringTire.h"
+#include "Command.h"
+#include "I2CMotor.h"
+#include "Math.h"
+#include "Steering.h"
+#include "mbed.h"
+
+const uint32_t SteeringTire::mReleaseStopTime_ms[] = {
+    //10,
+    100,
+    300
+};
+const float SteeringTire::mBrakeReleaseThreshold = 0.50f;
+
+SteeringTire::SteeringTire( I2CMotor** tire ) : mTire( tire ){
+    mIsReleaseStop  = false;
+    mTimer          = new Timer;
+    mPrevActionType = Steering::STOP;
+}
+
+SteeringTire::~SteeringTire(){
+    delete mTire;
+}
+
+void SteeringTire::update( Steering::ActionType action, Command command ){
+    bool isReleaseStop = ( mPrevActionType == Steering::MOVE );
+    isReleaseStop &= ( action == Steering::STOP );
+    
+    if ( isReleaseStop ){
+        mIsReleaseStop = true;
+        mTimer->reset();
+        mTimer->start();
+    }
+    
+    switch ( action ){
+        case Steering::STOP:
+            updateStop( command );
+            break;
+            
+        case Steering::MOVE:
+            updateMove( command );
+            break;
+            
+        case Steering::ROLL:
+            updateRoll( command );
+            break;
+            
+        case Steering::WAIT_SERVO:
+            updateWaitServo( command );
+            break;
+    }
+    
+    if ( action != Steering::WAIT_SERVO ){
+        mPrevActionType = action;
+    }
+}
+
+void SteeringTire::updateMove( Command command ){
+    I2CMotor::ActionType action = I2CMotor::FORWARD;
+    
+    // 目標角度>PI ならモータを逆に回す0-pi[rad]で対応
+    if ( command.getMoveDirection_rad() > gPI ){
+        action = I2CMotor::REVERSE;
+    }
+    // タイヤの回転方向（正転or逆転）と速さ（duty）を更新
+    for ( int i = 0; i < Steering::mNumOfTire; ++i ){
+        mTire[ i ]->setActionType( action );
+        mTire[ i ]->setPercent( command.getMoveDuty() );
+    }
+    
+    if ( isNear( command.getMoveDirection_rad(), 0.0f, 0.01f ) ||
+         isNear( command.getMoveDirection_rad(),  gPI, 0.01f ) ||
+         isNear( command.getMoveDirection_rad(), -gPI, 0.01f ) ){
+        mTire[ 0 ]->setPercent( command.getMoveDuty() * 0.30f );
+        mTire[ 2 ]->setPercent( command.getMoveDuty() * 0.30f );
+    }
+}
+
+void SteeringTire::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 = abs( coeff );
+    }
+    
+    for ( int i = 0; i < Steering::mNumOfTire; ++i ){
+        // 左半分と右半分のモータは逆に駆動する
+        mTire[ i ]->setActionType( action[ i / 2 ] );
+        mTire[ i ]->setPercent( coeff );
+    }
+}
+
+void SteeringTire::updateStop( Command command ){
+    I2CMotor::ActionType action = I2CMotor::BRAKE;
+    
+    if ( mIsReleaseStop ){
+        float t = mReleaseStopTime_ms[ 0 ];
+        if ( command.getMoveDuty() > mBrakeReleaseThreshold ){
+            t = mReleaseStopTime_ms[ 1 ];
+        }
+        if ( mTimer->read_ms() > t ){
+            mIsReleaseStop = false;
+            mTimer->stop();
+            mTimer->reset();
+        } else {
+            action = I2CMotor::RELEASE;
+        }
+    }
+    
+    for ( int i = 0; i < Steering::mNumOfTire; ++i ){
+        mTire[ i ]->setActionType( action );
+    }
+}
+
+void SteeringTire::updateWaitServo( Command command ){
+    // サーボの位置調整待ち中は移動用モータは止める
+    for ( int i = 0; i < Steering::mNumOfTire; ++i ){
+        mTire[ i ]->setActionType( I2CMotor::BRAKE );
+    }
+}