こちらは新しいシリーズでFATEKのSC シリーズSERVOを使用し様々な記事を展開していきます。第４話はBeckhoff TwinCAT3とEtherCAT経由で仮想軸とMC_GearInを使用し、同期制御を行います。

さ、FAを楽しもう。

Reference Link

http://soup01.com/ja/category/fatek/sc-series/

Beckhoff#Fatek SC3 サーボを制御しよう

Beckhoff#MC_MoveContinuousAbsoluteとMC_MoveContinuousRelativeを使ってみよう

MC_GearIn

機能ブロックMC_GearInは、直線的なマスタースレーブ結合（歯車結合）を有効にします。このFBは、分子/分母形式で固定された歯車比を受け入れます。

注意するのはスレーブ軸は停止時のみ、マスター軸と連結できます。このFBでは、移動中のマスター軸への同期は不可能で、その場合はMC_GearInVeloまたはMC_GearInPosを使用してください。

そして、スレーブ軸は機能ブロックMC_GearOutでデカップリングできます。スレーブが移動中にデカップリングされた場合、その速度を維持し、MC_StopまたはMC_Haltを使用して停止させることができます。

あるいは、動的に変更可能なギア比を備えた機能ブロックMC_GearInDynが利用可能です。

VAR_INPUT

変数名	データ・タイプ	説明
Execute	BOOL	立ち上がりエッジでコマンドが実行されます。
RatioNumerator	LREAL	ギア比の分子。分母が 1 の場合、分子に浮動小数点値としてギア比を直接指定することもできます。
RatioDenominator	UINT	ギア比の分母。
Acceleration	LREAL	加速（≧0）未実装。
Deceleration	LREAL	減速（≧0）未実装。
Jerk	LREAL	ジャーク（≧0）未実装。
BufferMode	MC_BufferMode	未実装
Options	ST_GearInOptions	未実装

VAR_IN_OUT

変数名	データ・タイプ	説明
Master	AXIS_REF	マスター軸の構造体。
Slave	AXIS_REF	スレーブ軸の構造体。

VAR_OUTPUT

変数名	データ・タイプ	説明
InGear	BOOL	カップリング（ギア結合）が正常に完了した場合に TRUE。
Busy	BOOL	「Execute」によりコマンドが開始されてから、処理中の TRUEです。Busy が FALSE の場合、このFBは次の指令を受け付け可能です。同時に、「InGear」「CommandAborted」「Error」のいずれか一つがセットされます。
Active	BOOL	コマンドが実行中であることを示します。（現在は Active = Busy）
CommandAborted	BOOL	コマンドが完全に実行できなかった場合に TRUE。同時コマンド実行などにより、結合処理中に軸がデカップル（切り離し）された可能性があります。
Error	BOOL	エラーが発生した場合に TRUE。
ErrorID	UDINT	Error 出力が TRUE の場合、そのエラー番号を示します。

MC_GearOut

FB MC_GearOutは、マスタースレーブ結合を無効にします。

注意するのは、スレーブ軸が動作中に切り離された場合、軸は自動的に停止せず、一定の速度に達すると無限に動きになり、その場合はMC_Halt または MC_Stop ファンクションブロックで軸を停止できます。

VAR_INPUT

変数名	データ・タイプ	説明
Execute	BOOL	立ち上がりエッジでコマンドが実行されます。
Options	ST_GearOutOptions	現在は未実装。

VAR_IN_OUT

変数名	データ・タイプ	説明
Slave	AXIS_REF	スレーブ軸の構造体。

VAR_OUTPUT

変数名	データ・タイプ	説明
Done	BOOL	軸のデカップル（ギア解除）が正常に完了した場合に TRUE。
Busy	BOOL	コマンド開始後、処理中の間 TRUE。Busy が FALSE の場合、このファンクションブロックは新しい指令を受け付け可能であり、同時に「Done」または「Error」のいずれかがセットされます。
Error	BOOL	エラーが発生した場合に TRUE。
ErrorID	UDINT	Error 出力が TRUE の場合、そのエラー番号を示します。

Implementation

こちらは今回の構成です。Part2と同じハードウェア構成ですが、TwinCAT3 Runtime上で１つの仮想軸を追加していきます。

eSystem

eSystem という ENUM（列挙型）を定義し、軸の数を固定します。

{attribute ‘qualified_only’}
{attribute ‘strict’}
{attribute ‘to_string’}
TYPE eSystem :
(
TotalAxis := 1
);
END_TYPE

GVL

GVLにAXIS_REFを格納する配列を定義し、上限が eSystem.TotalAxis で決めます。

{attribute ‘qualified_only’}
VAR_GLOBAL
arrAxis :ARRAY[0..eSystem.TotalAxis]OF AXIS_REF;
END_VAR

プログラム

次はFatekのSC3 Servoを動かすためのプログラムを作成します。こちらはPLC OPEN モーションのFBをします。それはBeckhoff TwinCAT3に標準実装されています。

VAR

この変数定義は、PLCopen Motion の各種FBを軸数に依存しない形で一括管理するためのものです。

eSystem.TotalAxis を基準に、

各軸の Power / Reset / Home / Move などの制御FB
移動指令や原点復帰などの軸別操作フラグ
位置・速度といった軸別パラメータ

をすべて配列として定義しています。

これにより、軸数が増減してもENUM 定義を変更するだけでプログラム全体が追従し、

