Motion sequencer（コード）

Motion sequencer モジュールのコードです。協調運動制御の中心的な機能を提供します。上位にモーション・インターラプタ、下位に各サーボモータの制御モジュールと連携します。

◀ この記事の前に：　Motion sequencer（操作説明）
▶ この記事の次に：　シーケンス・データ

概要

サーボモータの協調運動制御は時間経過に伴う処理です。時間経過による処理は汎用のタイマ割り込みにより、メインループで処理を行います。概要は以下のとおりです。

モジュールの構造

サーボモータの協調運動制御はシーケンスデータに基づき時間に沿った処理を行います。バックグラウンド処理で経時的な協調運動制御処理を行います。この部分をシーケンサと呼びます。シーケンサで扱う一連の協調運動制御をシーケンスと言います。シーケンスの開始や停止などの操作は、このモジュールの上位である Motion sequencer や、他のモジュールで行います。指示を受け付ける関数は APIになります。

シーケンサのバックグラウンド処理は、複数あるタイマ割り込みの中の定時割り込み方式のタイマ割り込みを使用します。このモジュールの下位である Servomotor driver は、これと異なり可変割り込み方式のタイマ割り込みを使用します。同じサーボモータ制御でも時間に関する扱いが異なります。

シーケンスの時間処理

検討課題はシーケンスの時間分解能です。サンプルプログラムでは１秒にしています。表現力を考えると 0.1秒が望ましいと思います。

シーケンス制御の時間管理はフォアグランドで行っています。
サーボモータ制御はサーボモータの仕様を満たすことと、時間のオーダーが μsであること、常時処理を続ける必要があることからバックグラウンド処理で処理をしています。シーケンス制御は時間のオーダーが 100 msであること、シーケンス上のコマンドを実行するタイミングのみの処理となることからフォアグランドで処理を行います。

時間管理は二つあります。

シーケンス処理のインターバル：　シーケンス処理そのものの処理時間の間隔を管理する
シーケンス処理内のコマンド実行時間：　シーケンスデータで実行するコマンドの待ち時間を管理する

メインループ（ loop() ）内にある loopCounter()でフォアグランド内のタイミングを管理しています。シーケンス用の処理のインターバルは gLoopMSで作っています。

定時的なシーケンス処理はフラグ gEventMS で、loop()内の procMS()を実行します。シーケンス処理の実体は procElementSq()で procMS()から呼び出されます。

シーケンス内部でコマンドの実行を時間管理するタイマは gMStimerです。

関数の概要

関数は以下のとおりです。

外部関数

他のジュールやファイルの外部からアクセス可能な関数です。

startSqMS( int sqNo )

引数：

int sqNo：　シーケンス番号

分類：

API（対象は Motion interpreter、他）

機能：

あらかじめ書き込まれたシーケンスを指定して開始する。シーケンスが実行中の場合はそのシーケンスが終了した時点で開始する。（繰り返しや次のシーケンスの実行をしている場合は、新しいシーケンスを開始する前に指定したシーケンスを開始する。）

関係する関数：

上位関数：　Motion interpreter
下位関数：　なし
この関数のコードはこちらです。

startSqForcedMS( int sqNo )

引数：

int sqNo：　シーケンス番号

分類：

API（対象は Motion interpreter、他）

機能：

あらかじめ書き込まれたシーケンスを指定して開始する。シーケンスが実行中の場合はそのシーケンスを中断して開始する。

関係する関数：

上位関数：　Motion interpreter
下位関数：　なし
この関数のコードはこちらです。

quitSqMS()

引数：

なし

分類：

API（対象は Motion interpreter、他）

機能：

実行中のシーケンスがある場合は終了する。

関係する関数：

上位関数：　Motion interpreter
下位関数：　なし
この関数のコードはこちらです。

内部関数

外部から直接アクセスできずモジュール（特定のスコープ内）で使用する関数です。
Motion interpreter（コード）では内部関数はありません。

InitMS()

引数：

なし

分類：

内部関数（初期化）　

機能：

初期化を行う。

関係する関数：

上位関数：　setup()、quitSqMS()、他
下位関数：　（なし）
この関数のコードはこちらです。

procMS()

引数：

なし

分類：

内部関数（メインループ）　

機能：

シーケンスの処理本体。ここでは実行時間の管理を行い、シーケンス処理の実態は下位関数で行う。

関係する関数：

上位関数：　loop()
下位関数：　procElementSq()
この関数のコードはこちらです。

procElementSq()

引数：

なし

分類：

内部関数（メインループ）　

機能：

シーケンスの処理本体。シーケンスデータに基づき、サーボモータへの指示やインターバルの制御を行う。

