Steering - | Mbed

Users » inst » Code » Steering

Steering.cpp@2:8be8699c5afd, 2015-10-14 (annotated)

Committer:: inst
Date:: Wed Oct 14 03:55:16 2015 +0000
Revision:: 2:8be8699c5afd
Parent:: 1:4b719f80e9c4
Child:: 3:77264fa5d84b

Who changed what in which revision?

User	Revision	Line number	New contents of line
inst	0:0bac1fed0273	1	#include "Steering.h"
inst	2:8be8699c5afd	2	#include "SteeringTire.h"
inst	0:0bac1fed0273	3	#include "I2CServo.h"
inst	0:0bac1fed0273	4	#include "Math.h"
inst	0:0bac1fed0273	5	#include "Command.h"
inst	2:8be8699c5afd	6	#include "ShootingSystem.h"
inst	2:8be8699c5afd	7	#include "Steering.h"
inst	0:0bac1fed0273	8
inst	1:4b719f80e9c4	9	// Tire Number
inst	0:0bac1fed0273	10	// 0 3
inst	0:0bac1fed0273	11	//
inst	0:0bac1fed0273	12	// 1 2
inst	2:8be8699c5afd	13	const int Steering::mNumOfTire = 3;
inst	0:0bac1fed0273	14	// 移動中にこれ以上大きな移動方向角度の変化があったらいったん止まる
inst	0:0bac1fed0273	15	const float Steering::mStoppingMoveAngle = gPI * 0.5f;
inst	0:0bac1fed0273	16	const float Steering::mRollTirePosition[] = {
inst	2:8be8699c5afd	17	0.1432678f,
inst	2:8be8699c5afd	18	1.00f,
inst	2:8be8699c5afd	19	0.8567322f
inst	0:0bac1fed0273	20	};
inst	0:0bac1fed0273	21
inst	2:8be8699c5afd	22	Steering::Steering( I2CMotor t, I2CServo s ) : mServo( s ){
inst	2:8be8699c5afd	23	mActionType = STOP;
inst	2:8be8699c5afd	24	mTire = new SteeringTire( t );
inst	0:0bac1fed0273	25	}
inst	0:0bac1fed0273	26
inst	0:0bac1fed0273	27	Steering::~Steering(){
inst	2:8be8699c5afd	28	delete mTire;
inst	0:0bac1fed0273	29	}
inst	0:0bac1fed0273	30
inst	0:0bac1fed0273	31	void Steering::update( Command command ){
inst	2:8be8699c5afd	32	mTire->update( mActionType, command );
inst	0:0bac1fed0273	33
inst	0:0bac1fed0273	34	switch ( mActionType ){
inst	2:8be8699c5afd	35	case MOVE:
inst	0:0bac1fed0273	36	updateMove( command );
inst	0:0bac1fed0273	37	break;
inst	0:0bac1fed0273	38
inst	2:8be8699c5afd	39	case ROLL:
inst	0:0bac1fed0273	40	updateRoll( command );
inst	0:0bac1fed0273	41	break;
inst	0:0bac1fed0273	42
inst	2:8be8699c5afd	43	case STOP:
inst	0:0bac1fed0273	44	updateStop( command );
inst	0:0bac1fed0273	45	break;
inst	0:0bac1fed0273	46
inst	2:8be8699c5afd	47	case WAIT_SERVO:
inst	0:0bac1fed0273	48	updateWaitServo( command );
inst	0:0bac1fed0273	49	break;
inst	2:8be8699c5afd	50
inst	0:0bac1fed0273	51	default:
inst	2:8be8699c5afd	52	mActionType = STOP;
inst	0:0bac1fed0273	53	updateStop( command );
inst	0:0bac1fed0273	54	break;
inst	0:0bac1fed0273	55	}
inst	2:8be8699c5afd	56
inst	2:8be8699c5afd	57	updateActionType( command );
inst	2:8be8699c5afd	58	}
inst	2:8be8699c5afd	59
inst	2:8be8699c5afd	60	void Steering::updateActionType( Command command ){
inst	2:8be8699c5afd	61	if ( command.getSteeringActionType() != mActionType ){
inst	2:8be8699c5afd	62	if ( command.getSteeringActionType() == STOP ){
inst	2:8be8699c5afd	63	mActionType = STOP;
inst	2:8be8699c5afd	64	setServoPositionByActionType( mNextActionType, command );
inst	2:8be8699c5afd	65	return;
inst	2:8be8699c5afd	66	}
inst	2:8be8699c5afd	67
inst	2:8be8699c5afd	68	// 現在と違う動作が求められたらサーボの向きを調整するためのシーケンス遷移をする
inst	2:8be8699c5afd	69	mActionType = WAIT_SERVO;
inst	2:8be8699c5afd	70	mNextActionType = command.getSteeringActionType();
inst	2:8be8699c5afd	71	setServoPositionByActionType( mNextActionType, command );
inst	2:8be8699c5afd	72	}
inst	0:0bac1fed0273	73	}
inst	0:0bac1fed0273	74
inst	0:0bac1fed0273	75	void Steering::updateMove( Command command ){
inst	2:8be8699c5afd	76	// モータの角度をサーボを使って調整（0度~180度）
inst	2:8be8699c5afd	77	setServoPositionByActionType( MOVE, command );
inst	0:0bac1fed0273	78
inst	2:8be8699c5afd	79	/////////////////////
inst	2:8be8699c5afd	80	/*
inst	0:0bac1fed0273	81	// 現在のサーボの目標角度と実際の角度の差で一番大きな値を求める
inst	2:8be8699c5afd	82	float maxDiff = 0.0f;
inst	0:0bac1fed0273	83	for ( int i = 0; i < mNumOfTire; ++i ){
inst	2:8be8699c5afd	84	float diff = abs( mServo[ i ]->getPosition() * gPI - mMoveDirection_rad );
inst	2:8be8699c5afd	85	if ( diff > maxDiff ){
inst	2:8be8699c5afd	86	maxDiff = diff;
inst	0:0bac1fed0273	87	}
inst	0:0bac1fed0273	88	}
inst	0:0bac1fed0273	89
inst	0:0bac1fed0273	90	// 一定以上の移動方向変化なら一旦タイヤの向きを変えるのを待つために状態遷移する
inst	2:8be8699c5afd	91	if ( maxDiff > mStoppingMoveAngle ){
inst	2:8be8699c5afd	92	mActionType = WAIT_SERVO;
inst	2:8be8699c5afd	93	mNextActionType = MOVE;
inst	2:8be8699c5afd	94	return;
inst	0:0bac1fed0273	95	}
inst	2:8be8699c5afd	96	////////////////
inst	2:8be8699c5afd	97	*/
inst	0:0bac1fed0273	98	}
inst	0:0bac1fed0273	99
inst	0:0bac1fed0273	100	void Steering::updateRoll( Command command ){
inst	2:8be8699c5afd	101	}
inst	2:8be8699c5afd	102
inst	2:8be8699c5afd	103	void Steering::updateStop( Command command ){
inst	2:8be8699c5afd	104	}
inst	2:8be8699c5afd	105
inst	2:8be8699c5afd	106	void Steering::updateWaitServo( Command command ){
inst	2:8be8699c5afd	107	if ( mNextActionType == STOP ){
inst	2:8be8699c5afd	108	// もし次の状態が停止状態ならサーボの状態に関係なく止める
inst	2:8be8699c5afd	109	// （もしサーボがI2Cバス上になかったらサーボ停止信号が受信できず止まらなくなるから）
inst	2:8be8699c5afd	110	mActionType = STOP;
inst	2:8be8699c5afd	111	return;
inst	0:0bac1fed0273	112	}
inst	0:0bac1fed0273	113
inst	0:0bac1fed0273	114	for ( int i = 0; i < mNumOfTire; ++i ){
inst	2:8be8699c5afd	115	// 一つでも目標角度に達していないサーボがあったら次の状態には遷移させない
inst	2:8be8699c5afd	116	if ( !mServo[ i ]->hasStopped() ){
inst	0:0bac1fed0273	117	return;
inst	0:0bac1fed0273	118	}
inst	0:0bac1fed0273	119	}
inst	0:0bac1fed0273	120
inst	0:0bac1fed0273	121	mActionType = mNextActionType;
inst	0:0bac1fed0273	122	}
inst	0:0bac1fed0273	123
inst	2:8be8699c5afd	124	void Steering::setServoPositionByActionType( ActionType action, Command command ){
inst	0:0bac1fed0273	125	switch ( action ){
inst	2:8be8699c5afd	126	case MOVE:
inst	1:4b719f80e9c4	127	setServoPositionWhenMove( command );
inst	1:4b719f80e9c4	128	break;
inst	0:0bac1fed0273	129
inst	2:8be8699c5afd	130	case ROLL:
inst	1:4b719f80e9c4	131	setServoPositionWhenRoll();
inst	0:0bac1fed0273	132	break;
inst	1:4b719f80e9c4	133
inst	2:8be8699c5afd	134	case STOP:
inst	1:4b719f80e9c4	135	setServoPositionWhenStop();
inst	0:0bac1fed0273	136	break;
inst	0:0bac1fed0273	137
inst	2:8be8699c5afd	138	case WAIT_SERVO:
inst	2:8be8699c5afd	139	break;
inst	2:8be8699c5afd	140
inst	0:0bac1fed0273	141	default:
inst	1:4b719f80e9c4	142	setServoPositionWhenStop();
inst	0:0bac1fed0273	143	return;
inst	0:0bac1fed0273	144	}
inst	0:0bac1fed0273	145	}
inst	2:8be8699c5afd	146
inst	2:8be8699c5afd	147	void adjust( float* angle_deg ){
inst	2:8be8699c5afd	148	float x = cos( *angle_deg );
inst	2:8be8699c5afd	149	float y = sin( *angle_deg );
inst	2:8be8699c5afd	150	float adjustY = 1.0f;
inst	2:8be8699c5afd	151	float p = 0.10f;
inst	2:8be8699c5afd	152
inst	2:8be8699c5afd	153	y = p * adjustY + ( 1.0f - p ) * y;
inst	2:8be8699c5afd	154
inst	2:8be8699c5afd	155	*angle_deg = atan2( y, x );
inst	2:8be8699c5afd	156
inst	2:8be8699c5afd	157	if ( *angle_deg < 0.0f ){
inst	2:8be8699c5afd	158	angle_deg += gPI 2.0f;
inst	2:8be8699c5afd	159	}
inst	2:8be8699c5afd	160	}
inst	2:8be8699c5afd	161
inst	1:4b719f80e9c4	162	void Steering::setServoPositionWhenMove( Command command ){
inst	1:4b719f80e9c4	163	float angle = command.getMoveDirection_rad();
inst	1:4b719f80e9c4	164
inst	2:8be8699c5afd	165	adjust( &angle );
inst	2:8be8699c5afd	166
inst	1:4b719f80e9c4	167	// 目標角度>PI ならモータを逆に回すことで180度引いた角度で対応
inst	1:4b719f80e9c4	168	if ( angle > gPI ){
inst	1:4b719f80e9c4	169	angle -= gPI;
inst	1:4b719f80e9c4	170	}
inst	1:4b719f80e9c4	171
inst	1:4b719f80e9c4	172	// 角度（0~pi）からサーボの位置データ（0.0~1.0）に変換
inst	1:4b719f80e9c4	173	float pos = convertRange( angle, 0.0f, gPI, 0.0f, 1.0f );
inst	1:4b719f80e9c4	174
inst	1:4b719f80e9c4	175	// タイヤの向き（0.0~1.0）を更新
inst	1:4b719f80e9c4	176	for ( int i = 0; i < mNumOfTire; ++i ){
inst	1:4b719f80e9c4	177	mServo[ i ]->setTargetPosition( pos );
inst	1:4b719f80e9c4	178	}
inst	1:4b719f80e9c4	179
inst	1:4b719f80e9c4	180	mMoveDirection_rad = angle;
inst	1:4b719f80e9c4	181	}
inst	1:4b719f80e9c4	182
inst	1:4b719f80e9c4	183	void Steering::setServoPositionWhenRoll(){
inst	1:4b719f80e9c4	184	// タイヤの向き（0.0~1.0）を更新
inst	1:4b719f80e9c4	185	for ( int i = 0; i < mNumOfTire; ++i ){
inst	1:4b719f80e9c4	186	mServo[ i ]->setTargetPosition( mRollTirePosition[ i ] );
inst	1:4b719f80e9c4	187	}
inst	1:4b719f80e9c4	188	}
inst	1:4b719f80e9c4	189
inst	1:4b719f80e9c4	190	void Steering::setServoPositionWhenStop(){
inst	2:8be8699c5afd	191	// サーボを止める
inst	1:4b719f80e9c4	192	for ( int i = 0; i < mNumOfTire; ++i ){
inst	2:8be8699c5afd	193	//mServo[ i ]->stop();
inst	1:4b719f80e9c4	194	}
inst	1:4b719f80e9c4	195	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	14 Oct 2015
Imports:	0
Forks:	0
Commits:	5
Dependents:	1
Dependencies:	1
Followers:	1

Steering.cpp@2:8be8699c5afd, 2015-10-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning