Servomotor driver（コード）

サーボモータそのものを制御します。電子回路と接点になる部分です。サーボモータ制御の最下位層になります。

◀ この記事の前に：　Servomotor driver（操作説明）
▶ この記事の次に：　サーボモータの制御（コード詳説）

概要

サーボモータの制御は時間経過に伴う処理です。時間経過による処理は専用のタイマ割り込みにより直接行います。

モジュールの構造

サーボモータ制御は経時的な分散制御で、個々のサーボモータは順番に処理と出力を行います。
制御時間は専用のタイマ割り込みを使用します。タイマ割り込み回数の低減と処理時間の均一化を行うため、可変割り込み方式を使用しています。詳細は以下をご覧ください。

関数の概要

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

外部関数

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

setSVposition( int svNo, int position )

引数：

・int　svNo：　対象とするサーボモータ
・int　position：　ポジション（角度）

分類：

API（対象は Motion sequencer、他）

機能：

指定したサーボモータを指示した角度まで動かします。

関係する関数：

上位関数：　Motion sequencer、アプリケーション
下位関数：　なし
この関数のコードはこちらです。

setSVpowerControl( int switchPower )

引数：

・int　switchPower：　サーボモータの電源 ON / OFF
　SV_POWER_ON ：　サーボモータの電源 ON
　SV_POWER_OFF：　サーボモータの電源 OFF

分類：

API（対象は Servomotor Interpreter、他）

機能：

サーボモータの電源を ON / OFF する。

関係する関数：

上位関数：　Servomotor interpreter、アプリケーション
下位関数：　なし

setSpeedSV( int speed )

引数：

・int　speed ：　設定する速度

分類：

API（対象は Motion sequencer、他）

機能：

サーボモータを制御する基底速度を設定します。

関係する関数：

上位関数：　Motion sequencer、アプリケーション
下位関数：　なし
この関数のコードはこちらです。

内部関数

外部から直接アクセスできずモジュール（特定のスコープ内）で使用する関数です。

setSVwidthFoaced( int svNo, int position )

引数：

・int　svNo：　対象とするサーボモータ
・int　position：　ポジション（角度）

分類：

初期化（ setup() / setupSV() ）

機能：

サーボモータ制御の初期値を設定して、既定の位置にします。

関係する関数：

上位関数：　setupSV()
下位関数：　なし
この関数のコードはこちらです。

setupSV()

引数：

なし

分類：

内部関数（初期化）　

機能：

初期化を行います。

関係する関数：

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

setupSVtimerInt()

引数：

なし

分類：

内部関数（初期化）　

機能：

サーボモータ制御の専用タイマの初期化を行います。内部で割り込み処理の関数 intSVtimer()を結び付けます。

関係する関数：

上位関数：　setup()
下位関数：　（なし）/ タイマ割り込み（ TIM1 ）
この関数のコードはこちらです。

intSVtimer()

引数：

なし

分類：

内部関数（タイマ割り込み / TIM2 ）　

機能：

サーボモータ制御の処理本体。タイマ割り込みにより自律的に制御を行います。制御はサーボモータ制御定数に設定した内容に基づき行います。（サーボモータ制御定数が実質的な指示となります。）

関係する関数：

上位関数：　（なし）/ タイマ割り込み（ TIM1 ）
下位関数：　（なし）
この関数のコードはこちらです。

convertSVangle( int svNo, int position )

引数：

・int　svNo：　対象とするサーボモータ
・int　position：　ポジション（角度）

戻り値：

・int　：　ポジションに対応するサーボモータの角度

分類：

内部関数　

機能：

Qumcum Lab. の頭部と四肢の位置は、Qumcum Lab. としての位置（角度）として表現して扱っています。この場合の位置を「ポジション」と呼びます。（例：腕を上に上げるは左右の腕どちらも「上に上げる」という表現になります。）実際はサーボモータの取付方向などで、サーボモータとしての角度とポジションは異なります。（腕は左右で取り付け位置が反転するため左右逆になります）
この関数はポジションとサーボモータの実際の角度を換算します。

関係する関数：

上位関数：　setSVposition(), setSVwidthFoaced()
下位関数：　（なし）
この関数のコードはこちらです。

データの構造

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

グローバル定義

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

定義

サーボモータの仕様に基づく基本的な定義

//******** Servomotors ********

//.... Moter specification ....
// PWM width : 50Hz ( 20msec. )
// Range of movement : 0 - 180 deg. / 0.5 - 2.4msec

const int		SV_ON		= HIGH;		// Output
const int 		SV_OFF		= LOW;
const int 		SV_TON_MIN	= 55;		//  50 X 10usec = 0.5msec.
const int 		SV_TON_MAX	= 240;		// 240 X 10usec = 2.4msec.
// Pulse count per degree
const	float 	SV_1DEG 	= ( (float)SV_TON_MAX - (float)SV_TON_MIN ) / 180;

//.... Number of motors ....
const int		SVN_MAX 	= 8;		// include dummy

//.... Timer Interruppt for PWM ....
// Clock : 80MHz
// Prescaler : 1/800 100kHz ( Timer clock )
//
const uint32_t	SV_INIT_INTERVAL	= 2000;	// 1000us = 1ms <<<<<<<<<<<<<<<<<<<<未使用
const int 		SV_PRESCALER		= 800;	// prescalor
const int 		SV_TM				= 2000;	// period motor control
const int 		SV_TMDIV			= SV_TM / SVN_MAX;
// divied on each motor

//.... Error code ....
const int SV_ERR_OVERANGLE = 999;

サーボモータ制御は Body MCUで行います。サーボモータの省電力制御のみ Peripheral MCU で行います。

//.... Power saving control of servo motors ....
#define SV_POWER_ON       1
#define SV_POWER_OFF      0

名称	型	値	説明
SV_ON	int	HIGH	ON時のサーボモータの出力（正論理）
SV_OFF	int	LOW	OFF時のサーボモータの出力（正論理）
SV_TON_MIN	int	55	ONデューティの最小幅時のカウント値（タイマクロックが　10μsの場合） ● クロックを変更した場合、ここも変更する。
SV_TON_MAX	int	240	ONデューティの最大幅時のカウント値（タイマクロックが　10μsの場合） ● クロックを変更した場合、ここも変更する。
SV_1DEG	float	計算値	1°あたりの ONデューティのカウント値 ( (float)SV_TON_MAX – (float)SV_TON_MIN ) / 180
SVN_MAX	int	8	サーボモータの搭載数（軸数）ダミーひとつを含む
SV_POWER_ON	define	1	サーボモータの電源 ON
SV_POWER_OFF	define	0	サーボモータの電源 OFF

タイマ割り込みに関係する定義

名称	型	値	説明
TM_SV_INIT_INTERVAL	uint32_t	2000
TM_SV_PRESCALER	int	800	タイマのプリスケーラ値システムクロック周波数 80MHzに対して、1/800で 10μsのクロックを作る
SV_TM	int	2000	サーボモータの1サイクル分の周期（カウント） ● 10μsの場合、( 2000 × 10μs ) / 1000 = 20 ms
SV_TMDIV	int	計算値	サーボモータひとつあたりの分割時間 SV_TM / SVN_MAX = 2.5 ms

サーボモータの部位に関する定義

ここで定義する番号は、Qumcum Lab. の四肢と頭部のサーボモータの位置に対応づけられています。
Qumcum Lab. を組み立てるときに、四肢と頭部の番号を指定してコネクタと対応づけます。そのときの番号になります。なお Qumcum Lab. ではサーボモータが動かす部位を部位の名称としています。（例：右脚の付け根にあるサーボモータは右足の方向を左右に動かしますので「右足」です。）
詳しくはこちらをご覧ください。

//.... Definitions Extremities ....
// Perspective on Robot
const int SV_DM = 0;	// dummy
const int SV_HD = 4;	// Head
const int SV_AR = 1;	// Arm Right
const int SV_AL = 7;	// Arm Left
const int SV_FR = 2;	// Foot Right
const int SV_FL = 6;	// Foot Left
const int SV_LR = 3;	// Leg Right
const int SV_LL = 5;	// Leg Left

名称	型	値	説明
SV_DM	int	0	（ダミー）
SV_HD	int	4	Head / 頭部
SV_AR	int	1	Arm Right / 右腕
SV_AL	int	7	Arm Left / 左腕
SV_FR	int	2	Foot Right / 右足
SV_FL	int	6	Foot Left / 左足
SV_LR	int	3	Leg Right / 右脚
SV_LL	int	5	Leg Left / 左脚

サーボモータの制御定数（ピン割り当てを含む）

struct SV_PHYSICS svConverts[]

各サーボモータの制御に必要な基礎的な定数です。構造体 SV_PHYSICS（後記）の1次元配列です。
この並び（添字）が前記のサーボモータとなるため、各サーボモータの制御定数は配列要素の指定で参照できます。
ピンの割り当ては Body MCUの Leafony Busのポートです。
例）svConverts[SV_HD].svMaxPos：　頭部の角度の既定値

struct SV_PHYSICS svConverts[]={
	{ 0,     0, 180,  1,   0, 0,  +10, -10 },	// 0:SV_DM, Mdummy
	{ PB12,  0, 180,  1,   0, 0, +180,   0 },	// 1:SV_AR, Arm Right
	{ PA9,  50, 130,  1,  90, 0,  +40, -40 },	// 2:SV_LR, Leg Right
//	{ PA10, 60, 120,  1,  90, 0,  +30, -30 },	// 3:SV_FR, Foot Right	//<<<< !!! actually PC0 for SMPLE
	{ PC1,  60, 120,  1,  90, 0,  +30, -30 },	// 3:SV_FR, Foot Right	//<<<< !!! actually PC0 for EXTEND
	{ PC0,   0, 180,  1,  90, 0,  +90, -90 },	// 4:SV_HD, Head
	{ PC7,  60, 120, -1,  90, 0,  +30, -30 },	// 5:SV_FL, Foot Left
	{ PB5,  50, 130, -1,  90, 0,  +40, -40 },	// 6:SV_LL, Leg Left
	{ PB4,   0, 180, -1, 180, 0, +180,   0 } 	// 7:SV_AL, Arm Left
};

	svPin	svLowestDeg	svHightestDeg	svFactorA	svFactorB	svMaxPos	svMinPos
SV_DM	0	0	180	1	0	+10	-10
SV_AR	PB12	0	180	1	0	+180	0
SV_FR	PA9	50	130	1	90	+40	-40
SV_LR	PC0	60	120	1	90	+30	-30
SV_HD	PC1	0	180	1	90	+90	-90
SV_LL	PC7	60	120	-1	90	+30	-30
SV_FL	PB3	50	130	-1	90	+40	-40
SV_AL	PB4	0	180	-1	180	+180	0

省電力制御用のピン

省電力制御は Body MCUで行います。

//.... Power saving control of servo motors ....
#define PIN_PS_SV       PB0

名称	STM32 – Pin.	用途
PIN_PS_SV	PB0	サーボモータの省電力制御（電源の ON / OFF ）

構造体

各サーボモータの制御定数

struct SV_PHYSICS {
	uint8_t	svPin;		// Outout port
	int	svLowestDeg;	// Lowest degrees
	int	svHightestDeg;	// Hightest degrees
	int	svFactorA;		// Convert factor A
	int	svFactorB;		// Convert factor B
	int	svDefaultPos;	// Default position
	int svMaxPos;		// Maximun position
	int svMinPos;		// Minimum position
};

要素名	型	値	単位	説明
svPin	uint8_t	–	モータ角度	サーボモータの接続ポート（ピン）
svLowestDeg	int	–	モータ角度	サーボモータの最小動作角度
svHightestDeg	int	–	モータ角度	サーボモータの最大動作角度
svFactorA	int	–	モータ角度	ポジションと角度の換算用係数 A
svFactorB	int	–	モータ角度	ポジションと角度の換算用係数 B
svDefaultPos	int	–	ポジション	既定の角度
svMaxPos	int	–	ポジション	角度の最大値　* 現在未使用
svMinPos	int	–	ポジション	角度の最小値　* 現在未使用

モータ角度：　サーボモータとしての実際の角度です。

グローバル変数

各サーボモータの制御変数

//.... Current control parameters ....
struct SV_CURRENT {
	int svPos;				// Current Position
	int	svTon;				// Current ON width
	int	svToff;				// Current OFF width

	float svDegStart;		// Start angle
	float svDegCurrent;		// Current angle
	float svDegTarget;		// Target angle
};

要素名	型	値	説明
svPos	int	–	現在の角度（ポジション）
svTon	int	–	現在の ONデューティ
svToff	int	–	現在の OFFデューティ
svDegStart	float	–	変更前（操作開始時）の角度
svDegCurrent	float	–	現在（操作中）の角度
svDegTarget;	float	–	変更後（目標）の角度

サーボモータの補正値

//.... Current control parameters ....
struct SV_PARAMETER {
	int svOffset;		// Current Position
};

要素名	型	値	説明
svOffset	int	–	補正値

サーボモータの制御変数（共通）

/*============================================================================*/
// Motion driver / MD

//.... Control Variable ....
// Servomotor 
int		ggSVno		= 0;		// cyclic control
int		ggSVout		= SV_OFF;	// output control

float	gSVspeedDev	= 1.0; 		// angular velocity

変数名	型	初期値	説明
gSVno	int	0	現在の制御対象となっているサーボモータ　intSVtimer()内
gSVout	int	SV_OFF	現在の制御対象となっているサーボモータの出力状態　intSVtimer()内
gSVspeedDev	float	1.0	サーボモータの角速度サーボモータ制御の１サイクルあたりの増減分　intSVtimer()内詳細はこちらをご覧ください。

gSVspeedDev のアクセス箇所
参照：　intSVtimer()　タイマ割り込みによるサーボモータのリアルタイム制御の実体
設定：　setSpeedSV()
setSpeedSV() は、Motion sequencer の procElementSq()、Motion interpreter の procMIsetSpeedSV() で設定します。（サンプルプログラムでは）

サーボモータの制御変数（各サーボモータ）

struct SV_CURRENT svCurrents[]

各サーボモータの制御に必要な基礎的な定数です。構造体 SV_CURRENT（前記）の1次元配列です。
この並び（添字）が前記のサーボモータとなるため、各サーボモータの制御定数は配列要素の指定で参照できます。
例）svConverts[SV_HD].svMaxPos：　頭部の角度の既定値

svConverts[] の初期値

struct SV_CURRENT svCurrents[]={
	{ 0, 0, 0, 0, 0, 0 },		// 0:SV_DM, Mdummy
	{ 0, 0, 0, 0, 0, 0 },		// 1:SV_AR, Arm Right
	{ 0, 0, 0, 0, 0, 0 },		// 2:SV_LR, Leg Right
	{ 0, 0, 0, 0, 0, 0 },		// 3:SV_FR, Foot Right
	{ 0, 0, 0, 0, 0, 0 },		// 4:SV_HD, Head
	{ 0, 0, 0, 0, 0, 0 },		// 5:SV_FL, Foot Left
	{ 0, 0, 0, 0, 0, 0 },		// 6:SV_LL, Leg Left
	{ 0, 0, 0, 0, 0, 0 }		// 7:SV_AL, Arm Left
};

	svPos	svTon	svToff	svDegStart	svDegCurrent	svDegTarget
SV_DM	0	0	0	0	0	0
SV_AR	0	0	0	0	0	0
SV_FR	0	0	0	0	0	0
SV_LR	0	0	0	0	0	0
SV_HD	0	0	0	0	0	0
SV_LL	0	0	0	0	0	0
SV_FL	0	0	0	0	0	0
SV_AL	0	0	0	0	0	0

サーボモータの補正値

struct SV_PARAMETER svParameters[]

各サーボモータに物理的に固有なズレを補正するためのパラメータ（補正値）です。
サーボモータは個々の部品や、取り付けにばらつきがあります。このばらつきを調整することが目的です。そのため個々の Qumcum Lab.によりこの値は異なります。

この値は個々のにより異なるため、サンプルプログラムではスマートフォン（リモコン）から設定できます。また、補正値は MCUのフラッシュメモリに保存して電源 OFF時にも補正値は保存します。

例）svParameters[SV_LR].svOffset：　右脚の角度補正値