関係する関数：

上位関数：　procMS()、startSqForcedMS()
下位関数：　setSVposition()
この関数のコードはこちらです。

データの構造

データの構造は以下のとおりです。

シーケンスデータ

シーケンスデータの構造はこちらをご覧ください。
シーケンスデータに関するグローバル定義（構造体、定数、変数）も含まれます。

シーケンスデータと変数の関係

シーケンスデータと変数の関係は以下のとおりです。

グローバル定義

Motion sequencer のグローバル定義は以下のとおりです。

構造体

構造体はありません。

グローバル変数

変数名	型	初期値	説明
gLoopMS	uint8_t	0	定時的なシーケンス処理用のループカウンタ　loop()内
gEventMS	bool	FALSE	定時的なシーケンス処理用のイベントフラグ　loop()内
gMScurrent	int	0	現在実行しているシーケンスの（識別）番号
gMScurrentSq	int	0	現在実行しているシーケンスデータのコマンドの位置
gMStimer	int	0	シーケンス制御用のタイマシーケンスデータのインターバル（次のコマンドまでの待ち時間）を耐回り込みによりダウンカウント
gMSrunningFlag	bool	FALSE	シーケンスの実行中フラグ TRUE：　シーケンスの実行中 FALSE：　シーケンスを実行していない
gMSRequestFlag	bool	FALSE	次に実行するシーケンスの開始の依頼フラグ TRUE：　シーケンスの実行要求があった FALSE：　シーケンスの実行要求はないシーケンスを実行中に次のシーケンスの開始を指示された場合に保持する
gMSnextSq	int	0	次に実行するシーケンスの（識別）番号シーケンスを実行中に次のシーケンスの開始を指示された場合に保持する

コード

コードは以下のとおりです。

関数間の関係

関数の関係は以下のとおりです。

startSqMS( int sqNo )

あらかじめ書き込まれたシーケンスを指定して開始します。

/*----------------------------------------------------------------------------*/
/* Start sequence MS
/* param     : sqNo : Sequence Number
/* return    : non
/*----------------------------------------------------------------------------*/
void startSqMS( int sqNo )
{
	if( gMSrunningFlag == true ){

		gMSnextSq		= sqNo;	 
		gMSRequestFlag	= true;	 

	} else {

		startSqForcedMS( sqNo );

	}
}

シーケンスが実行中の場合はそのシーケンスが終了した時点で開始します。そのため、現在シーケンスが実行中か否かを判断するフラグ gMSrunningFlag を参照、設定しています。処理の実体は下位関数である startSqForcedMS()です。この関数はそのまま startSqMS()と同じ様に APIを提供する外部関数でもあります。

startSqForcedMS( int sqNo )

あらかじめ書き込まれたシーケンスを指定して開始します。前記 startSqMS()と異なりのシーケンスが実行中の場合はそのシーケンスを中断して指示したシーケンスを開始します。

/*----------------------------------------------------------------------------*/
/* Start sequence MS
/* param     : sqNo : Sequence Number
/* return    : non
/*----------------------------------------------------------------------------*/
void startSqForcedMS( int sqNo )
{
	gMScurrent		= sqNo;
	gMSrunningFlag	= true;
	gMScurrentSq	= -1;			// caution! Initial value is not 0.
	gMStimer		= sequenceSV[sqNo][0].period;

	gMSRequestFlag	= false;

	procElementSq();
}

シーケンサはデータ駆動型のため、必要なデータを設定するだけでシーケンス制御を始めます。この処理の中で最初のシーケンス procElementSq() を実行します。

quitSqMS()

実行中のシーケンスがある場合は終了します。

/*----------------------------------------------------------------------------*/
/* Quit sequence MS
/* param     : non
/* return    : non
/*----------------------------------------------------------------------------*/
void quitSqMS()
{
	InitMS();
}

シーケンス制御の初期化を使用します。

InitMS()

このモジュールの初期化です。

/*----------------------------------------------------------------------------*/
/* Initialize MS
/* param     : non
/* return    : non
/*----------------------------------------------------------------------------*/
void InitMS()
{
	gMScurrent		= 0;
	gMScurrentSq	= 0;
	gMSrunningFlag	= false;
	gMStimer		= 0;

	gMSnextSq		= SV_SQ_STOP;
	gMSRequestFlag	= false;
}

制御処理用の各変数を初期化します。データ駆動型ですので変数の初期化がモジュールの初期化となります。この関数は setup() 内から呼ばれます。

procMS()

このモジュールの処理本体です。

/*----------------------------------------------------------------------------*/
/* Core processing MS
/* param     : non
/* return    : non
/*----------------------------------------------------------------------------*/
void procMS()
{
	--gMStimer;
	if (gMStimer > 0) return;		// on running timer

	procElementSq();				// sequecer
}

シーケンサ用の基底時間のタイマをカウントしています。設定したインターバルごとにシーケンスの処理実体 procElementSq() を実行します。

procElementSq()

シーケンスの処理本体。シーケンスデータに基づき、サーボモータへの指示やインターバルの制御を行います。

/*----------------------------------------------------------------------------*/
/* Sequence element processing MS
/* param     : non
/* return    : non
/*----------------------------------------------------------------------------*/
int procElementSq()
{
	int svNo, svPos, svPeriod, nextSq;

	if ( gMSrunningFlag	== false ){
		return -1;
	};

	//******** Switch Command *******
	int fTerminate = false;

	for( int i = 0; i < 30; i++ ){		// 連続動作(=0）は上限30回まで（仮）

		// next command
		++gMScurrentSq;
		svNo		= sequenceSV[gMScurrent][gMScurrentSq].svNo;
		svPos		= sequenceSV[gMScurrent][gMScurrentSq].svPos;
		svPeriod	= sequenceSV[gMScurrent][gMScurrentSq].period;

		//.... terminate sequence ....
		if(svNo > 8){					// 8 以上ならばコマンド
			fTerminate = true;
			break;

		} else if(svPeriod == 0) {		// 直接制御　かつ連続動作
			setSVposition( svNo, svPos );

		//.... continue sequence ....
		} else {						// 直接制御　かつ単発動作
			setSVposition( svNo, svPos );
			break;
		}
	}

	//........ process terminate .......
	if( fTerminate == true ){			// 終端コマンドの場合

		if( gMSRequestFlag == true ){	// リクエストがある場合
			gMSRequestFlag	= false;
			startSqForcedMS( gMSnextSq );	// 次を強制実行

		} else {

			switch ( svNo ) {	
	
				//.... finish sequence ....
			  case SV_CMD_FINISH:
				InitMS();
				break;
				
				//.... restart same sequence / loop ....
			  case SV_CMD_LOOP:	
				nextSq = gMScurrent;
				InitMS();
				startSqMS( nextSq );
				break;
				
				//.... next aequence ....
			  case SV_CMD_NEXT:	
				InitMS();
				startSqMS( svPos );
				
				//.... set speed ....
			  case SV_CMD_SET_SPEED:	
				setSpeedSV( svPos );
				break;
			}
		}	

	} else {
			gMStimer = sequenceSV[gMScurrent][gMScurrentSq].period;
	}
	return -1;
}

シーケンスの時間に関して

参考情報

他のサイトで参考になる情報は以下のとおりです。

このホームページ内

Motion Interpreter（概要）：　サーボモータ制御の最上位、Motion Interrupter の概要です。
Motion Interpreter（操作説明）：　アプリケーションからの操作方法です。（ API ）
Motion Interpreter（コード）：　コードの詳細を説明します。
Motion sequencer（概要）：　サーボモータの協調運動制御の概要です。
Motion sequencer（操作説明）：　シーケンスの開始・停止等の操作方法です。（ API ）
Motion sequencer（コード）：　コードの詳細を説明します。
シーケンスデータ：　シーケンスデータの仕様と作成方法です。
Servomotor Driver（概要）：　サーボモータそのものの制御です。（ API ）
Servomotor Driver（操作説明）：　個々のサーボモータを制御する方法です。
Servomotor Driver（コード）：　コードの詳細を説明します。
サーボモータの制御（コード詳説）：　バックグラウンド処理の分散化の実現方法です。
サーボモータ：　サーボモータの回路です。ハードウエアの説明です。

Motion sequencer（コード）

概 要

モジュールの構造

シーケンスの時間処理

関数の概要

外部関数

startSqMS( int sqNo )

引 数：

分 類：

機 能：

関係する関数：

startSqForcedMS( int sqNo )

引 数：

分 類：

機 能：

関係する関数：

quitSqMS()

引 数：

分 類：

機 能：

関係する関数：

内部関数

InitMS()

引 数：

分 類：

機 能：

関係する関数：

procMS()

引 数：

分 類：

機 能：

関係する関数：

procElementSq()

引 数：

分 類：

機 能：

関係する関数：

データの構造

シーケンスデータ

シーケンスデータと変数の関係

グローバル定義

構造体

グローバル変数

コード

関数間の関係

startSqMS( int sqNo )

startSqForcedMS( int sqNo )

quitSqMS()

InitMS()

procMS()

procElementSq()

シーケンスの時間に関して

参考情報

このホームページ内

他のWebサイト

概要

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：