Beckhoff#Delta ASDA-A3 サーボを動かしてみよう

Beckhoffベッコフオートメーション, EtherCAT, Protocol, TwinCAT3

今回の記事では、Beckhoff TwinCAT3 と Delta 社のサーボシステムをEtherCAT で動かします。記事の中にInterface・Properties・Methods・Function Block・Extendsなどを組み合わせてライブプログラムします。また、TwinCATから簡単なScopeプロジェクトの作成方法も紹介します。

さ、FAを楽しもう。

Reference Video

Beckhoff.Delta ASDA-A3 サーボを動かしてみよう

Reference Link

Project#Beckhoff TwinCAT3 x Siemens S210 Servo Drvie_Part1
Project#Beckhoff TwinCAT3 x Siemens S210 Servo Drvie_Part2
Project#Beckhoff TwinCAT3 x Siemens S210 Servo Drvie_Part3

Servo ASDA-A3?

今回使用するのはDelta社のASDA-A3シリーズのServoになります。

Layout

EtherCAT Mode

ASDA-A3サーボシステムではEtherCATモードを使用でき、2つのEtherCATコネクタ (CN6) のピン配置は同じです。

注意するのはIN コネクタは受信用としてコントローラ（マスタ）または前のサーボドライブに接続でき、 OUT コネクタは出力用として次のサーボドライブにのみ接続できます。また、接続を誤ると通信ができなくなります。

Connection Example

下図では複数のASDA-A3 サーボシステムをEtherCAT ネットワークの接続例です、

Specification

こちらはASDA-A3サーボシステムのEtherCAT仕様になります。

Architecture

サーボドライブのEtherCATアーキテクチャは以下の通りです：

  • Communication profile: このプロトコルには、通信オブジェクト（PDO、SDO、SYNC、緊急オブジェクト）と関連するcommunication object dictionaryが含まれる。
  • DS402:ドライブとモーションコントロール用のデバイスプロファイルで、各モーションモードの動作と、実行に必要なオブジェクトパラメータ設定を定義します。

Synchronous mode

Delta社サーボドライブは2つの同期モードをサポートしています： それはFree ModeとDC-Synchronous modeです。

Free Run mode (Asynchronous)

Free Run modeでは、マスター局とスレーブ局は非同期で動作します。各ステーションは時間を計算する個別のクロックを持っており、マスターとスレーブのクロックは同期していません。マスターとスレーブ間のコマンドとフィードバックの送信は、正確な時間同期ではなく、シーケンシャルな順序に基づいている。例えば、マスターは時刻T1にPDOを送信し、スレーブはSM2イベント後のT2にPDOを受信する。

DC-Synchronous mode (SYNC0 synchronization)

マスタ局とスレーブ局の間には正確な時刻同期があり、マスターは制御プログラムを定期的に実行し、同期クロックに従って決まった時間にPDOパケットを送信するようになります。マスターはスレーブにコマンドを送信し、スレーブからのフィードバックを受信し、スレーブは同期クロックに従って一定時間ごとにPDOデータを受信し、更新するようになります。

PDO mapping configuration

PDOマッピング・オブジェクトは、object dictionaryの中にRxPDOはインデックスOD 1600からOD 1603まで、TxPDOはインデックスOD 1A00からOD 1A03まで割り当てられます。

RxPDOとTxPDOの各グループは、最大8セットの32ビットオブジェクトのPDOデータ更新をサポートできます。

Default PDO mapping configuration

下表はデータ交換のためのEtherCATサーボドライブのデフォルトPDOマッピング設定です。これはEtherCATスレーブのXMLファイルでも定義され、要求に応じてPDOマッピング設定を変更することができます。

Delta ASDA-x3-E rev0.03.xmlでは、PDOコンフィギュレーションの第1グループから第4グループは以下のようになっています：

ASDA-Software Tools

Delta社のServoとCommissioningするために、下記のLinkでASDA-SoftをDownloadしましょう。

