tarou yamada / Steering

Dependencies:   SteeringTire

Dependents:   OBROT_ALL

Steering.cpp@0:0bac1fed0273, 2015-08-15 (annotated)

Committer:
inst
Date:
Sat Aug 15 13:31:39 2015 +0000
Revision:
0:0bac1fed0273
Child:
1:4b719f80e9c4
new;

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