svParameters[] の初期値（値は例です。個々の Qumcum Lab.により異なります。）
角度はポジションの表記です。● サーボモータの実角度に変更の予定あり。

struct SV_PARAMETER svParameters[]={
	{  0 },	// 0:SV_DM, Mdummy
	{  4 },	// 1:SV_AR, Arm Right
	{ -4 },	// 2:SV_FR, Foot Right	
	{ -4 },	// 3:SV_LR, Leg Right	
	{ -4 },	// 4:SV_HD, Head
	{ -4 },	// 5:SV_LL, Leg Left	
	{ -4 },	// 6:SV_FL, Foot Left	
	{  0 }	// 7:SV_AL, Arm Left
};

サーボモータ	svOffset
SV_DM	0
SV_AR	4
SV_FR	-4
SV_LR	-4
SV_HD	-4
SV_LL	-4
SV_FL	-4
SV_AL	0

制御変数と関数の関係

主要な制御変数（定数を含む）と関数の関係は以下のとおりです。制御定数はおもに仕様に関係するデータ、制御変数は個々のサーボモータの制御に関係するデータです。

コード

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

オブジェクト

ハードウエアタイマを使用します。STM32L4 は用途別に10本を超えるタイマを搭載しています。そのうち汎用タイマであるTIM1（タイマ１）を使用します。


//---------------------------------------------------------------------
// object
//---------------------------------------------------------------------

//------------------------------
// Timer
//------------------------------
HardwareTimer *SVtimer = new HardwareTimer( TIM1 );

関数間の関係

関数の関係は以下のとおりです。
割り込みに関しては以下もご覧ください。
可変タイマ割り込み

setSVpowerControl( int switchPower )

サーボモータの電源を ON / OFF する。

setSVpowerControl( int switchPower )  {

	digitalWrite( PIN_PS_SV, switchPower );

}

setSVposition( int svNo, int position )

指定したサーボモータを指示した角度まで動かします。

/*----------------------------------------------------------------------------*/
// Set position servomoter / Servomotor
//
// - convert from position to angle.
// - check range of movement.
//
// param     : int svNo :
//						 int position :
// return    : angle / Error code
//
// reference : svCnverts / Physical parameters
//
// param     : non
// return    : non
/*----------------------------------------------------------------------------*/
int setSVposition( int svNo, int position )
{
	position += svParameters[svNo].svOffset;			// debug 補正は本来、angleで行いたい

	int	angle = convertSVangle( svNo, position );

	if( angle != SV_ERR_OVERANGLE ){
		svCurrents[svNo].svDegTarget = angle;
	}

	return angle;
}

サーボモータの制御はバックグラウンド処理で自律的に行っています。

Motion sequencer やアプリケーションなどからサーボモータの角度を変更する場合に setSVposition() を使用します。この関数は変更したい角度（目標角度）をサーボモータの制御変数 svCurrents[] に書き込みます。

サーボモータ制御の実体は、専用のタイマ割り込みで連続的に処理を行っています。その中でサーボモータの制御変数 svCurrents[] を参照して、目標角度が現在の角度と異なる場合に、目標角度になるように intSVtimer() が自律的にサーボモータのリアルタイム制御処理を行います。

要点が二つあります。ひとつはデータを介した制御を行うデータ駆動型であることです。もうひとつはバックグラウンド処理とフォアグランド処理の連携方法です。このしくみによりアプリケーション側とサーボモータがそれぞれ独立して動作することができます。また、プログラミングで相互依存しないため作りやすくなることと、バグの発生を抑えることができます。

setSpeedSV( int speed )

サーボモータを制御する基底速度を設定します。

/*----------------------------------------------------------------------------*/
/* Core processing MS
/* param     : non
/* return    : non
/*----------------------------------------------------------------------------*/
void setSpeedSV( int speed )
{
	//---- update PWM width ----
	// caution
	// Pause Interrupt
	noInterrupts();		// pause
	gSVspeedDev = speed;
	interrupts();		// resume
}

setSVwidthFoaced( int svNo, int position )

サーボモータ制御の初期値を設定して、既定の位置にします。

/*----------------------------------------------------------------------------*/
/* Core processing MS
/* param     : non
/* return    : non
/*----------------------------------------------------------------------------*/
int setSVwidthFoaced( int svNo, int position )
{
	position += svParameters[svNo].svOffset;			// debug 補正は本来、angleで行いたい
	int	angle = convertSVangle( svNo, position );

	if( angle != SV_ERR_OVERANGLE ){
		svCurrents[svNo].svDegTarget	=	angle ;
		int	Ton = (int)(SV_1DEG * angle + SV_TON_MIN);
		int	Toff = SV_TMDIV - Ton;

	//---- update PWM width ----
	// caution
	// Pause Interrupt
	noInterrupts();					// pause
		svCurrents[svNo].svPos  = position;
		svCurrents[svNo].svTon  = Ton;
		svCurrents[svNo].svToff = Toff;
	
		svCurrents[svNo].svDegStart		=	angle;
		svCurrents[svNo].svDegCurrent	=	angle;
		svCurrents[svNo].svDegTarget	=	angle;
	interrupts();					 // resume
	}

	return angle;
}

setupSV()

初期化を行います。

/*----------------------------------------------------------------------------*/
/* Initialize Servomotors
/* param     : -
/* return    : -
/*----------------------------------------------------------------------------*/
void setupSV()
{
	int		pin;
	int		position;

	// * caution *
	// SV_DM(1) is dummy
	for (int i=1; i < SVN_MAX; i++){

		//.... Initialize Ports ....
		pin = svConverts[i].svPin;
		pinMode( pin, OUTPUT );			// need to "OUTPUT" <<< next is OUTPUT_OPEN_DRAIN
		digitalWrite( pin, SV_OFF );

		//.... Initialize Servomotor Position ...
		position = svConverts[i].svDefaultPos;
		setSVwidthFoaced( i, position );

	}
		//.... Dummy ...
		position = svConverts[0].svDefaultPos;
		setSVwidthFoaced( 0, position );
}

Servomotor driver の初期化は二つあります。setupSV() と setupSVtimerInt()です。
setupSV() 内では、ハードウエアに関係するポートの設定と、初期角度の設定です。

setupSVtimerInt()

サーボモータ制御の専用タイマの初期化を行う。内部で割り込み処理の関数 intSVtimer()を結び付けます。

/*----------------------------------------------------------------------------*/
/* Initialize control servomotors timer
/* param     : -
/* return    : -
/*----------------------------------------------------------------------------*/
void setupSVtimerInt()
{
	//.... Initialize Timer and Interrupt ....
	SVtimer->pause();
	SVtimer->setPrescaleFactor( TM_SV_PRESCALER );
	SVtimer->setOverflow( 50 );
	SVtimer->attachInterrupt( intSVtimer );
	SVtimer->refresh();
	SVtimer->resume();
}

サーボモータはリアルタイム制御です。専用のタイマ割り込みを使用します。（ TIM1 ）
この中でタイマ割り込み処理 intSVtimer() をタイマ割り込みに対応づけます。

intSVtimer()

