Steering - | Mbed

Users » inst » Code » Steering

Steering.cpp@4:912ac0c2c52a, 2015-10-15 (annotated)

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

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@4:912ac0c2c52a, 2015-10-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning