Beckhoff#TwinCAT3 Play with Xplanar And TF5430_Part1

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

TF5430 TwinCAT3 Planar Motion パッケージはXPlanarを効率的に制御しあなたのXPlanarアプリケーションを実現できます。TF5430 はTF5890の一部です。

TwinCATではXPlanarをPlanar Mover Objectとして表現し、実機のMoverとリンクします。

Beckhoff TwinCAT3の便利なInterfaceのおかけでPLCから直接TwinCAT環境からXplanarを構築・プログラミング・設定・通信・制御を一貫管理できます。

この記事では全てあなたのPCだけでXplanarをSimulationすることでき、特別なハードウェアはいりません。どうぞよろしくお願いします。


Install packages

TF5430ではXPlanarを効率的に制御するには必要なライブラリが入っており、そのTF5430はTF5890 TwinCAT3 XPlanarに含まれています。すべてのFunction blockがライブラリ Tc3_Mc3PlanarMotion入っており、なおかつTc3_physicsと組み合わせて使用するイメージになります。

TF5400

https://www.beckhoff.com/en-en/products/motion/xplanar-planar-motor-system/xplanar-software/tf5430.html#

TF5400のインストールfileをクリックします。

言語を選びます。ここで英語にします。

インストールが起動します。

Next>します。

ライセンス同意し、Nextします。

User Nameなど入れて、Next>します。

もしTwinCATバージョンが古い場合、Warningが出てきます。

そのときBeckhoffのHPから最新バージョンをDownloadしてください。

もしTwinCATのバージョンがOKであればWarningがでなくなります。

しばらく待ちます。

これでインストール完成です。

再起動してください。

TF5890

https://www.beckhoff.com/en-en/products/motion/xplanar-planar-motor-system/xplanar-software/tf5890.html#

TF5890のインストールfileをクリックします。

言語を選びます。ここで英語にします。

Next>します。

ライセンスに同意し、Next>します。

User Nameなど入れて、Next>します。

Installで進みます。

しばらく待ちます…

これでインストール完成です。

再起動してください。


Basic Setup

Motion ProjectとPLC Project２つ大きな部分に分けています。

Add Motion Project

MOTION>右クリック>Add New Itemします。

MC Configurationを選び>OKします。

MC Projectが追加されました。

Add XPlanar

MC Project>右クリック>Add New Itemします。

Panar Mover を選び>Okします。

それでXPlanarが追加されました。

Simulation Setup

先程追加したMoverをクリックします。

そのようなセットアップ画面が出てきます。

Parameter(Init)Tabを開きます。下にあるShow Hidden ParametersのCheckboxを入れます。

そうするとオレンジ色のパラメータが表示されます。

Simulation ModeのValueのTRUEに変更します。これで準備OKです。


ADD PLC

次はPLCを追加します。PLC>Add New Itemします。

Standard PLC Projectを追加します。

Add Reference

ライブラリを追加します。Reference>右クリック>Add libraryします。

Tc3_physicsを追加します。

同じくTc3_Mc3PlanarMotionを追加します。

Add MC_PlanarMover

Planar Obejctを定義します。GVLs>Add>Global Variable Listを作成します。

MC_PlanarMover変数を定義します。

Build>Build Solutionでコンパイルをかけます。

Link to Mover

Mover1をクリックします。

Link to PLCのボタンをクリックします。

定義したMoverを選び>OKします。

API

XPlanarを制御するにはTc3_PhysicsとTc3_Mc3PlanarMotionライブラリを使用する必要があります。先程のStepもすでにReference を追加しましたので大丈夫です。ここからは今回の記事で使用するFunction block・DUTを紹介します。

DUT

EPlanarObjectType

PlanarのObject 種類を定義します。

NameTypeValueDescription
InvalidUNIT0部品がReadyしてないか、接続してないか
NoneUNIT301Planarありません
MoverUNIT302Planar Mover
TrackUNIT303Planar Track
EnvironmentUNIT304Planar Environment

PlanarObjectInfo

Planarの独自のIDを定義するDUTです。

NameTypeDescription
ObjectTypeEPlanarObjectTypeObjectの種類
IdUDINTObjectのID

MC_PLANAR_STATE

部品の状態を表現するDUTです。

NameTypeValueDescription
InvalidUNIT0部品がReadyしてないか、接続してないか
DisabledUNIT1部品が無効されてた
EnablingUNIT2部品が有効されてる
EnabledUNIT3部品が有効されてた
DisablingUNIT4部品が無効されてる
ErrorUNIT5部品はエラー状態
ErrorPendingUNIT6部品が正常状態からエラー状態に移行してる
ResettingUNIT7部品はエラーからリセット中

MoverVector

6個の数値が格納されてるVectorで6 Coordinatesを表現するDUTです。

加速度や位置や速度などで使用されています。

NameTypeDescription
xLREALx座標
yLREALy座標
zLREALz座標
aLREALa座標
bLREALb座標
cLREALc座標

MoverBusy

PlanarMoverの座標Busy状態を表現するDUTです。

NameTypeDescription
busyMoverBITTrue=PlanarMover がBusy中
busyXYCBITTrue=主座標がBusy中
busyXBITTrue=x座標がBusy中
busyYBITTrue=y座標がBusy中
busyZBITTrue=z座標がBusy中
busyABITTrue=a座標がBusy中
busyBBITTrue=b座標がBusy中
busyCBITTrue=c座標がBusy中

ST_MoveToPositionOptions

Planar MoverのMoveToPositionコマンドにあるOptionsです。

NameTypeDefaultDescription
useOrientationBOOLTRUETRUE=Orientationにも合わせて移動する

ST_ExternalSetpointGenerationOptions

Planar moverのExternalSetpointGenerationコマンドにあるOptionsです。

NameTypeDefaultDescription
modeMC_EXTERNAL_SET_POSITION_MODEMC_EXTERNAL_SET_POSITION_MODE.Absolute相対移動や絶対移動を設定できます。

MC_EXTERNAL_SET_POSITION_MODE

External setpoint generation modeを表現するDUTです。

NameTypeInitialDescription
InvalidUINT0無効なデータ
AbsoluteUINT1Planar moverはExternal setpointだけに従います（絶対位置決め）
RelativeUINT2Planar moveは相対位置決めになります
NoneUINT3External setpoint生成なし

PositionXYC

XYC位置のFunction blockです。注意するのは直接そのFBを呼び出さないことです。必ずそのObjectのMethodだけを使用してください。

SetValuesXY

VAR_INPUT

NameTypeDescription
xLREALx位置
yLREALy位置

Example

VAR
  position  : PositionXYC;
  x : LREAL := 20.0;
  y : LREAL := 5.3;
VAR_END

position.SetValuesXY(x, y);


MC_PlanarMover

PlanarMover Objectは実際Planarを制御するObjectになります。注意するのは直接そのFBを呼び出さないことです。必ずそのObjectのMethodだけを使用してください。

MoveToPosition

直接PlanarMoverに目標値命令を発行します。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
constraint     IPlcDynamicConstraintPlanarの移動に対する制約
options        ST_MoveToPositionOptionsPlanarの移動オプション

VAR_INOUT

NameTypeDescription
targetPosition PositionXYC目標値

Enable

XPlanarを有効にする。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback

Update

Objectの内部状態を更新するためのMethodです。毎サイクルも呼ぶ必要があります。

Disable

XPlanarを無効にする。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback

Reset

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback

MoveZ

Z移動コマンドを発行します。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
targetPosition    LREALPlanarのTarget位置
constraint       IPlcDynamicConstraintPlanarの移動に対する制約

MoveA

A移動コマンドを発行します。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
targetPosition    LREALPlanarのTarget位置
constraint       IPlcDynamicConstraintPlanarの移動に対する制約

MoveB

B移動コマンドを発行します。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
targetPosition    LREALPlanarのTarget位置
constraint       IPlcDynamicConstraintPlanarの移動に対する制約

MoveC

Cの移動コマンドを発行します。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
targetPosition    LREALPlanarのTarget位置
constraint       IPlcDynamicConstraintPlanarの移動に対する制約

StartExternalSetpointGeneration

External setpoint生成が始まります。プログラムでPLCサイクルごとにSetpointを更新する必要があります。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
options   ST_ExternalSetpointGenerationOptionsXplanar移動のOptions

StopExternalSetpointGeneration

External setpoint生成を停止します。SetExternalSetpoint MethodがCallされた同じPLCサイクルにCallする必要があります。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback

SetExternalSetpoint

Planar Moverに external setpointを設定します。プログラムでSetpoin生成をするとき、PLCサイクルごとにそのMethodを呼び出してください。

VAR_INPUT

NameTypeDescription
setPositionMoverVectorThe position setpoint is sent to the Planar Mover.
setVelocityMoverVectorThe velocity setpoint is sent to the Planar Mover.
setAccelerationMoverVectorThe acceleration setpoint is sent to the Planar Mover.

MC_PlanarFeedback

コマンドのFeedback情報を取得します。そのMC_PlanarFeedback Instanceはコマンド発行のFunction Blockのパラメータとして渡しておけばOKです。

VAR_OUTPUT

NameTypeDescription
ActiveBOOLコマンドはXplanarに受けられ、これから実行する
BusyBOOLコマンド実行中
DoneBOOLコマンドエラーなしで実行された
AbortedBOOLコマンド実行中に中断された
ErrorBOOLコマンド実行中にエラーあり
ErrorIdBOOLエラー発生するときのID
ObjectInfoPlannarObjectInfoいまのFeedbackされてるObjectの情報

Update

Objectの内部状態を更新するためのMethodです。毎サイクルも呼ぶ必要があります。


Flow

このFlow図はPlanar Mover・Plannar Track・Planar Groupの状態遷移図です。すべての美品では以下の7つの状態があります。それは:

  • Enabling
  • Enabled
  • Disabling
  • Disabled
  • Resetting
  • ErrorPending
  • Error.

Enabling

Enableコマンドを実行したあとに部品がEnabing状態に移行します。この状態でDisableコマンドを発行すると部品の状態をDisabling状態に変更することできます。

Enabled

部品がEnabled状態に移行したら、Userが該当する部品の機能をすべて使用できます。その状態ではDisableコマンドを発行し部品をDisabling状態に移行できます。

Disabling

Disbaleコマンドを実行したあとに部品がDisabling状態に移行します。この状態でEnableコマンドを発行すると部品の状態をEnabling状態に変更することできます。

Disabled

システムが起動するたびに部品はそのDisbaled状態になります。そのときEnableコマンドを発行するとEnablingに移行し、最後にEnabled状態に変わります。

Disabled状態では部品の機能は使用できません。


Resetting

部品のエラーからリセットしている状態を示しています。もしエラーが解消されたら、Enabled状態に変わり、エラーがまだKeepしてるであればDisabled状態に変わります。

ErrorPending

部品がエラー発生するとその状態に移行し、最後にError状態に変わります。(Resetting以外)


Error

エラー状態では該当する部品がエラー発生してます。Resetコマンドを発行するとResetting状態に変わります。

Planar mover command diagram

Planar moverは6種類のコマンドモードがあります。

  • OnTrack
  • LeavingTrack
  • JoiningTrack
  • ExternalSetpointGeneration
  • CRotationFreeMovement/-
  • FreeMovement

FreeMovement

今回は初めてのXPlanarシリーズで自分も勉強を始めたばかりなので、一番簡単な動作モードから始めます。それはFreemovementです。

Moverを有効化しEnabled状態に移行した後に自動的にこのコマンドModeに変わります。そのModeに入るとMoveToPositionを使用し自由にMoverを動かすことができます。、

もちろんこのModeからExternalSetpointGenerationやJoiningTrackに移行することも可能です。MoveC コマンドを使用するとCRotationFreeMovementモードに移行できるなど、自由度が高いです。


ExternalSetpointGeneration 

次はExternalSetpointGeneration です。そのExternalSetpointGeneration modeを使用することにより現在のPositionを足すOffsetのような形で相対位置決めを実装できます。

Implementation-1 Move it!

では最初にまずFreeMovement モードからMoveToPositionを使用しPlanar moverを移動してみましょう。

GVL

Global Variable ListにMover ObjectとコマンドのFeedback用のObjectを定義します。

VAR_GLOBAL
mover:MC_PlanarMover;
commandFeedback:MC_PlanarFeedback;
END_VAR

MAIN

流れとしてはXplanar初期化>Xplanar有効>MoveToPositionコマンド発行>戻すの無限Loopになります。MoveToPositionのTarget値はARRAY[0..5]OF PositionXYCで5まで行ったらまた0に戻る。

VAR

PROGRAM MAIN
VAR

    　iStep :UDINT;
moverStatus :MC_PLANAR_STATE;
counter :INT;
TargetPositions :ARRAY[0..5]OF PositionXYC;
i :INT;
bGo :BOOL;
 bReset :BOOL;
 bDisable :BOOL;

END_VAR

VAR CONSTANT
cStepXplanarInit :UDINT :=0;
cStepXplanarEnable :UDINT :=10;
cStepMoveToPosition :UDINT :=20;
cStepDisable :UDINT :=99;
cStepReset :UDINT :=98;
END_VAR

Code

//Sending Data to RoboDK by OPCUA
GVL2RoboDK.xplanar1.pos.ActPos:=GVL.mover.MCTOPLC.ACT.ActPos;

//Target Positions Init
TargetPositions[0].SetValuesXY(0,200);
TargetPositions[1].SetValuesXY(200,-200);
TargetPositions[2].SetValuesXY(100,20);
TargetPositions[3].SetValuesXY(40,0);
TargetPositions[4].SetValuesXY(110,60);
TargetPositions[5].SetValuesXY(50,200);

//xPlanar Status
moverStatus:=GVL.mover.MCTOPLC.STD.State;

//Disable
IF bDisable THEN
GVL.mover.Disable(commandFeedback:=GVL.commandFeedback);
iStep:=cStepDisable;
END_IF
//Reset
IF bReset THEN
GVL.mover.Reset(commandFeedback:=GVL.commandFeedback);
iStep:=cStepReset;
END_IF

CASE iStep OF

cStepXplanarInit:
GVL. mover.Enable(commandFeedback:=GVL.commandFeedback);
    iStep := cStepXplanarEnable;
 
cStepXplanarEnable:
IF GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled THEN
  iStep := cStepMoveToPosition;
END_IF
counter:=0;
bGo:=TRUE;
cStepMoveToPosition:
IF bGo THEN
GVL.mover.MoveToPosition(
commandFeedback:=GVL.commandFeedback
,targetPosition:=TargetPositions[counter]
,constraint:=0
,options:=0
);
bGo:=FALSE;
END_IF;
IF GVL.commandFeedback.Done THEN
counter:=counter+1;
bGo:=TRUE;
IF counter > 5 THEN
iStep:=cStepXplanarEnable;
END_IF
END_IF

cStepDisable:
IF moverStatus= MC_PLANAR_STATE.Disabled THEN
bDisable:=FALSE;
iStep:=cStepXplanarInit;
END_IF
cStepReset:
IF moverStatus= MC_PLANAR_STATE.Enabled THEN
bReset:=FALSE;
iStep:=cStepXplanarInit;
END_IF

END_CASE

//Update
GVL.mover.Update();
GVL.commandFeedback.Update();


Result

GVL.mover.MCTOPLC.ACT.ActPosにxyzabc座標が移動してるとわかります。ちなみにMCTOPLC.ACTは現在データになります。

Simulation With RoboDK

本来ならTF5890 Xplanar Configuratorを使用したいのですが、何回やってもTCCOM Not FoundとかNo Servicesとかのエラーが発生し解決できなく、代わりにRoboDKで可視化しました。でもRoboDKならあとのロボット追加も便利なので、こっちのほうがよいかもしれません。残りはMoverのCAD FileをDownloadできる場所を探すとか…

Reference Link

RoboDKとBeckhoff TwinCAT3の連携した記事↓

Beckhoff#TwinCAT x RoboDK x Pi4 x Coral USB ML – Image Classification

Add a “Plate” Object

まずはPlateを追加します。このPlateは実際Xplanarとサイズが違いますが、Simulationで練習と勉強するのにあまり細かいこときにしないでください。

RoboDKを起動し、地球マークをクリックします。

RoboDKの公式ライブラリHPに飛んできます。TypeをObject選び>RoboDK RDK Plateを選びます。

Downloadします。

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

先Downloadした.sldを開けばOKです。


Rename the plate

先ほど追加したPlateの名前を変更します。

右クリック>Renameでxplanar1と名前をつけてください。そのxplanar1という名前はPython Scriptで使用しますので。

Add Python Script

ToolbarsにあるAdd Python ProgramをクリックしPython Scriptを追加します。

画面ではこのようにProg1のScriptが追加されました。

Program

そのScriptをクリックし>Edit Python scriptします。

こちらは今回のScriptになります。

RoboDKからOPCUA Clientを立ち上げ、Beckhoff TwinCATからXplanarの6軸座標XYZABCを取得しPlateの現在座標を更新し続けます。難しいことはやっていません。

from robodk import robolink    # RoboDK API
from robodk import robomath    # Robot toolbox
from opcua import Client,ua
RDK = robolink.Robolink()

from robodk import *      # RoboDK API
from robolink import *    # Robot toolbox


XPLANAR=’xplanar1′
ENDPOINT=”opc.tcp://DESKTOP-7FE1JP2:4840″
client=Client(ENDPOINT)
NODES=’ns=4;s=GVL2RoboDK.xplanar1.pos.ActPos’

s

# Program example:
xplanar = RDK.Item(XPLANAR)
if xplanar.Valid():
    print(‘Item selected: ‘ + xplanar.Name())
    print(‘Item posistion: ‘ + repr(xplanar.Pose()))


client.connect()


print(‘Connected to OPCUA server..’)

NodeFromServer=client.get_node(NODES)
client.load_type_definitions()
#
while True:
    Actpos=NodeFromServer.get_value()

    #
　    　　　　　　　CurrentPos=xyzrpw_2_pose([Actpos.x,Actpos.y,Actpos.z,Actpos.a,Actpos.b,Actpos.c])
    xplanar.setPoseAbs(CurrentPos)

client.disconnect()

TwinCAT Side

RoboDK側は準備OKなのでまたTwinCAT側に戻ります。RoboDKとやり取り用のDUT・変数・データ転送プログラムを作成します。

Reference Link

こちらはBeckhoff TwinCAT3 TF6100 OPCUA Serverの立ち上げ手順です↓

Beckhoff#TwinCAT3 TF6100 OPCUA _Part1_Server立ち上げよう

DUT_Pos 

MoverVectorの変数を定義し、OPCUA ServerにStruct Typeプロパティをつけて、Clientからアクセスできるようにします。

TYPE DUT_Pos :
STRUCT
{attribute ‘OPC.UA.DA’ := ‘1’}
{attribute ‘OPC.UA.DA.StructuredType’ := ‘1’}
ActPos :MoverVector;
END_STRUCT
END_TYPE

DUT_xPlanar 

DUT_PosをDUT_xPlanarのグループ内に定義します。

TYPE DUT_xPlanar :
STRUCT
pos :DUT_Pos;
END_STRUCT
END_TYPE

GVL2RoboDK

Global Variable List GVL2RoboDKを追加し、実際のxplanar Moverの座標値を格納する変数を定義します。

{attribute ‘qualified_only’}
VAR_GLOBAL
{attribute ‘OPC.UA.DA’ := ‘1’}
{attribute ‘OPC.UA.DA.StructuredType’ := ‘1’}
xplanar1 :DUT_xPlanar;
END_VAR

MAIN

MAINプログラムは先程とおなじく、転送プログラム 1 Lineを追加するだけです。

//Sending Data to RoboDK by OPCUA
GVL2RoboDK.xplanar1.pos.ActPos:=GVL.mover.MCTOPLC.ACT.ActPos;

Result

RoboDKと連携すると実際Plateどう動いてるかわかりますね！

Implementation-2 Move axis by axis!

次は各軸ごとに移動してみますね！まずは軸AB、次は軸CZです。

GVL

Example1のGVLとわかりませんが、MC_PlanarFeedbackの配列、commandFeedbacksを追加しただけです。MoveA、MoveB、MoveZ、MoveCコマンドを発行するとき別々のコマンドがDoneしてるかをCheckするためです。

{attribute ‘qualified_only’}
VAR_GLOBAL
mover:MC_PlanarMover;
commandFeedback:MC_PlanarFeedback;
commandFeedbacks:ARRAY[0..4]OF MC_PlanarFeedback;
END_VAR

MAIN

流れとしてはXplanar初期化>Xplanar有効>MoveA MoveBコマンド発行>次はMoveC MoveZコマンド発行>ENDになります。

VAR

PROGRAM MAIN_Example2
VAR

    iStep :UDINT;
moverStatus :MC_PLANAR_STATE;
counter :INT;
i :INT;
bGo :BOOL;
 bReset :BOOL;
 bDisable :BOOL;
 TargetA :LREAL:=1.0;
TargetB :LREAL:=-1.0;
TargetC :LREAL:=10.0;
TargetZ :LREAL:=5.0;
ZeroPosition :PositionXYC;
binit :BOOL;
END_VAR

VAR CONSTANT
cStepXplanarInit :UDINT :=0;
cStepResetPosition :UDINT :=5;
cStepXplanarEnable :UDINT :=10;
cStepMoveAB :UDINT :=20;
cStepDelay :UDINT :=30;
cStepMoveCZ :UDINT :=40;
cStepDisable :UDINT :=99;
cStepReset :UDINT :=98;
END_VAR


Code

//Sending Data to RoboDK by OPCUA
GVL2RoboDK.xplanar1.pos.ActPos:=GVL.mover.MCTOPLC.ACT.ActPos;

//Target Positions Init
ZeroPosition.SetValuesXYC(x:=0,y:=0,c:=0);

//xPlanar Status
moverStatus:=GVL.mover.MCTOPLC.STD.State;

//Disable
IF bDisable THEN
GVL.mover.Disable(commandFeedback:=GVL.commandFeedback);
iStep:=cStepDisable;
END_IF
//Reset
IF bReset THEN
GVL.mover.Reset(commandFeedback:=GVL.commandFeedback);
iStep:=cStepReset;
END_IF

CASE iStep OF

cStepXplanarInit:
binit:=FALSE;
iStep:=cStepResetPosition;

cStepResetPosition:
IF  GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Disabled AND NOT binit THEN
GVL.mover.SetPosition(commandFeedback:=GVL.commandFeedback,position:=ZeroPosition);
binit:=TRUE;
END_IF;
IF GVL.commandFeedback.Done THEN
    iStep := cStepXplanarEnable;
  END_IF;

cStepXplanarEnable:
GVL. mover.Enable(commandFeedback:=GVL.commandFeedback);
IF GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled THEN
  iStep := cStepMoveAB;
END_IF
counter:=0;
bGo:=TRUE;
cStepMoveAB:
IF bGo THEN
GVL.mover.MoveA(
commandFeedback:=GVL.commandFeedbacks[0]
,targetPosition:=TargetA
,constraint:=0
);
GVL.mover.MoveB(
commandFeedback:=GVL.commandFeedbacks[1]
,targetPosition:=TargetB
,constraint:=0
);
bGo:=FALSE;
END_IF;
IF GVL.commandFeedbacks[0].Done AND GVL.commandFeedbacks[1].Done THEN
iStep:=cStepMoveCZ;
bGo:=TRUE;
END_IF

cStepMoveCZ:
IF bGo THEN
GVL.mover.MoveZ(
commandFeedback:=GVL.commandFeedbacks[0]
,targetPosition:=TargetZ
,constraint:=0
);
GVL.mover.MoveC(
commandFeedback:=GVL.commandFeedback
,targetPosition:=TargetC
,constraint:=0
,options:=0
);
bGo:=FALSE;
END_IF
IF GVL.commandFeedbacks[0].Done AND GVL.commandFeedbacks[1].Done THEN
iStep:=cStepDelay;
END_IF

cStepDisable:
IF moverStatus= MC_PLANAR_STATE.Disabled THEN
bDisable:=FALSE;
iStep:=cStepXplanarInit;
END_IF
cStepReset:
IF moverStatus= MC_PLANAR_STATE.Enabled THEN
bReset:=FALSE;
iStep:=cStepXplanarInit;
END_IF

END_CASE

//Update
GVL.mover.Update();
GVL.commandFeedback.Update();
GVL.commandFeedbacks[0].Update();
GVL.commandFeedbacks[1].Update();
GVL.commandFeedbacks[2].Update();
GVL.commandFeedbacks[3].Update();
GVL.commandFeedbacks[4].Update();

Result

少しわかりにくいかもしれませんが、PlateのABCZ軸は移動したとわかります！

Implementation-3 ExternalSetpointGeneration!

このExampleではMoverを有効しexternal setpoint generationをスタートします。cStepSetExternalSetpointのStepでずっとSetpointを更新し、Xplanarが相対移動量を変化させています。cStepStopExternalSetpointGenerationのStepでSetpointを更新停止します。

MAIN

VAR

PROGRAM MAIN_Example3
VAR

    iStep :UDINT;
moverStatus :MC_PLANAR_STATE;
counter :INT;
i :INT;
bGo :BOOL;
 bReset :BOOL;
 bDisable :BOOL;
 TargetA :LREAL:=1.0;
TargetB :LREAL:=-1.0;
TargetC :LREAL:=10.0;
TargetZ :LREAL:=5.0;
ZeroPosition :PositionXYC;
binit :BOOL;

setPosition
,setVelocity
,setAcceleration :MoverVector;
 dxPosition
,dxVelocity
,dxAcceleration :LREAL:=0.001;
END_VAR

VAR CONSTANT
cStepXplanarInit :UDINT :=0;
cStepResetPosition :UDINT :=5;
cStepXplanarEnable :UDINT :=10;

cStepStartExternalSetpointGeneration :UDINT :=20;
cStepSetExternalSetpoint :UDINT :=30;

cStepStopExternalSetpointGeneration :UDINT :=50;
cStepStopExternalSetpointGenerationWait :UDINT :=55;

cStepDisable :UDINT :=99;
cStepReset :UDINT :=98;
END_VAR

Code

//Sending Data to RoboDK by OPCUA
GVL2RoboDK.xplanar1.pos.ActPos:=GVL.mover.MCTOPLC.ACT.ActPos;

//Target Positions Init
ZeroPosition.SetValuesXYC(x:=0,y:=0,c:=0);

//xPlanar Status
moverStatus:=GVL.mover.MCTOPLC.STD.State;

//Disable
IF bDisable THEN
GVL.mover.Disable(commandFeedback:=GVL.commandFeedback);
iStep:=cStepDisable;
END_IF
//Reset
IF bReset THEN
GVL.mover.Reset(commandFeedback:=GVL.commandFeedback);
iStep:=cStepReset;
END_IF

CASE iStep OF

cStepXplanarInit:
binit:=FALSE;
iStep:=cStepResetPosition;

cStepResetPosition:
IF  GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Disabled AND NOT binit THEN
GVL.mover.SetPosition(commandFeedback:=GVL.commandFeedback,position:=ZeroPosition);
binit:=TRUE;

END_IF;
IF GVL.commandFeedback.Done THEN
    iStep := cStepXplanarEnable;
  END_IF;

cStepXplanarEnable:
GVL. mover.Enable(commandFeedback:=GVL.commandFeedback);
IF GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled THEN
  iStep := cStepStartExternalSetpointGeneration;
END_IF
bGo:=TRUE;

cStepStartExternalSetpointGeneration:
setPosition.x:=0.0;
setVelocity.x:=0.0;
setAcceleration.x:=0.0;
GVL.mover.StartExternalSetpointGeneration(commandFeedback:=GVL.commandFeedback,options:=0);
GVL.mover.SetExternalSetpoint(setPosition:=setPosition,setVelocity:=setVelocity,setAcceleration:=setAcceleration);
IF GVL.commandFeedback.Busy THEN
iStep:=cStepSetExternalSetpoint;
END_IF

cStepSetExternalSetpoint:
setPosition.x:=setPosition.x+dxPosition*setVelocity.x;
setVelocity.x:=setVelocity.x+dxVelocity*setAcceleration.x;
setAcceleration.x:=setAcceleration.x+dxAcceleration*1.0;
GVL.mover.SetExternalSetpoint(setPosition:=setPosition,setVelocity:=setVelocity,setAcceleration:=setAcceleration);
IF setAcceleration.x >=1.0 THEN
iStep:=cStepStopExternalSetpointGeneration;
END_IF

cStepStopExternalSetpointGeneration:
GVL.mover.StopExternalSetpointGeneration(commandFeedback:=GVL.commandFeedback);
iStep:=cStepStopExternalSetpointGenerationWait;
cStepStopExternalSetpointGenerationWait:
IF GVL.commandFeedback.Done THEN
iStep:=999;
END_IF

cStepDisable:
IF moverStatus= MC_PLANAR_STATE.Disabled THEN
bDisable:=FALSE;
iStep:=cStepXplanarInit;
END_IF
cStepReset:
IF moverStatus= MC_PLANAR_STATE.Enabled THEN
bReset:=FALSE;
iStep:=cStepXplanarInit;
END_IF

END_CASE

//Update
GVL.mover.Update();
GVL.commandFeedback.Update();
GVL.commandFeedbacks[0].Update();
GVL.commandFeedbacks[1].Update();
GVL.commandFeedbacks[2].Update();
GVL.commandFeedbacks[3].Update();
GVL.commandFeedbacks[4].Update();

Result

Plateがずっと以下の式でPositionを更新しています！

setPosition.x:=setPosition.x+dxPosition*setVelocity.x;

Points!

  • 実機がない場合は必ずSimulation ModeをTureにしてください。
  • MC_PlanarMoverやMC_PlanarFeedback Function Blockは直接Callせず、MethodでXplanarを制御してください。
  • MC_PlanarMoverやMC_PlanarFeedback Function Block使用するとき必ずUpdare() Methodで内部Statusを更新すること
  • MC_PlanarFeedbackは各コマンド(例えばMoveZ)のInput Parameter commandFeedbackそのまま渡せばOKです
  • SetExternalSetpointをCallするときSetpointをCycleごとにUpdateしてください。

Source Code

以下のLinkからSource CodeをDownloadしてください。

https://github.com/soup01Threes/TwinCAT3/blob/main/TwinCAT%20Project%20xplanar1%20.zip

TwinCAT Project xplanar1.tnzipはTwinCAT Projectです。

xplanar simulation.rdkはROBODKのプロジェクトです。

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