/*----------------------------------------------------------------------------*/
// Timer interrupt / Control Servomoters
// param     : -
// return    : void
/*----------------------------------------------------------------------------*/
void intSVtimer()
{
	int	width;
	int pin;

	//---- Pause Interrupt ----
	SVtimer->pause();

		//******** Detect edge and Set next timing ********

		//........ Edge of ON -> OFF ........
		if( gSVout == SV_ON ){
			gSVout = SV_OFF;
			width  = svCurrents[gSVno].svToff;

		//........ Edge of OFF -> ON ........
		} else {

			// Next Servomotor
			gSVno++;
			if( gSVno >= SVN_MAX ) gSVno = 0;

			float svDegStart	= svCurrents[gSVno].svDegStart;		// Start angle
			float svDegCurrent	= svCurrents[gSVno].svDegCurrent;	// Current angle
			float svDegTarget	= svCurrents[gSVno].svDegTarget;	// Target angle
		
			if ( svDegStart == svDegTarget ) {			//	stop
				svDegStart			= svDegTarget;
				svDegCurrent		= svDegTarget;

			} else if ( svDegStart < svDegTarget ) {	// increse angle
					svDegCurrent += gSVspeedDev;
					if ( svDegCurrent > svDegTarget ) {	//	stop
						svDegStart		= svDegTarget;
						svDegCurrent	= svDegTarget;
					}
			
			} else if ( svDegStart > svDegTarget ) {	// decrese angle
					svDegCurrent -= gSVspeedDev;
					if ( svDegCurrent < svDegTarget ) {	//	stop
						svDegStart		= svDegTarget;
						svDegCurrent	= svDegTarget;
					}
			}

			svCurrents[gSVno].svDegStart	= svDegStart;		// Start angle
			svCurrents[gSVno].svDegCurrent	= svDegCurrent;		// Current angle
			svCurrents[gSVno].svDegTarget	= svDegTarget;		// Target angle
			svCurrents[gSVno].svTon			= (int)(SV_1DEG * svDegCurrent + SV_TON_MIN);
			svCurrents[gSVno].svToff		= SV_TMDIV - svCurrents[gSVno].svTon;

			gSVout = SV_ON;
			width = svCurrents[gSVno].svTon;
		}	

		//******** Output to Servomotor ********

		//........ Output ........
		if( gSVno != SV_DM ){
			pin = svConverts[gSVno].svPin;
			digitalWrite( pin, gSVout );

		//........ Dummy / no output ........
		}else{
		}

		//******** Restart Timer ********
		SVtimer->setOverflow( width );
		SVtimer->refresh();

	//---- Resume Interrupt ----
	SVtimer->resume();

}

サーボモータ制御は分散処理でサーボモータを制御します。タイマ割り込みがあるたびに順番に各サーボモータの処理を行い出力をおこないます。また、可変割り込み方式なので割り込みのたびに次の割り込みまでの間隔をタイマにオーバフロー値として設定します。

処理の最初にタイマのカウントを停止して、処理が終了したときにタイマのカウントを再開します。
制御はサーボモータの信号線の１サイクルがひとつの処理になっており、出力が OFFから ONになるとき（ ONデューティの始め）に各設定値を計算します。出力が ONから OFFになるとき（ OFFデューティ）には信号線の変更のみを行います。

処理は現在の角度と目標角度を比較して、その大小により角速度に応じた増減を行います。現在の角度は実測値でなくプログラムが認識している値です。角度はサーボモータとしての実際の角度です。（ポジションではありません。）

現在の角度 = 目標角度：　何もしない
現在の角度 < 目標角度：　増分を現在の角度に加算する
現在の角度 > 目標角度：　減分を現在の角度に減算する

増分と減分は角速度に応じた増減値 gSVspeedDevです。
現在の角度と目標角度が異なる場合、この処理を行うごとに現在の角度は目標角度に近づきます。同じ角度になった時点で増減がなくなり角度が一定となりこれが目標角度です。角度の算出後はサーボモータを制御するための ONデューティの時間に変換します。計算式は以下のとおりです。

ONデューティ = SV_1DEG × svDegCurrent + SV_TON_MIN
OFFデューティ = SV_TMDIV – svCurrents[gSVno].svTon

・SV_1DEG ：　１°あたりの ONデューティ時間
・svDegCurrent：　現在の角度（ °（度））
・SV_TON_MIN：　ONデューティの最小値（ 0.5 ms ）
・SV_TMDIV：　サーボモータ１つあたりの制御幅（ 20 ms ）
・svCurrents[gSVno].svTon：　ONデューティ時間

以下を設定してタイマ割り込み処理から戻ります。

出力：　ポート、ON / OFF
タイマ割り込み：　次の割り込みまでの間隔
制御変数：　現在の角度の更新（開始角度、目標角度の更新 / 目標角度になった場合）

convertSVangle( int svNo, int position )

ポジションとして表現した角度から、サーボモータの実際の角度に換算します。

/*----------------------------------------------------------------------------*/
// Convert from Position to Angle / Servomotor
//
// - convert from position to angle.
// - check range of movement.
//
// param     : int svNo :
//						 int position :
// return    : angle / Error code
//
// reference : svCnverts / Physical parameters
//
/*----------------------------------------------------------------------------*/
int convertSVangle( int svNo, int position )
{
	int		fA 	 = svConverts[svNo].svFactorA;
	int		fB 	 = svConverts[svNo].svFactorB;
	int		posL = svConverts[svNo].svLowestDeg;
	int		posH = svConverts[svNo].svHightestDeg;
	
	int		angle = fB + fA * position;

	if ( posL > angle ) angle = SV_ERR_OVERANGLE;
	if ( posH < angle ) angle = SV_ERR_OVERANGLE;

	return angle;
}