https://downloadcenter.deltaww.com/en-US/DownloadCenter?v=1&CID=06&itemID=060201&dataType=8&sort_expr=cdate&sort_dir=DESC

インストールEXEを起動し、Next>で進みます。

ツールのインストール場所を設定し、Installで進みます。

しばらく待ちます…

USB Driverのインストール画面が表示され、InstallをクリックしてServo Driveと通信するDriverをインストールしましょう。

Done!

Implementation1-Commissioning

ASDA-Softwareツールを起動しましょう。

Connect to the Drive

Com portでPCとServoを接続するUSB Portを設定し、AddでServo Driveと繋がりましょう。

Done!

Panel Operation

Digital IO/ Jog Controlをクリックします。

こちらはServoのDIO画面になります。

Set Servo ONをクリックしServo ONコマンドを発行できます。

次は矢印を押したらServoのJOG操作が可能になります。

DIOの意味を変更したい場合は、Edit DI/O ItemのCheckBoxを入れてください。

そうすると、各DIOにDrop-listから割り付けを変更できます。

Auto Tuning

次はAuto Tuningをするため、Tuning>Auto Tuningをクリックします。

今回は単体でやりますので、Motion Command from Driveをクリックしましょう。

次はMotion Profile画面が表示されます。

Servo ONし、Jog Speed・加減速度・そしてMotorを回しPosition1/Position2をクリックし、記録してください。最後はNextをクリックします。

Okで進みます。

しばらく待ちます…

Done!

最後はUpdateボタンをクリックしパラメータを更新しましょう。

Implementation2

Delta Drive Side

パラメータでP1.001を0x000Cに設定してください。こちらはEtherCAT通信する設定になります。

TwinCAT Side

Download ESI File

下記のLinkからDelta者のESI FileをDownloadしてください。

https://downloadcenter.deltaww.com/en-US/DownloadCenter?v=1&CID=06&itemID=060201&dataType=8&sort_expr=cdate&sort_dir=DESC

Install the ESI File

先ほどDownloadしたESI File TwinCAT/3.1/Config/io/EtherCAT に格納しましょう。

次はTwinCATを再起動するか、もしくはExtensions>TwinCAT Device>Reload Device Descriptionをクリックし、ESI Fileを再ロードしましょう。

Connect to TwinCAT Runtime

もしTwinCAT Runtimeがローカルではない場合、接続先を設定する必要があり、SYSTEMを開きます。

Choose Targetをクリックします。

Search(Ethernet)をクリックします。

Route Dialog画面が表示され、TwinCAT RuntimeがインストールされているPCのIPを入力しましょう。そしてPCと接続したいRuntimeを選択します。

Add Routeをクリックします。

PCのUser NameとPasswowrdを入力し、OKで進みます。

Done!

現在別のPCのTwinCATの状態も確認できるようになりました。

Configure EtherCAT Master

EtherCAT Masterを追加するた、I/O>Devices>Add New Itemします。

EtherCAT>EtherCAT Masterを選び、Okで進みます。

こちらはEtherCAT Maseterとして使用したいEthernet Adapterを設定する画面ですが、一旦Noneを選択し、Okで進みます。

Done!EtherCAT Masterが追加されました。

Configure the PCI Bus/Slot

次はEthernet Adapterを設定するため、Compatible Devicesをクリックします。

もし該当するTwinCAT XAEに下図のような画面が表示されれば、TwinCAT RT-ET Ethernet Adaptersをインストールする必要があります。

EtherCAT Masterとして使用したいEthernet Adapterを選び>Installをクリックします。

Installで進みます。

Done!

次はSearchボタンをクリックします。

先程インストールしたEthernet RT Driverを使えるようになりました！

Done!

Scan Network

TwinCATのAuto Scan機能を使用し、ネットワーク内のEtherCAT Slaveを検索します。

DeltaのServoが検出され、該当する軸をNC軸にするかCNC軸にするかを選択できます。

今回はNC-Configurationを選択し、Okで進みます。

