SteeringTire.cpp@0:0cd6b505ba45, 2015-10-15 (annotated)
- Committer:
- inst
- Date:
- Thu Oct 15 08:46:09 2015 +0000
- Revision:
- 0:0cd6b505ba45
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
inst | 0:0cd6b505ba45 | 1 | #include "SteeringTire.h" |
inst | 0:0cd6b505ba45 | 2 | #include "Command.h" |
inst | 0:0cd6b505ba45 | 3 | #include "I2CMotor.h" |
inst | 0:0cd6b505ba45 | 4 | #include "Math.h" |
inst | 0:0cd6b505ba45 | 5 | #include "Steering.h" |
inst | 0:0cd6b505ba45 | 6 | #include "mbed.h" |
inst | 0:0cd6b505ba45 | 7 | |
inst | 0:0cd6b505ba45 | 8 | const uint32_t SteeringTire::mReleaseStopTime_ms[] = { |
inst | 0:0cd6b505ba45 | 9 | //10, |
inst | 0:0cd6b505ba45 | 10 | 100, |
inst | 0:0cd6b505ba45 | 11 | 300 |
inst | 0:0cd6b505ba45 | 12 | }; |
inst | 0:0cd6b505ba45 | 13 | const float SteeringTire::mBrakeReleaseThreshold = 0.50f; |
inst | 0:0cd6b505ba45 | 14 | |
inst | 0:0cd6b505ba45 | 15 | SteeringTire::SteeringTire( I2CMotor** tire ) : mTire( tire ){ |
inst | 0:0cd6b505ba45 | 16 | mIsReleaseStop = false; |
inst | 0:0cd6b505ba45 | 17 | mTimer = new Timer; |
inst | 0:0cd6b505ba45 | 18 | mPrevActionType = Steering::STOP; |
inst | 0:0cd6b505ba45 | 19 | } |
inst | 0:0cd6b505ba45 | 20 | |
inst | 0:0cd6b505ba45 | 21 | SteeringTire::~SteeringTire(){ |
inst | 0:0cd6b505ba45 | 22 | delete mTire; |
inst | 0:0cd6b505ba45 | 23 | } |
inst | 0:0cd6b505ba45 | 24 | |
inst | 0:0cd6b505ba45 | 25 | void SteeringTire::update( Steering::ActionType action, Command command ){ |
inst | 0:0cd6b505ba45 | 26 | bool isReleaseStop = ( mPrevActionType == Steering::MOVE ); |
inst | 0:0cd6b505ba45 | 27 | isReleaseStop &= ( action == Steering::STOP ); |
inst | 0:0cd6b505ba45 | 28 | |
inst | 0:0cd6b505ba45 | 29 | if ( isReleaseStop ){ |
inst | 0:0cd6b505ba45 | 30 | mIsReleaseStop = true; |
inst | 0:0cd6b505ba45 | 31 | mTimer->reset(); |
inst | 0:0cd6b505ba45 | 32 | mTimer->start(); |
inst | 0:0cd6b505ba45 | 33 | } |
inst | 0:0cd6b505ba45 | 34 | |
inst | 0:0cd6b505ba45 | 35 | switch ( action ){ |
inst | 0:0cd6b505ba45 | 36 | case Steering::STOP: |
inst | 0:0cd6b505ba45 | 37 | updateStop( command ); |
inst | 0:0cd6b505ba45 | 38 | break; |
inst | 0:0cd6b505ba45 | 39 | |
inst | 0:0cd6b505ba45 | 40 | case Steering::MOVE: |
inst | 0:0cd6b505ba45 | 41 | updateMove( command ); |
inst | 0:0cd6b505ba45 | 42 | break; |
inst | 0:0cd6b505ba45 | 43 | |
inst | 0:0cd6b505ba45 | 44 | case Steering::ROLL: |
inst | 0:0cd6b505ba45 | 45 | updateRoll( command ); |
inst | 0:0cd6b505ba45 | 46 | break; |
inst | 0:0cd6b505ba45 | 47 | |
inst | 0:0cd6b505ba45 | 48 | case Steering::WAIT_SERVO: |
inst | 0:0cd6b505ba45 | 49 | updateWaitServo( command ); |
inst | 0:0cd6b505ba45 | 50 | break; |
inst | 0:0cd6b505ba45 | 51 | } |
inst | 0:0cd6b505ba45 | 52 | |
inst | 0:0cd6b505ba45 | 53 | if ( action != Steering::WAIT_SERVO ){ |
inst | 0:0cd6b505ba45 | 54 | mPrevActionType = action; |
inst | 0:0cd6b505ba45 | 55 | } |
inst | 0:0cd6b505ba45 | 56 | } |
inst | 0:0cd6b505ba45 | 57 | |
inst | 0:0cd6b505ba45 | 58 | void SteeringTire::updateMove( Command command ){ |
inst | 0:0cd6b505ba45 | 59 | I2CMotor::ActionType action = I2CMotor::FORWARD; |
inst | 0:0cd6b505ba45 | 60 | |
inst | 0:0cd6b505ba45 | 61 | // 目標角度>PI ならモータを逆に回す0-pi[rad]で対応 |
inst | 0:0cd6b505ba45 | 62 | if ( command.getMoveDirection_rad() > gPI ){ |
inst | 0:0cd6b505ba45 | 63 | action = I2CMotor::REVERSE; |
inst | 0:0cd6b505ba45 | 64 | } |
inst | 0:0cd6b505ba45 | 65 | // タイヤの回転方向(正転or逆転)と速さ(duty)を更新 |
inst | 0:0cd6b505ba45 | 66 | for ( int i = 0; i < Steering::mNumOfTire; ++i ){ |
inst | 0:0cd6b505ba45 | 67 | mTire[ i ]->setActionType( action ); |
inst | 0:0cd6b505ba45 | 68 | mTire[ i ]->setPercent( command.getMoveDuty() ); |
inst | 0:0cd6b505ba45 | 69 | } |
inst | 0:0cd6b505ba45 | 70 | |
inst | 0:0cd6b505ba45 | 71 | if ( isNear( command.getMoveDirection_rad(), 0.0f, 0.01f ) || |
inst | 0:0cd6b505ba45 | 72 | isNear( command.getMoveDirection_rad(), gPI, 0.01f ) || |
inst | 0:0cd6b505ba45 | 73 | isNear( command.getMoveDirection_rad(), -gPI, 0.01f ) ){ |
inst | 0:0cd6b505ba45 | 74 | mTire[ 0 ]->setPercent( command.getMoveDuty() * 0.30f ); |
inst | 0:0cd6b505ba45 | 75 | mTire[ 2 ]->setPercent( command.getMoveDuty() * 0.30f ); |
inst | 0:0cd6b505ba45 | 76 | } |
inst | 0:0cd6b505ba45 | 77 | } |
inst | 0:0cd6b505ba45 | 78 | |
inst | 0:0cd6b505ba45 | 79 | void SteeringTire::updateRoll( Command command ){ |
inst | 0:0cd6b505ba45 | 80 | float coeff = command.getRollCoeff(); |
inst | 0:0cd6b505ba45 | 81 | // デフォルトでは0,1が正転で2,3が逆転1(時計回り) |
inst | 0:0cd6b505ba45 | 82 | I2CMotor::ActionType action[] = { I2CMotor::FORWARD, I2CMotor::REVERSE }; |
inst | 0:0cd6b505ba45 | 83 | |
inst | 0:0cd6b505ba45 | 84 | if ( coeff < 0.0f ){ |
inst | 0:0cd6b505ba45 | 85 | // 回転係数がマイナスなら逆回転(反時計周り) |
inst | 0:0cd6b505ba45 | 86 | I2CMotor::ActionType temp = action[ 0 ]; |
inst | 0:0cd6b505ba45 | 87 | action[ 0 ] = action[ 1 ]; |
inst | 0:0cd6b505ba45 | 88 | action[ 1 ] = temp; |
inst | 0:0cd6b505ba45 | 89 | |
inst | 0:0cd6b505ba45 | 90 | coeff = abs( coeff ); |
inst | 0:0cd6b505ba45 | 91 | } |
inst | 0:0cd6b505ba45 | 92 | |
inst | 0:0cd6b505ba45 | 93 | for ( int i = 0; i < Steering::mNumOfTire; ++i ){ |
inst | 0:0cd6b505ba45 | 94 | // 左半分と右半分のモータは逆に駆動する |
inst | 0:0cd6b505ba45 | 95 | mTire[ i ]->setActionType( action[ i / 2 ] ); |
inst | 0:0cd6b505ba45 | 96 | mTire[ i ]->setPercent( coeff ); |
inst | 0:0cd6b505ba45 | 97 | } |
inst | 0:0cd6b505ba45 | 98 | } |
inst | 0:0cd6b505ba45 | 99 | |
inst | 0:0cd6b505ba45 | 100 | void SteeringTire::updateStop( Command command ){ |
inst | 0:0cd6b505ba45 | 101 | I2CMotor::ActionType action = I2CMotor::BRAKE; |
inst | 0:0cd6b505ba45 | 102 | |
inst | 0:0cd6b505ba45 | 103 | if ( mIsReleaseStop ){ |
inst | 0:0cd6b505ba45 | 104 | float t = mReleaseStopTime_ms[ 0 ]; |
inst | 0:0cd6b505ba45 | 105 | if ( command.getMoveDuty() > mBrakeReleaseThreshold ){ |
inst | 0:0cd6b505ba45 | 106 | t = mReleaseStopTime_ms[ 1 ]; |
inst | 0:0cd6b505ba45 | 107 | } |
inst | 0:0cd6b505ba45 | 108 | if ( mTimer->read_ms() > t ){ |
inst | 0:0cd6b505ba45 | 109 | mIsReleaseStop = false; |
inst | 0:0cd6b505ba45 | 110 | mTimer->stop(); |
inst | 0:0cd6b505ba45 | 111 | mTimer->reset(); |
inst | 0:0cd6b505ba45 | 112 | } else { |
inst | 0:0cd6b505ba45 | 113 | action = I2CMotor::RELEASE; |
inst | 0:0cd6b505ba45 | 114 | } |
inst | 0:0cd6b505ba45 | 115 | } |
inst | 0:0cd6b505ba45 | 116 | |
inst | 0:0cd6b505ba45 | 117 | for ( int i = 0; i < Steering::mNumOfTire; ++i ){ |
inst | 0:0cd6b505ba45 | 118 | mTire[ i ]->setActionType( action ); |
inst | 0:0cd6b505ba45 | 119 | } |
inst | 0:0cd6b505ba45 | 120 | } |
inst | 0:0cd6b505ba45 | 121 | |
inst | 0:0cd6b505ba45 | 122 | void SteeringTire::updateWaitServo( Command command ){ |
inst | 0:0cd6b505ba45 | 123 | // サーボの位置調整待ち中は移動用モータは止める |
inst | 0:0cd6b505ba45 | 124 | for ( int i = 0; i < Steering::mNumOfTire; ++i ){ |
inst | 0:0cd6b505ba45 | 125 | mTire[ i ]->setActionType( I2CMotor::BRAKE ); |
inst | 0:0cd6b505ba45 | 126 | } |
inst | 0:0cd6b505ba45 | 127 | } |