tarou yamada / Steering

Dependencies:   SteeringTire

Dependents:   OBROT_ALL

Steering.cpp@1:4b719f80e9c4, 2015-08-21 (annotated)

Committer:
inst
Date:
Fri Aug 21 04:52:10 2015 +0000
Revision:
1:4b719f80e9c4
Parent:
0:0bac1fed0273
Child:
2:8be8699c5afd
y evol

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inst 0:0bac1fed0273 1 #include "Steering.h"
inst 0:0bac1fed0273 2 #include "I2CMotor.h"
inst 0:0bac1fed0273 3 #include "I2CServo.h"
inst 0:0bac1fed0273 4 #include "Math.h"
inst 0:0bac1fed0273 5 #include "Command.h"
inst 0:0bac1fed0273 6
inst 1:4b719f80e9c4 7 // Tire Number
inst 0:0bac1fed0273 8 // 0 3
inst 0:0bac1fed0273 9 //
inst 0:0bac1fed0273 10 // 1 2
inst 0:0bac1fed0273 11 const int Steering::mNumOfTire = 4;
inst 0:0bac1fed0273 12 const float Steering::mAllowableError = 0.08f;
inst 0:0bac1fed0273 13 // 移動中にこれ以上大きな移動方向角度の変化があったらいったん止まる
inst 0:0bac1fed0273 14 const float Steering::mStoppingMoveAngle = gPI * 0.5f;
inst 0:0bac1fed0273 15 const float Steering::mRollTirePosition[] = {
inst 0:0bac1fed0273 16 0.25f,
inst 0:0bac1fed0273 17 0.75f,
inst 0:0bac1fed0273 18 0.25f,
inst 0:0bac1fed0273 19 0.75f
inst 0:0bac1fed0273 20 };
inst 0:0bac1fed0273 21
inst 0:0bac1fed0273 22 Steering::Steering( I2CMotor** t, I2CServo** s, XBee* xbee ) :
inst 0:0bac1fed0273 23 mTire( t ),
inst 0:0bac1fed0273 24 mServo( s ),
inst 0:0bac1fed0273 25 mXBee( xbee ){
inst 0:0bac1fed0273 26 mActionType = Command::STOP;
inst 0:0bac1fed0273 27 }
inst 0:0bac1fed0273 28
inst 0:0bac1fed0273 29 Steering::~Steering(){
inst 0:0bac1fed0273 30 }
inst 0:0bac1fed0273 31
inst 0:0bac1fed0273 32 void Steering::update( Command command ){
inst 1:4b719f80e9c4 33 Command::ActionType now = mActionType;
inst 1:4b719f80e9c4 34 if ( mActionType == Command::WAIT_SERVO ){
inst 1:4b719f80e9c4 35 // もう既にサーボの動作完了待ちならサーボ完了後に移行予定の動作と比較する
inst 1:4b719f80e9c4 36 now = mNextActionType;
inst 1:4b719f80e9c4 37 }
inst 1:4b719f80e9c4 38 if ( command.getActionType() != now ){
inst 1:4b719f80e9c4 39 // 現在と違う動作が求められたらサーボの向きを調整するためのシーケンス遷移をする
inst 0:0bac1fed0273 40 mActionType = Command::WAIT_SERVO;
inst 0:0bac1fed0273 41 mNextActionType = command.getActionType();
inst 0:0bac1fed0273 42 setServoPositionByActionType( mNextActionType, command );
inst 0:0bac1fed0273 43 }
inst 0:0bac1fed0273 44
inst 0:0bac1fed0273 45 switch ( mActionType ){
inst 0:0bac1fed0273 46 case Command::MOVE:
inst 0:0bac1fed0273 47 updateMove( command );
inst 0:0bac1fed0273 48 break;
inst 0:0bac1fed0273 49
inst 0:0bac1fed0273 50 case Command::ROLL:
inst 0:0bac1fed0273 51 updateRoll( command );
inst 0:0bac1fed0273 52 break;
inst 0:0bac1fed0273 53
inst 0:0bac1fed0273 54 case Command::STOP:
inst 0:0bac1fed0273 55 updateStop( command );
inst 0:0bac1fed0273 56 break;
inst 0:0bac1fed0273 57
inst 0:0bac1fed0273 58 case Command::WAIT_SERVO:
inst 0:0bac1fed0273 59 updateWaitServo( command );
inst 0:0bac1fed0273 60 break;
inst 0:0bac1fed0273 61
inst 0:0bac1fed0273 62 default:
inst 0:0bac1fed0273 63 mActionType = Command::STOP;
inst 0:0bac1fed0273 64 updateStop( command );
inst 0:0bac1fed0273 65 break;
inst 0:0bac1fed0273 66 }
inst 0:0bac1fed0273 67 }
inst 0:0bac1fed0273 68
inst 0:0bac1fed0273 69 void Steering::updateMove( Command command ){
inst 0:0bac1fed0273 70 I2CMotor::ActionType action = I2CMotor::FORWARD;
inst 0:0bac1fed0273 71
inst 0:0bac1fed0273 72 // 目標角度>PI ならモータを逆に回すことでプラス角度で対応（角度の調整はここではやらない）
inst 0:0bac1fed0273 73 if ( command.getMoveDirection_rad() > gPI ){
inst 0:0bac1fed0273 74 action = I2CMotor::REVERSE;
inst 0:0bac1fed0273 75 }
inst 0:0bac1fed0273 76
inst 0:0bac1fed0273 77 // モータの角度をサーボを使って調整（0度~180度）
inst 0:0bac1fed0273 78 setServoPositionByActionType( Command::MOVE, command );
inst 0:0bac1fed0273 79
inst 0:0bac1fed0273 80 // 現在のサーボの目標角度と実際の角度の差で一番大きな値を求める
inst 0:0bac1fed0273 81 float diff = 0.0f;
inst 0:0bac1fed0273 82 for ( int i = 0; i < mNumOfTire; ++i ){
inst 0:0bac1fed0273 83 if ( abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad ) > diff ){
inst 0:0bac1fed0273 84 diff = abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad );
inst 0:0bac1fed0273 85 }
inst 0:0bac1fed0273 86 }
inst 0:0bac1fed0273 87
inst 0:0bac1fed0273 88 // 一定以上の移動方向変化なら一旦タイヤの向きを変えるのを待つために状態遷移する
inst 0:0bac1fed0273 89 if ( diff > mStoppingMoveAngle ){
inst 0:0bac1fed0273 90 mActionType = Command::WAIT_SERVO;
inst 0:0bac1fed0273 91 mNextActionType = Command::MOVE;
inst 0:0bac1fed0273 92 }
inst 0:0bac1fed0273 93
inst 0:0bac1fed0273 94 // タイヤの回転方向（正転or逆転）と速さ（duty）を更新
inst 0:0bac1fed0273 95 for ( int i = 0; i < mNumOfTire; ++i ){
inst 0:0bac1fed0273 96 mTire[ i ]->setActionType( action );
inst 0:0bac1fed0273 97 mTire[ i ]->setDuty( command.getMoveDuty() );
inst 0:0bac1fed0273 98 }
inst 0:0bac1fed0273 99 }
inst 0:0bac1fed0273 100
inst 0:0bac1fed0273 101 void Steering::updateAbsRoll( Command command ){
inst 0:0bac1fed0273 102 // todo : implement
inst 0:0bac1fed0273 103 }
inst 0:0bac1fed0273 104
inst 1:4b719f80e9c4 105 // Tire Number
inst 1:4b719f80e9c4 106 // 0 3
inst 1:4b719f80e9c4 107 //
inst 1:4b719f80e9c4 108 // 1 2
inst 0:0bac1fed0273 109 void Steering::updateRoll( Command command ){
inst 0:0bac1fed0273 110 float coeff = command.getRollCoeff();
inst 1:4b719f80e9c4 111 // デフォルトでは0,1が正転で2,3が逆転1（時計回り）
inst 0:0bac1fed0273 112 I2CMotor::ActionType action[] = { I2CMotor::FORWARD, I2CMotor::REVERSE };
inst 0:0bac1fed0273 113
inst 0:0bac1fed0273 114 if ( coeff < 0.0f ){
inst 1:4b719f80e9c4 115 // 回転係数がマイナスなら逆回転（反時計周り）
inst 0:0bac1fed0273 116 I2CMotor::ActionType temp = action[ 0 ];
inst 0:0bac1fed0273 117 action[ 0 ] = action[ 1 ];
inst 0:0bac1fed0273 118 action[ 1 ] = temp;
inst 0:0bac1fed0273 119
inst 0:0bac1fed0273 120 coeff *= -1.0f;
inst 0:0bac1fed0273 121 }
inst 0:0bac1fed0273 122
inst 0:0bac1fed0273 123 for ( int i = 0; i < mNumOfTire; ++i ){
inst 0:0bac1fed0273 124 // 左半分と右半分のモータは逆に駆動する
inst 0:0bac1fed0273 125 mTire[ i ]->setActionType( action[ i / 2 ] );
inst 0:0bac1fed0273 126 mTire[ i ]->setDuty( coeff );
inst 0:0bac1fed0273 127 }
inst 0:0bac1fed0273 128 }
inst 0:0bac1fed0273 129
inst 0:0bac1fed0273 130 void Steering::updateStop( Command command ){
inst 1:4b719f80e9c4 131 // 全駆動用モータがブレーキ
inst 0:0bac1fed0273 132 for ( int i = 0; i < mNumOfTire; ++i ){
inst 0:0bac1fed0273 133 mTire[ i ]->setActionType( I2CMotor::BRAKE );
inst 0:0bac1fed0273 134 }
inst 0:0bac1fed0273 135 }
inst 0:0bac1fed0273 136
inst 0:0bac1fed0273 137 void Steering::updateWaitServo( Command command ){
inst 0:0bac1fed0273 138 if ( command.getActionType() != mNextActionType ){
inst 0:0bac1fed0273 139 mNextActionType = command.getActionType();
inst 0:0bac1fed0273 140 setServoPositionByActionType( mNextActionType, command );
inst 0:0bac1fed0273 141 }
inst 0:0bac1fed0273 142
inst 0:0bac1fed0273 143 for ( int i = 0; i < mNumOfTire; ++i ){
inst 0:0bac1fed0273 144 // 一つでも目標角度に達していないサーボがあったら抜ける
inst 0:0bac1fed0273 145 if ( mServo[ i ]->isStop() == false ){
inst 0:0bac1fed0273 146 return;
inst 0:0bac1fed0273 147 }
inst 0:0bac1fed0273 148 }
inst 0:0bac1fed0273 149
inst 0:0bac1fed0273 150 mActionType = mNextActionType;
inst 0:0bac1fed0273 151 }
inst 0:0bac1fed0273 152
inst 0:0bac1fed0273 153 void Steering::setServoPositionByActionType( Command::ActionType action, Command command ){
inst 0:0bac1fed0273 154 switch ( action ){
inst 1:4b719f80e9c4 155 case Command::MOVE:
inst 1:4b719f80e9c4 156 setServoPositionWhenMove( command );
inst 1:4b719f80e9c4 157 break;
inst 0:0bac1fed0273 158
inst 1:4b719f80e9c4 159 case Command::ROLL:
inst 1:4b719f80e9c4 160 setServoPositionWhenRoll();
inst 0:0bac1fed0273 161 break;
inst 1:4b719f80e9c4 162
inst 1:4b719f80e9c4 163 case Command::STOP:
inst 1:4b719f80e9c4 164 setServoPositionWhenStop();
inst 0:0bac1fed0273 165 break;
inst 0:0bac1fed0273 166
inst 0:0bac1fed0273 167 default:
inst 1:4b719f80e9c4 168 setServoPositionWhenStop();
inst 0:0bac1fed0273 169 return;
inst 0:0bac1fed0273 170 }
inst 0:0bac1fed0273 171 }
inst 1:4b719f80e9c4 172 void Steering::setServoPositionWhenMove( Command command ){
inst 1:4b719f80e9c4 173 float angle = command.getMoveDirection_rad();
inst 1:4b719f80e9c4 174
inst 1:4b719f80e9c4 175 // 目標角度>PI ならモータを逆に回すことで180度引いた角度で対応
inst 1:4b719f80e9c4 176 if ( angle > gPI ){
inst 1:4b719f80e9c4 177 angle -= gPI;
inst 1:4b719f80e9c4 178 }
inst 1:4b719f80e9c4 179
inst 1:4b719f80e9c4 180 // 角度（0~pi）からサーボの位置データ（0.0~1.0）に変換
inst 1:4b719f80e9c4 181 float pos = convertRange( angle, 0.0f, gPI, 0.0f, 1.0f );
inst 1:4b719f80e9c4 182
inst 1:4b719f80e9c4 183 // タイヤの向き（0.0~1.0）を更新
inst 1:4b719f80e9c4 184 for ( int i = 0; i < mNumOfTire; ++i ){
inst 1:4b719f80e9c4 185 mServo[ i ]->setTargetPosition( pos );
inst 1:4b719f80e9c4 186 }
inst 1:4b719f80e9c4 187
inst 1:4b719f80e9c4 188 mMoveDirection_rad = angle;
inst 1:4b719f80e9c4 189 }
inst 1:4b719f80e9c4 190
inst 1:4b719f80e9c4 191 void Steering::setServoPositionWhenRoll(){
inst 1:4b719f80e9c4 192 // タイヤの向き（0.0~1.0）を更新
inst 1:4b719f80e9c4 193 for ( int i = 0; i < mNumOfTire; ++i ){
inst 1:4b719f80e9c4 194 mServo[ i ]->setTargetPosition( mRollTirePosition[ i ] );
inst 1:4b719f80e9c4 195 }
inst 1:4b719f80e9c4 196 }
inst 1:4b719f80e9c4 197
inst 1:4b719f80e9c4 198 void Steering::setServoPositionWhenStop(){
inst 1:4b719f80e9c4 199 // 現在の位置が目標位置であるとする
inst 1:4b719f80e9c4 200 for ( int i = 0; i < mNumOfTire; ++i ){
inst 1:4b719f80e9c4 201 mServo[ i ]->setTargetPosition( mServo[ i ]->getPosition() );
inst 1:4b719f80e9c4 202 }
inst 1:4b719f80e9c4 203 }