Done!Delta者のServo DriveとNC 軸にも追加されました。

Activate Configuration

次はActive Configurationをクリックし、一回プロジェクトをRuntimeにDownloadします。

OKで進みます。

Enc Setting

次はEnc 設定画面でScaling Factorを設定します。

  • Numerator:100
  • Denominator: 16777216

Delta社の取説にも書きましたが、ASDA-A3の分解能は24ビットで、モーター1回転あたり16,777,216パルスを生成します。エンコーダの分解能（17ビット、20ビット、22ビット）に関係なく、Eギア比はASDA-A3サーボドライブの24ビット分解能に従って設定されます。また、E-Gearの比率というパラメーターがあり、その設定が1の場合、モーター1回転につき16,777,216パルスを発生させます。

つまり、現在16777216パルスあたりに(1回転)で、BeckhoffのNC Driveが100mmとして認識します。

Commissioning with Panel

最後はAxis>Axis 1 >Onlineで直接Servo Driveを操作してみます。

Set をクリックします。

Allを選択し>Okで確認します。

Done!現在はServoONになりました。F1/F2/F3/F4などでJog操作やFunction Tabから位置決め操作を試してください！

Implementation3

今度はTwinCATのPLCプログラムからServoを制御します。

Beckhoff Side

Add PLC Project

PLC>右クリック>Add New ItemでPLCプロジェクトを追加します。

Standard PLC Projectを選び>Addでプロジェクトを追加しましょう。

Done!PLCプロジェクトが追加されました。

Add Library

次はMotion制御のライブラリTc2_MC2を追加するため、References>Add libraryします。

Configure Home Method

Delta Servo Driveを選択>Startup>右クリック>Add New Itemで初期パラメーターを設定します。

Startup パラメーターの設定画面が表示されます。

6098はHoming Methodのパラメーターで、そのパラメーターをダブルクリックしてください。

Valueを35にします。35=Direct Homingであり、本来はServoが後退して原点センサーを検知したりなど、様々な手法がありますが、Servoのみしかない環境なので、Direct Homingを使用します。

Done!

Lag Mode

また、最初にTuningしてないとき、Lag Monitorエラーのアラームが頻繁に出ますので、必要に合わせてPosition Lag Monitoring機能を一時的にFalseにしましょう。

Program

次はプログラムを作成します。今回はInterface・Function Block・Extendなど少しOOPのコンセプトを踏みながらプログラムを作成していきます。

Interface

まずはIntefaceの作成から始めます。Intefaceを使用すると、該当するFunction Blockが”なに”をすることだけに集中でき、”どう”やるかのプログラムと分割することができます。また、複数の異なるFunction BlockにもIntefaceだけ共通し、より統一的なプログラムを作成することが可能です。

INTERFACE ITF_SpeedAxis

こちらはServoを速度制御するときの”やること”を定義します。

METHOD METH_Half

こちらのMethodはServoが実行中のJOBを停止できます。

METHOD METH_Half : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
METHOD METH_JogFWBW

こちらのMethodはServoのJOG操作になります。

METHOD METH_JogFWBW : INT
VAR_INPUT
iFw: BOOL;
iBw: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
END_VAR
METHOD METH_PowerON

こちらのMethodはServo ON操作になります。

METHOD METH_PowerON : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
METHOD METH_Reset

こちらのMethodはServoのリセット操作になります。

METHOD METH_Reset : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
METHOD METH_VelocityCmd 

こちらのMethodはServoの定速運転操作になります。

METHOD METH_VelocityCmd : INT
VAR_INPUT
iCmd: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
END_VAR
PROPERTY Prop_CommunicationOK : BOOL

こちらのPROPERTYからServoの通信状態を取得できます。

PROPERTY Prop_Error : BOOL

こちらのPROPERTYからServoのエラー状態を取得できます。

PROPERTY Prop_MCState : ST_Axisstatus

こちらのPROPERTYからServo軸状態を取得できます。