アプリケーションなどはサーボモータの角度を「ポジション」という角度の表現で扱います。実際のサーボモータの角度とは異なります。この関数はポジションをサーボモータの実際の角度に変換します。同時に可動範囲を超えないように角度を制限します。

角度の変換

頭部はポジションの表現で、正面を向いている時が 0°、右を向いているときが -90°、左を向いているときが +90°になります。これをサーボモータの実際の角度にすると、右を向いているときが 0°、正面を向いているときが 90°、左を向いているときが180°になります。
ポジションにオフセット値を加算すると変換できます。

回転方向の変換

左または右腕が下向きのときはポジションの表現で 0°、前向き（前へならえの位置）で 90°、上向き（万歳の向き）で 180°です。これをサーボモータの実際の角度にすると、左腕と右腕のサーボモータの取付方向は正反対です。そのため、右腕のサーボモータの実際の動作方向は左腕と正反対になります。
ポジションに -1を掛けると変換できます。

変換式

前記より変換式は以下のとおりです。

θa = fa × θp + fb

・θp：　ポジション表記の角度　
・θa：　サーボモータの実際の角度
・fa：　回転方向の変換
・fb：　角度のオフセット値

回転角度の制限値

サーボモータを設置する部位により回転できる範囲（可動範囲）が決まっています。その範囲を越えると破損や故障につながります。そのため、各サーボモータに制限値を設定しています。ポジションから角度に変換した後で、角度が制限内に入っていることの確認を行います。制限内に入っていない場合はエラーコード（ SV_ERR_OVERANGLE ）を戻り値として返します。

係数と制限値の参照

角度の変更と制限値はサーボモータの制御定数 avConverts[]にあります。

回転方向の変換を行うための係数 fa：　svConverts[svNo].svFactorA
角度の変更のためのオフセット値 fb：　svConverts[svNo].svFactorB
回転角度の制限値（下限）：　svConverts[svNo].svLowestDeg
回転角度の制限値（上限）：　svConverts[svNo].svHightestDeg

SvNo は対象とするサーボモータです。（ SV_HD, SV_AL, SV_AR, ･･･）

参考情報

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

このホームページ内

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

Servomotor driver（ コード ）

概 要

モジュールの構造

関数の概要

外部関数

setSVposition( int svNo, int position )

引 数：

分 類：

機 能：

関係する関数：

setSVpowerControl( int switchPower )

引 数：

分 類：

機 能：

関係する関数：

setSpeedSV( int speed )

引 数：

分 類：

機 能：

関係する関数：

内部関数

setSVwidthFoaced( int svNo, int position )

引 数：

分 類：

機 能：

関係する関数：

setupSV()

引 数：

分 類：

機 能：

関係する関数：

setupSVtimerInt()

引 数：

分 類：

機 能：

関係する関数：

intSVtimer()

引 数：

分 類：

機 能：

関係する関数：

convertSVangle( int svNo, int position )

引 数：

戻り値：

分 類：

機 能：

関係する関数：

データの構造

グローバル定義

定 義

サーボモータの仕様に基づく基本的な定義

タイマ割り込みに関係する定義

サーボモータの部位に関する定義

サーボモータの制御定数（ ピン割り当てを含む ）

省電力制御用のピン

構造体

各サーボモータの制御定数

グローバル変数

各サーボモータの制御変数

サーボモータの補正値

サーボモータの制御変数（ 共通 ）

サーボモータの制御変数（ 各サーボモータ ）

サーボモータの補正値

制御変数と関数の関係

コード

オブジェクト

関数間の関係

参考情報

このホームページ内

他のWebサイト

Servomotor driver（コード）

概要

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

引数：

分類：

機能：

定義

サーボモータの制御定数（ピン割り当てを含む）

サーボモータの制御変数（共通）

サーボモータの制御変数（各サーボモータ）