可読性と拡張性を両立した多軸サーボ制御が可能になります。

また、GearIn / GearOut はマスター・スレーブの1組を扱うため単体FBとして管理し、

同期制御と個別軸制御を明確に分離しています。

PROGRAM pServo
VAR
MC_Power:ARRAY[0..eSystem.TotalAxis] OF MC_Power;
MC_Reset:ARRAY[0..eSystem.TotalAxis] OF MC_Reset;
MC_SetPosition:ARRAY[0..eSystem.TotalAxis] OF MC_SetPosition;
MC_Home:ARRAY[0..eSystem.TotalAxis] OF MC_Home;
MC_ReadActualPosition:ARRAY[0..eSystem.TotalAxis] OF MC_ReadActualPosition;
MC_ReadActualVelocity:ARRAY[0..eSystem.TotalAxis] OF MC_ReadActualVelocity;
MC_ReadAxisError:ARRAY[0..eSystem.TotalAxis] OF MC_ReadAxisError;
MC_ReadStatus:ARRAY[0..eSystem.TotalAxis] OF MC_ReadStatus;
MC_MoveAbsolute:ARRAY[0..eSystem.TotalAxis] OF MC_MoveAbsolute;
MC_MoveRelative:ARRAY[0..eSystem.TotalAxis] OF MC_MoveRelative;
MC_GearIn :MC_GearIn;
MC_GearOut :MC_GearOut;
END_VAR
VAR
xEnable :BOOL;
xEnabled :BOOL;
xReset :BOOL;
iCounter :INT;
arrxMoveRelative
,arrxMoveAbsolute
,arrxHome
,arrxSetPosition
:ARRAY[0..eSystem.TotalAxis]OF BOOL;
arrMoveAbosolutePos
,arrMoveAbosoluteVel
,arrMoveRelativePos
,arrMoveRelativeVel
:ARRAY[0..eSystem.TotalAxis]OF LREAL;

xMC_GearinExe
,xMC_GearinOK
,xMC_GearinActive
,xMC_GearinError :BOOL;

xMC_GearOutExe
,xMC_GearOutOK
,xMC_GearOutActive
,xMC_GearOutError :BOOL;

END_VAR

Program

このプログラムは、複数軸のサーボを FOR ループで一括制御するためのものです。eSystem.TotalAxis を基準に、すべての軸に対して同じ処理を順番に行うことで、軸数が増減してもコードを大きく書き換えずに対応できる構成になっています。

また、ループ外では、

全軸の Enable 状態からシステム全体の有効状態を判定
Master / Slave を指定した GearIn による同期制御
GearOut による同期解除

を行い、個別軸制御と同期制御を分離しています。

FOR iCounter:=0 TO eSystem.TotalAxis DO

IF arrMoveAbosoluteVel[iCounter] <= 0.0 THEN
arrMoveAbosoluteVel[iCounter] :=100.0;
END_IF
IF arrMoveRelativeVel[iCounter] <= 0.0 THEN
arrMoveRelativeVel[iCounter] :=100.0;
END_IF

MC_Power[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Enable:=xEnable
,Enable_Positive:=TRUE
,Enable_Negative:=TRUE
);
MC_Home[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Execute:=arrxHome[iCounter]
);
IF MC_Home[iCounter].Busy THEN
arrxHome[iCounter]:=FALSE;
END_IF;
MC_SetPosition[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Execute:=arrxSetPosition[iCounter]
,Position:=0.0
);
MC_Reset[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Execute:=xReset
);
MC_ReadActualPosition[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Enable:=TRUE
);
MC_ReadActualVelocity[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Enable:=TRUE
);
MC_ReadAxisError[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Enable:=TRUE
);
MC_ReadStatus[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Enable:=TRUE
);

MC_MoveAbsolute[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Execute:=arrxMoveAbsolute[iCounter]
,Position:=arrMoveAbosolutePos[iCounter]
,Velocity:=arrMoveAbosoluteVel[iCounter]
,Acceleration:=100.0
,Deceleration:=100.0
,Jerk:=10.0
);
IF MC_MoveAbsolute[iCounter].Busy THEN
arrxMoveAbsolute[iCounter]:=FALSE;
END_IF;

MC_MoveRelative[iCounter](
Axis:=GVL.arrAxis[iCounter]
,Execute:=arrxMoveRelative[iCounter]
,Distance:=arrMoveRelativePos[iCounter]
,Velocity:=arrMoveRelativeVel[iCounter]
,Acceleration:=100.0
,Deceleration:=100.0
,Jerk:=10.0
);

IF MC_MoveRelative[iCounter].Busy THEN
arrxMoveRelative[iCounter]:=FALSE;
END_IF;

END_FOR;

xEnabled:=MC_ReadStatus[0].Enable AND MC_ReadStatus[1].Enable;

MC_GearIn(
Master:=GVL.arrAxis[1]
,Slave:=GVL.arrAxis[0]
,Execute:=xMC_GearinExe
,RatioDenominator:=1
,RatioNumerator:=1
,Acceleration:=100.0
,Deceleration:=100.0
,Jerk:=10.0
,InGear=>xMC_GearinOK
,Error=>xMC_GearinError
,Active=>xMC_GearinActive
,Error=>xMC_GearinError
);

MC_GearOut(
Slave:=GVL.arrAxis[0]
,Execute:=xMC_GearOutExe
);