PROPERTY Prop_ReadActualPosition : lReal

こちらのPROPERTYからServoの現在位置を取得できます。

PROPERTY Prop_ReadActualSpeed : lReal

こちらのPROPERTYからServoの現在速度を取得できます。

PROPERTY Prop_Ready : BOOL

こちらのPROPERTYからServoの準備OK信号を取得できます。

PROPERTY Prop_Running : BOOL

こちらのPROPERTYからServoの移動中信号を取得できます。

PROPERTY Prop_Stopped : BOOL

こちらのPROPERTYからServoの停止中信号を取得できます。

DUT

次は構造体を定義します。

eAxisBasicReturnValue 

こちらの列挙からMethodの戻り値になりあす。

{attribute ‘qualified_only’ := ”}
{attribute ‘strict’ := ”}
TYPE eAxisBasicReturnValue :
(
init,
ERROR,
Done,
Busy,
nop,
InVelocity
);
END_TYPE

Function Block

FB_Delta_ASDA_A3_SpeedAxis

こちらのFunction Blockでは先ほど定義したITF_SpeedAxis Interfaceを実装し、BeckhoffのMotion ライブラリに必要な関数をInstance定義します。

FUNCTION_BLOCK FB_Delta_ASDA_A3_SpeedAxis IMPLEMENTS ITF_SpeedAxis
VAR_INPUT
ioAxis: REFERENCE TO AXIS_REF;
END_VAR
VAR
_MC_Power: MC_Power;
_MC_Reset: MC_Reset;
_MC_Half: MC_Halt;
_MC_Jog: MC_Jog;
_MC_MoveVelocity: MC_MoveVelocity;
_MC_ReadActualPosition:MC_ReadActualPosition;
_MC_ReadActualVelocity:MC_ReadActualVelocity;
_MC_ReadStatus:MC_ReadStatus;
END_VAR
METHOD METH_Half

METHOD METH_Half : INT

こちらは実行中のJOBを停止するためのMethodの実装になります。

VAR_INPUT
iCmd: BOOL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR

MC_Halfを使用しServo軸が実行中のJobを実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

R_TRIG(CLK:=iCmd);

_MC_Half(
Axis:=ioAxis
,Execute:=iCmd
,Deceleration:=100.0
);

IF R_TRIG.Q THEN
METH_Half:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_Half.Error THEN
METH_Half:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_Half.Busy THEN
METH_Half:=eAxisBasicReturnValue.Busy;
ELSIF _MC_Half.Done THEN
METH_Half:=eAxisBasicReturnValue.Done;
END_IF
METH_JogFWBW 

こちらはSERVOのJOB操作するMethodの実装になります。

METHOD METH_JogFWBW : INT
VAR_INPUT
iFw: BOOL;
iBw: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
END_VAR
VAR
fw: BOOL;
bw: BOOL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR

MC_Jogを使用しServo軸のJOG操作を実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

fw:=iFw AND NOT iBw;
bw:=iBw AND NOT iFw;

R_TRIG(
CLK:=(iFw AND NOT iBw)
OR (iBw AND NOT iFw)
);

_MC_Jog(
Axis:=ioAxis
,JogForward:=fw
,JogBackwards:=bw
,Velocity:=iVelSetpoint
,Acceleration:=iVelAcc
,Deceleration:=iVelDec
,Mode:=E_JogMode.MC_JOGMODE_CONTINOUS
);

IF R_TRIG.Q THEN
METH_JogFWBW:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_Jog.Error THEN
METH_JogFWBW:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_Jog.Busy THEN
METH_JogFWBW:=eAxisBasicReturnValue.Busy;
END_IF
METH_PowerON 

こちらはSERVO ON操作の実装になります。

METHOD METH_PowerON : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR

MC_Powerを使用しServo軸のServo ON/OFFを実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

R_TRIG(CLK:=iCmd);

_MC_Power(
Axis:=ioAxis
,Enable:=iCmd
,Enable_Positive:=TRUE
,Enable_Negative:=TRUE
);

IF R_TRIG.Q THEN
METH_PowerON:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_Power.Error THEN
METH_PowerON:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_Power.Busy THEN
METH_PowerON:=eAxisBasicReturnValue.Busy;
ELSIF _MC_Power.Enable THEN
METH_PowerON:=eAxisBasicReturnValue.Done;
END_IF
METH_Reset 

こちらはSERVOのリセット操作の実装になります。

METHOD METH_Reset : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR

MC_Resetを使用しServo軸のServo ON/OFFを実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

R_TRIG(CLK:=iCmd);

_MC_Reset(
Axis:=ioAxis
,Execute:=iCmd
);
IF R_TRIG.Q THEN
METH_Reset:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_Reset.Error THEN
METH_Reset:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_Reset.Busy THEN
METH_Reset:=eAxisBasicReturnValue.Busy;
ELSIF _MC_Reset.Done THEN
METH_Reset:=eAxisBasicReturnValue.Done;
END_IF
METH_VelocityCmd 

こちらはSERVOの定速運転の実装になります。

METHOD METH_VelocityCmd : INT
VAR_INPUT
iCmd: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR

MC_MoveVelocityを使用しServo軸のServo ON/OFFを実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

R_TRIG(CLK:=iCmd);

_MC_MoveVelocity(
Axis:=ioAxis
,Execute:=iCmd
,Velocity:=iVelSetpoint
,Acceleration:=iVelAcc
,Deceleration:=iVelDec
);

IF R_TRIG.Q THEN
METH_VelocityCmd:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_MoveVelocity.Error THEN
METH_VelocityCmd:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_MoveVelocity.Busy THEN
METH_VelocityCmd:=eAxisBasicReturnValue.Busy;
ELSIF _MC_MoveVelocity.InVelocity THEN
METH_VelocityCmd:=eAxisBasicReturnValue.InVelocity;
END_IF
PROPERTY Prop_CommunicationOK : BOOL

こちらのPropertyはIodataInvalidの変数から通信OKかを判定します。

Prop_CommunicationOK:=not ioAxis.Status.IoDataInvalid;
PROPERTY Prop_Error : BOOL

こちらのPropertyはErrorの変数から通信OKかを判定します。

Prop_Error:=ioAxis.Status.Error;
PROPERTY Prop_MCState : Tc2_MC2.ST_AxisStatus

こちらのPropertyはMC_ReadStatusを使用し、軸の状態を取得します。

_MC_ReadStatus(
Axis:=ioAxis
,Enable:=TRUE
);

Prop_MCState:=_MC_ReadStatus.Status;
PROPERTY Prop_ReadActualPosition : LREAL

こちらのPropertyはMC_ReadActualPositionを使用し、軸の現在位置を取得します。

_MC_ReadActualPosition(
Axis:=ioAxis
,Enable:=True

);

Prop_ReadActualPosition:=_MC_ReadActualPosition.Position;
PROPERTY Prop_ReadActualSpeed : LREAL

こちらのPropertyはMC_ReadActualVelocityを使用し、軸の現在速度を取得します。

_MC_ReadAutualVelocity(
Axis:=ioAxis
,Enable:=TRUE);

Prop_ReadActualSpeed:=_MC_ReadAutualVelocity.ActualVelocity;
PROPERTY Prop_Ready : BOOL

こちらのPropertyはReadyの変数からServoの準備OK状態を判定します。

Prop_Ready:=ioAxis.Status.Operational;
PROPERTY Prop_Running : BOOL

こちらのPropertyはReadyの変数からServoが移動してるかを判定します。

Prop_Running:=ioAxis.Status.Moving;
PROPERTY Prop_Stoppped : BOOL

こちらのPropertyはReadyの変数からServoが停止してるかを判定します。

Prop_Stoppped:=ioAxis.Status.NotMoving;

Program-Position Axis

先ほどSpeedAxisのFunction Blockを定義しましたが、今回の記事では簡単な位置決めにもやりたいので、新たなFunction Block、Interfaceも作成しますし、元にあるInterfaceやFBから拡張していきましょう。

Interface‐ITF_PositionAxis

新しいInterfaceを定義し、INTERFACE ITF_SpeedAxisから拡張しましょう。

METHOD METH_Home

こちらのMethodはServoをHomeすることができます。

METHOD METH_Home : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
METHOD METH_MoveAbs

こちらのMethodはServoに絶対位置決め操作を実装します。

METHOD METH_MoveAbs : INT
VAR_INPUT
iCmd: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
iPosition: LREAL;
END_VAR
METHOD METH_MoveRelative

こちらのMethodはServoに相対位置決め操作を実装します。

METHOD METH_MoveRelative : INT
VAR_INPUT
iCmd: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
iDistance: LREAL;
END_VAR

Function Block

次は位置決め用のFBを作成しましょう。

FB_Delta_ASDA_A3_PositionAxis

こちらのFB先ほど作ったFB_Delta_ASDA_A3_SpeedAxisを拡張し、またITF_PositionAxisを実装する形になります。また、BeckhoffのMotion ライブラリに必要な関数をInstance定義します。

FUNCTION_BLOCK FB_Delta_ASDA_A3_PositionAxis EXTENDS FB_Delta_ASDA_A3_SpeedAxis IMPLEMENTS ITF_SpeedAxis, ITF_PositionAxis
VAR
_MC_MoveAbsolute: MC_MoveAbsolute;
_MC_MoveRelative: MC_MoveRelative;
_MC_Home: MC_Home;
END_VAR
METH_Home 

こちらはSERVOのHOME操作するMethodの実装になります。

METHOD METH_Home : INT
VAR_INPUT
iCmd: BOOL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR

MC_Homeを使用しServo軸のHome操作を実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

R_TRIG(CLK:=iCmd);

_MC_Home(
Axis:=ioAxis
,Execute:=iCmd
,Position:=0.0
,HomingMode:=MC_HomingMode.MC_Direct
);

IF R_TRIG.Q THEN
METH_Home:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_Home.Error THEN
METH_Home:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_Home.Busy THEN
METH_Home:=eAxisBasicReturnValue.Busy;
ELSIF _MC_Home.Done THEN
METH_Home:=eAxisBasicReturnValue.Done;
END_IF
METH_MoveAbs 

MC_MoveAbsoluteを使用しServo軸の絶対位置決めを実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

METHOD METH_MoveAbs : INT
VAR_INPUT
iCmd: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
iPosition: LREAL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR
R_TRIG(CLK:=iCmd);

_MC_MoveAbsolute(
Axis:=ioAxis
,Execute:=iCmd
,Position:=iPosition
,Velocity:=iVelSetpoint
,Acceleration:=iVelAcc
,Deceleration:=iVelDec
);

IF R_TRIG.Q THEN
METH_MoveAbs:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_MoveAbsolute.Error THEN
METH_MoveAbs:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_MoveAbsolute.Busy THEN
METH_MoveAbs:=eAxisBasicReturnValue.Busy;
ELSIF _MC_MoveAbsolute.Done THEN
METH_MoveAbs:=eAxisBasicReturnValue.Done;
END_IF
METH_MoveRelative 

MC_MoveRelativeを使用しServo軸の相対位置決めを実行させ、またFunction Blockの実行状態・結果に沿って適切な戻り値を返します。

METHOD METH_MoveRelative : INT
VAR_INPUT
iCmd: BOOL;
iVelSetpoint: LREAL;
iVelAcc: LREAL;
iVelDec: LREAL;
iDistance: LREAL;
END_VAR
VAR_INST
R_TRIG:R_TRIG;
END_VAR
R_TRIG(CLK:=iCmd);

_MC_MoveRelative(
Axis:=ioAxis
,Execute:=iCmd
,Distance:=iDistance
,Velocity:=iVelSetpoint
,Acceleration:=iVelAcc
,Deceleration:=iVelDec
);

IF R_TRIG.Q THEN
METH_MoveRelative:=eAxisBasicReturnValue.nop;
END_IF;

IF _MC_MoveRelative.Error THEN
METH_MoveRelative:=eAxisBasicReturnValue.ERROR;
ELSIF _MC_MoveRelative.Busy THEN
METH_MoveRelative:=eAxisBasicReturnValue.Busy;
ELSIF _MC_MoveRelative.Done THEN
METH_MoveRelative:=eAxisBasicReturnValue.Done;
END_IF

GVL_Axis

Global Variables Listを追加し、AXIS_REF変数を定義します。

{attribute ‘qualified_only’}
VAR_GLOBAL
axies:ARRAY[0..9]OF AXIS_REF;
END_VAR

Program-pServo

こちらはメインプログラムになり、先程作成したFunction Blockを呼び出し、CASE文を使用し絶対位置決めと相対位置決めの繰り返しをします。

PROGRAM pServo
VAR
Axis1: FB_Delta_ASDA_A3_PositionAxis;

Status:ST_AxisStatus;

ActVelocity,ActPosition:LREAL;

bFw: BOOL;
bBw: BOOL;
bHome: BOOL;
bMoveAbs: BOOL;
bMoveRelative: BOOL;
bCommunicationOK: BOOL;
bError: BOOL;
bRunning: BOOL;
bStopped: BOOL;
bPowerON: BOOL;
bReset: BOOL;
RetValue: INT;
RetValueAbs: INT;
RetValueRet:INT;
bReady: BOOL;
bRun: BOOL;
bStop: BOOL;

fSetValue: REAL;
f32AbsPos: REAL;
f32AbsVel: REAL:=100.0;
f32AbsAcc: REAL:=100.0;
f32AbsDec: REAL:=100.0;
f32RelativePos: REAL;
f32RelativeVel: REAL:=100.0;
f32RelativeAcc: REAL:=100.0;
f32RelativeDec: REAL:=100.0;

//
iStep:INT;
TON:TON;
iStart:BOOL;
iStop:BOOL;
myCounter:INT;

END_VAR
//Axis of Delta Servo
Axis1(ioAxis:=GVL_Axis.axies[0]);

//Power ON the Axis
RetValue:= Axis1.METH_PowerON(
iCmd:=bPowerON
);
//Get the Status
//Ready
bReady:=Axis1.Prop_Ready;
//Running
bRunning:=Axis1.Prop_Running;
//Error
bError:=Axis1.Prop_Error;
//Stopped
bStopped:=Axis1.Prop_Stopped;
//CommunicationOK
bCommunicationOK:=Axis1.Prop_CommunicationOK;
//Status
Status:=Axis1.Prop_MCState;
//ActSpeed
ActVelocity:=Axis1.Prop_ReadActualSpeed;
//ActPostion
ActPosition:=Axis1.Prop_ReadActualPosition;

//Reset
RetValue:=Axis1.METH_Reset(
iCmd:=bReset
);
IF RetValue = eAxisBasicReturnValue.Busy THEN
bReset:=FALSE;
END_IF
//Halt
RetValue:=Axis1.METH_Half(
iCmd:=bStop
);
IF Status.Stopping OR bError THEN
bStop:=FALSE;
END_IF
//Home
RetValue:=Axis1.METH_Home(
iCmd:=bHome
);

//MoveVelocity
RetValue:=Axis1.METH_VelocityCmd(
iCmd:=bRun
,iVelSetpoint:=fSetValue
,iVelAcc:=100.0
,iVelDec:=100.0
);

//Jog
RetValue:=Axis1.METH_JogFWBW(
iFw:=bFw AND NOT bBw
,iBw:=bBw AND NOT bFw
,iVelSetpoint:=fSetValue
,iVelAcc:=100.0
,iVelDec:=100.0
);

//MoveAbsolute
RetValueAbs:=Axis1.METH_MoveAbs(
iCmd:=bMoveAbs
,iVelSetpoint:=f32AbsVel
,iVelAcc:=f32AbsAcc
,iVelDec:=f32AbsDec
,iPosition:=f32AbsPos
);

//MoveRelative
RetValueRet:=Axis1.METH_MoveRelative(
iCmd:=bMoveRelative
,iVelSetpoint:=f32RelativeVel
,iVelAcc:=f32RelativeAcc
,iVelDec:=f32RelativeDec
,iDistance:=f32RelativePos
);
//
IF Status.Moving OR bError THEN
bRun:=FALSE;
bMoveAbs:=FALSE;
bMoveRelative:=FALSE;

END_IF


CASE iStep OF

0:
iStop:=FALSE;
IF iStart AND iStep =0 THEN
iStep:=10;

iStart:=FALSE;
myCounter:=0;
END_IF

10:
bMoveAbs:=TRUE;
f32AbsPos:=1000.0;
IF GVL_Axis.axies[0].Status.Moving THEN
iStep:=20;
bMoveAbs:=FALSE;
END_IF
20:
IF RetValueAbs = eAxisBasicReturnValue.Done THEN
iStep:=30;
END_IF
30:
bMoveRelative:=TRUE;
f32RelativePos:=-200.0;
IF GVL_Axis.axies[0].Status.Moving THEN
iStep:=40;
bMoveRelative:=FALSE;
END_IF
40:
IF RetValueRet = eAxisBasicReturnValue.Done THEN
IF NOT iStop THEN
IF myCounter >3 THEN
iStep:=10;
myCounter:=0;
ELSE
myCounter:=myCounter+1;
iStep:=30;
END_IF;
ELSE
iStep:=0;
END_IF;
END_IF
END_CASE

Program-Main

最後はMAINプログラムで先程作ったpServo POUを呼び出します。

PROGRAM MAIN
VAR
END_VAR
pServo();

Link to PLC

Global Variables Listで定義したAXIS_REFとAXISを接続するため、Axis>Settings>Link to PLCをクリックします。

Done!

Active Configuration

Active Configurationをクリックし、一回プロジェクトをRuntimeにDownloadします。

OKで進みます。

OKでTwinCAT RuntimeをRun Modeにソフトします。

Login

LoginでプログラムをDownloadします。

Yesで進みましょう。

Start

最後はStartボタンでRuntimeのプログラムを実行します。

Add Scope Project

最後はScopeプロジェクトを追加し、TwinCAT Runtimeの変数をリアルタイムでMonitorしましょう。Solution>右クリック>Add>New Projectをクリックします。

Measurement Wizardを選び>Nextで進みます。

Measurementのプロジェクト名を入力し、Createで進みます。

今回はTC3 Scope Viewを使用します。

Monitorしたい変数を設定するため、Variablesをクリックします。

Monitorしたい変数があるTarget システムを選択しましょう。

今回はPLC Runtimeの変数をMonitorしたいので、Port 851を選び、Nextで進みます。

PLC runtimeにある変数を一覧できます。

次は監視する変数を選び、Nextで進みます。

Time-based Chartを使用します。

CreateでChartを作成しましょう。

Done!

変数をレコードしたい場合、Start Recordボタンをクリックしてください。

Done!!

レコードを中止したい場合、Stop Recordをクリックしましょう。

OKでレコードを停止していきましょう。

Result

こちらの動画から動作確認できます。

こちらのLinkから記事のプロジェクトをDownloadしてください。

https://github.com/soup01Threes/TwinCAT3/blob/main/TwinCAT_TestwithDelatServo.tnzip

Footer_Basic

Please Support some devices for my blog

Amazon Gift List

Find ME

Twitter:＠3threes2
Email:soup01threes*gmail.com (* to ＠）
YoutubeChannel:https://www.youtube.com/channel/UCQ3CHGAIXZAbeOC_9mjQiWQ

シェアする

  • このエントリーをはてなブックマークに追加

フォローする

gomamenotes