Beckhoff#TwinCAT3 Play with Xplanar And TF5430 And RoboDK_Part2

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

前回はTF5430を使用しRoboDKと連携でXplanarの簡単移動を説明しました。今回はTrack機能を使用し複数の仮想Trackを作成、MoverがそのTrack沿って移動するプログラムを紹介します。よろしくお願いします。

Reference Link

Beckhoff#TwinCAT3 Play with Xplanar And TF5430_Part1

Track?

TrackはUser定義できる経路で、Moverはその経路に沿って移動し、なおかつ複数のTrackを同時に接続することができます。MoverはTrack間を自由に切り替えられます。(切り替えるのタイミングはMoverは移動してるTrackのEnd Pointに到達したとき)

衝突の話は今回触れませんが、複数のMoversを衝突防止するために様々な設定があり、Mover間の興味を近すぎないように制限します。

API

今回も同じくTutorial内で使用するDUTやFunction Blockのみ説明します。

DUT

ST_JoinTrackOptions

JoinTrackコマンドオプションです。

NameTypeDefaultDescription
useOrientationBOOLTruethe target　orientation is also reachedat the end of themovement

ST_LeaveTrackOptions

LeaveTrackコマンドオプションです。

NameTypeDefaultDescription
useOrientationBOOLTruethe target　orientation is also reachedat the end of themovement

ST_MoveOnTrackOptions

MoveOnTrackコマンドのオプションです。

NameTypeDefaultDescription
gapLREAL200.0Track上のMover間の最小距離
directionMC_DIRECTIONMC_DIRECTION．mcDirectionNonModuloMover移動するの方向
additionalTurnsUDINT0
moduloToleranceLREAL0.0

MC_PlanarMover

前回にもPlanarMoverの一部の機能を説明しました。今回はTutorialに合わせて使用してるMethodを説明します。注意するのは直接そのFBを呼び出さないことです。必ずそのObjectのMethodだけを使用してください。

JoinTrack

Mover にtrack を参加し、そのmover がtrack に沿って移動するようになります。

VAR_INPUT

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

VAR_INOUT

NameTypeDescription
targetTrack MC_PlanarTrack Planarが沿ってるTrack

LeaveTrack

Mover がtrack から脱離します。離脱するときもmover の移動先を決めることできます。

VAR_INPUT

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

VAR_INOUT

NameTypeDescription
targetPosition PositionXYC 移動先のTarget位置

MoveOnTrack

Mover にtrack を沿って指定の場所に移動します。

VAR_INPUT

NameTypeDescription
commandFeedbackMC_PlanarFeedback発行したコマンドに対するFeedback
targetTrack MC_PlanarTrack Target track for the movement. If none is specified, this defaultsto the current track.
targetPositionOnTrackLREALTarget position on the target track
constraint IPlcDynamicConstraint Planarの移動に対する制約
optionsST_MoveOnTrackOptionsPlanarの移動オプション

Enable

XPlanarを有効にする。

VAR_INPUT

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

Update

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

MC_PlanarTrack

Planar MoversはそのTrack Objectにそって移動できます。

すべてのTrackに乗せてるPlanar Moverはお互いに干渉しないように制御します。

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

AppendLine

Planar TrackにLineを追加します。

VAR_INPUT

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

VAR_INOUT

NameTypeDescription
start PositionXYC Lineの起動位置
end PositionXYC Lineの終了位置

StartFromTrack

ほかのtrack のendpoint と自分のtrack と繋がります。

VAR_INPUT

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

VAR_INOUT

NameTypeDescription
track MC_PlanarTrack次のStart pointとして設定するTracker 

EndAtTrack

自分のtrackのendpoint と別のtrack のstart point と繋がります。

VAR_INPUT

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

VAR_INOUT

NameTypeDescription
track MC_PlanarTrackPlanar Track Object

Enable

XPlanarを有効にする。

VAR_INPUT

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

Update

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

AddTrack

Track 列にTrack を追加します、その追加されたtrack はいまのtrack 列の一番最後になります。

NameTypeDescription
track MC_PlanarTrackTrack Trailの最後に追加するTrackです。

Planar mover command diagram

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

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

JoiningTrack

Joining track  mode のmover はコマンド一切受け付けません。mover がJoinTrack コマンドで完了したら自動的にその状態からOnTrackに移行します。

OnTrack

OnTrack mode ではmover 既にtrack に参加済みでMoveOnTrackコマンドで移動やLeaveTrackコマンドでtrackから脱離し、mover をleaving track モードに変更できます。MoveCコマンドを発行すると、mover がCRotationOnTrack mode に変わります。

LeavingTrack

LeavingTrack mode のmover はコマンド一切受け付けません。mover がLeaveTrack コマンド完了したら自動的にその状態からFreemovementに移行します。

Planar track operation modes

Planar track は４つのモードがあり、いまのtrack 状態を示します。それは、

  • Moving
  • Standing
  • Configuring
  • Uninitialized.

Moving

Moving mode では、一つや複数のmover がtrack の上に移動してます。最初のmover はtrack の上に移動し始めると自動的standing mode からmoving mode に変更します。そしてtrack 上に最後のmover が移動完了したらまた自動的にstanding mode に戻ります。Mover はmoving modeのときはJoinTrack やLeaveTrack コマンドを実行できません。

Standing

Standing mode ではmoversたちがこのtrackを使用でき、すべてのmovers がtravel コマンドを待ってる状態です。そのmode ではmoverたちがJoinTrack、 LeaveTrack  、MoveOnTrackコマンドを受け付けます。

Configuring

Configuration mode は一つや複数の上にmovers がtrack に離脱してる(LeaveTrack)か参加してる(JoinTrack)かどっちかになってます。最初のmover がtrack の脱離や参加がはじめたら自動的にconfiguration mode に変わり、すべてのmoversが完了したらまた自動的にstanding mode に戻ります。

Uninitialized

Track がuninitiated の状態でmovers が使用できません。理由としてはgeomoetric の設定は完了してなく、user がtrack を使う前に必ずgeometric 情報設定し、そしてtrack 有効すれば自動的にstanding mode に変わります。

Basic Setup

ではhardware configuration から始めましょう。

Add Planar Object

Motion>your mc project >Axes>Groups で右クリックし、Add New Item します。

Planar Track を選び、okします。

いまPlanar Track group が追加された。

中をクリックしますとTrack のパラメータ設定ができます。

Group1>McTOPlc>STD を選び、右クリックしCahnge Link します。

TRACK と繋がってください。

Add One More Mover

MC Projectでもう一つのMoverを追加してください。

Set Mover Init Position

次はMover1,2の初期位置を設定します。

Mover1をクリックします。

次はMover2です。

Mover2は少し離れてるところにあります。

Implementation-1

Implementation-1ではTrackが1つのみで、Start Pontが0,0、End Pointは400,0になります。Xplanar1とXplanar2はそのTrack1にそって移動します。

GVL

{attribute ‘qualified_only’}
VAR_GLOBAL

mover:MC_PlanarMover;
mover2:MC_PlanarMover;
tracks : ARRAY[1..5]OF MC_PlanarTrack;
commandFeedback:MC_PlanarFeedback;
commandFeedbacks:ARRAY[0..4]OF MC_PlanarFeedback;
END_VAR

Code

VAR

PROGRAM MAIN_TrackExample1
VAR

//Track Options
MoveOnTrackOption:ST_MoveOnTrackOptions;
   
//Control Variables
iStep : UDINT;

//Position Setpoint
pos1, pos2 : PositionXYC;
MoveOnTrackPosition1:REAL:=150;
MoveOnTrackPosition2:REAL:=350;

END_VAR

VAR CONSTANT
cStepInit :UDINT := 0;
cStepCheckTrackerEnabled :UDINT := 10;
cStepEnableMovers :UDINT := 20;
cStepCheckMoversEnabled :UDINT := 30;
cStepJoinTrack :UDINT := 40;
cStepCheckOnTrack :UDINT := 50;
cStepMoveOnTrack :UDINT := 60;
cStepCheckPosition :UDINT := 70;
cStepEnd 　　　　　　  :UDINT := 80;
END_VAR

PROGRAM

いまプログラムやってるのはTrackを初期化>Start PointとEnd Point設定>Track有効>Movers有効>移動するの流れです。

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

//Gap Setting between the mover
MoveOnTrackOption.gap:=200.0;

CASE iStep OF

cStepInit:
pos1.SetValuesXY(x:=0,y:=0);
pos2.SetValuesXY(x:=400,y:=0);
GVL.tracks[1].AppendLine(
commandFeedback:=GVL.commandFeedbacks[0]
,start:=pos1
,end:=pos2
);
GVL.tracks[1].Enable(commandFeedback:=GVL.commandFeedbacks[1]);
iStep :=cStepCheckTrackerEnabled;

cStepCheckTrackerEnabled:
IF GVL.tracks[1].MCTOPLC_STD.State = MC_PLANAR_STATE.Enabled THEN
iStep := cStepEnableMovers;
END_IF

cStepEnableMovers:
GVL.mover.Enable(commandFeedback:=GVL.commandFeedbacks[0]);
GVL.mover2.Enable(commandFeedback:=GVL.commandFeedbacks[1]);
iStep := cStepCheckMoversEnabled;

cStepCheckMoversEnabled:
IF GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled
AND GVL.mover2.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled
THEN
iStep := cStepJoinTrack;
END_IF

cStepJoinTrack:
GVL.mover.JoinTrack(
commandFeedback:=GVL.commandFeedbacks[0]
,targetTrack:=GVL.tracks[1]
,constraint:=0
,options:=0
);
GVL.mover2.JoinTrack(
commandFeedback:=GVL.commandFeedbacks[0]
,targetTrack:=GVL.tracks[1]
,constraint:=0
,options:=0
);
iStep := cStepCheckOnTrack;

cStepCheckOnTrack:
IF GVL.mover.MCTOPLC.STD.CommandMode = MC_PLANAR_MOVER_COMMAND_MODE.OnTrack
AND GVL.mover2.MCTOPLC.STD.CommandMode=MC_PLANAR_MOVER_COMMAND_MODE.OnTrack
THEN
  iStep := cStepMoveOnTrack;
END_IF

cStepMoveOnTrack:
GVL.mover.MoveOnTrack(
commandFeedback:=GVL.commandFeedbacks[0]
,targetTrack:=GVL.tracks[1]
,targetPositionOnTrack:=MoveOnTrackPosition1
,constraint:=0
,options:=MoveOnTrackOption
);
GVL.mover2.MoveOnTrack(
commandFeedback:=GVL.commandFeedbacks[0]
,targetTrack:=GVL.tracks[1]
,targetPositionOnTrack:=MoveOnTrackPosition2
,constraint:=0
,options:=MoveOnTrackOption
);
iStep := cStepCheckPosition;

cStepCheckPosition:
IF GVL.mover.MCTOPLC.SETONTRACK.SetPos >= MoveOnTrackPosition1-0.1
AND GVL.mover2.MCTOPLC.SETONTRACK.SetPos >= MoveOnTrackPosition2-0.1
THEN
  iStep := cStepEnd;
END_IF

cStepEnd:

END_CASE

//Update
GVL.mover.Update();
GVL.mover2.Update();
GVL.tracks[1].Update();

Result

Implementation-2

Implementation-1ではTrackが2つあり、Xplanar1とXplanar2はそのTrack1にそって移動し、MoverがTrack1のEndついてたらTrack2に参加しRobotの手前に止まり、Robotが簡単Pickup動作するのDEMOです。

Reference Link

http://soup01.com/ja/category/%e3%83%ad%e3%83%9c%e3%83%83%e3%83%88/robodk/

RoboDK#Install in ubuntu
RoboDK#TwinCATとOPCUA連携し異なるプログラム起動する
Beckhoff#TwinCAT x RoboDK x Pi4 x Coral USB ML – Image Classification

RoboDK

次はrobodk にロボットを追加し簡単な連動を作ろうと思ってます。こちらはtwincat とrobodk のhnadsharkになります。

まずtwincat からCanPick信号をロボットに送信し、ロボットがpick プログラムを実行します。ロボットがプログラム実行完了したらPicked の信号を返し自分のCanPick 信号をFalse に戻します。最後にロボット側もPicked 信号をFalse 信号にし、完了です。

Configuration

前回のTutorialと同じくもうひとつのPlateを追加し、名前を変えます。いまはxplanar1とxplaanr2の名前に変わりました。

次はロボットを追加します。

簡単なロボットプログラムを追加します。

Python

先程のTwinCAT連携Flowを追加するだけです。

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

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


def SetNodeValue(Node,value):
    DataType=Node.get_data_type_as_variant_type()
    DataValue=ua.DataValue(ua.Variant(value,DataType))
    Node.set_data_value(DataValue)


XPLANAR=’xplanar1′
XPLANAR2=’xplanar2′
UR3=’UR3′
ENDPOINT=”opc.tcp://DESKTOP-7FE1JP2:4840″
client=Client(ENDPOINT)
NODES=’ns=4;s=GVL2RoboDK.xplanar1.pos.ActPos’
NODES2=’ns=4;s=GVL2RoboDK.xplanar2.pos.ActPos’
NODESUR_PICKED=’ns=4;s=GVL2RoboDK.Picked’
NODESUR_CANPICK=’ns=4;s=GVL2RoboDK.canPick’
PROG=’Prog2′

# Program example:
xplanar = RDK.Item(XPLANAR)
xplanar2 = RDK.Item(XPLANAR2)
ur3=RDK.Item(UR3)
robotprog=RDK.Item(PROG)
if xplanar.Valid() and xplanar2.Valid() and ur3.Valid() and robotprog.Valid():
    print(‘Item selected..’)
   

client.connect()


print(‘Connected to OPCUA server..’)

NodeFromServer=client.get_node(NODES)
NodeFromServer2=client.get_node(NODES2)
NodeCanPick=client.get_node(NODESUR_CANPICK)
NodePicked=client.get_node(NODESUR_PICKED)

client.load_type_definitions()

_busy=False;


#
while True:
    Actpos=NodeFromServer.get_value()
    CanPick=NodeCanPick.get_value()
    #
    CurrentPos=xyzrpw_2_pose([Actpos.x,Actpos.y,Actpos.z,Actpos.a,Actpos.b,Actpos.c])
    xplanar.setPoseAbs(CurrentPos)
    #print(Actpos.x,Actpos.y,Actpos.z,Actpos.a,Actpos.b,Actpos.c)

   
    Actpos=NodeFromServer2.get_value()

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

    if not _busy and CanPick:
        value=robotprog.RunProgram()
        _busy=True
    if not robotprog.Busy() and _busy:
        SetNodeValue(NodePicked,True)
        _busy=False
    if not CanPick:
        SetNodeValue(NodePicked,False)

    print(_busy)
    print(‘CanPick=’,CanPick)
    #time.sleep(0.1)
   
client.disconnect()

TwinCAT3 Side

RoboDK Sideの準備が終わったところで、次はTwinCAT側ですね。

Confugaraton

2つのTrackがありますので、MC Project内でもう1つのPlanar Trackを追加します。

そしてGVLのObjectと紐つけてください。

Code

VAR

PROGRAM MAIN_TrackExample2
VAR
    state : UDINT;
    pos1, pos2 : PositionXYC;
MoveOnTrackOption:ST_MoveOnTrackOptions;
MoveOnTrackOption2:ST_MoveOnTrackOptions;
pos3:PositionXYC;
postReset:BOOL;

TON:TON;
END_VAR

VAR CONSTANT
cStepInit :UDINT := 0;
cStepCheckTrackerEnabled :UDINT := 10;
cStepEnableMovers :UDINT := 20;
cStepCheckMoversEnabled :UDINT := 30;
cStepJoinTrack :UDINT := 40;
cStepCheckOnTrack :UDINT := 50;
cStepMoveOnTrack :UDINT := 60;
cStepCheckPosition :UDINT := 70;
cStepMover2JoinTrack2 :UDINT := 80;
cStepCheckOnTrack2 :UDINT := 90;
cStepMover2MoveOnTrack2 :UDINT := 100;
cStepRobotCanPickMover2 :UDINT := 110;
cStepWaitRobotDoneMover2             :UDINT := 120;
cStepMover2MoveBack :UDINT := 130;
cStepMover1JoinTrack2 :UDINT := 140;
cStepMover1MoveOnTrack2 :UDINT := 150;
cStepRobotCanPickMover1 :UDINT := 160;
cStepWaitRobotDoneMover1             :UDINT := 170;
cStepMover1MoveBack :UDINT := 180;

cStepEnd :UDINT := 999;
END_VAR

PROGRAM

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

//
MoveOnTrackOption.gap:=200.0;
MoveOnTrackOption.direction:=1;

CASE state OF
cStepInit:
pos1.SetValuesXY(x:=0,y:=0);
pos2.SetValuesXY(x:=400,y:=0);
GVL.tracks[1].AppendLine(commandFeedback:=GVL.commandFeedbacks[0],start:=pos1,end:=pos2);

pos1.SetValuesXY(x:=450,y:=100);
pos2.SetValuesXY(x:=450,y:=700);
GVL.tracks[2].StartFromTrack(0,track:=GVL.tracks[1]);
GVL.tracks[2].AppendLine(commandFeedback:=GVL.commandFeedbacks[2],start:=pos1,end:=pos2);
GVL.tracks[2].EndAtTrack(0,track:=GVL.tracks[1]);
pos3.x:=200;
pos3.y:=0;
GVL.mover2.SetPosition(0,pos3);
state := cStepCheckTrackerEnabled;

cStepCheckTrackerEnabled:
GVL.tracks[1].Enable(commandFeedback:=GVL.commandFeedbacks[1]);
GVL.tracks[2].Enable(commandFeedback:=GVL.commandFeedbacks[3]);
IF GVL.tracks[1].MCTOPLC_STD.State = MC_PLANAR_STATE.Enabled
AND GVL.tracks[2].MCTOPLC_STD.State = MC_PLANAR_STATE.Enabled
THEN
state := cStepEnableMovers;
END_IF

cStepEnableMovers:
GVL.mover.Enable(commandFeedback:=GVL.commandFeedbacks[0]);
GVL.mover2.Enable(commandFeedback:=GVL.commandFeedbacks[1]);
state := cStepCheckMoversEnabled;

cStepCheckMoversEnabled:
IF GVL.mover.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled
AND GVL.mover2.MCTOPLC.STD.State = MC_PLANAR_STATE.Enabled THEN
  state := cStepJoinTrack;
END_IF
cStepJoinTrack:
GVL.mover.JoinTrack(0, GVL.tracks[1], 0, 0);
GVL.mover2.JoinTrack(0, GVL.tracks[1], 0, 0);
state := cStepCheckOnTrack;

cStepCheckOnTrack:
IF GVL.mover.MCTOPLC.STD.CommandMode = MC_PLANAR_MOVER_COMMAND_MODE.OnTrack
AND GVL.mover2.MCTOPLC.STD.CommandMode=MC_PLANAR_MOVER_COMMAND_MODE.OnTrack THEN
  state := cStepMoveOnTrack;
END_IF

cStepMoveOnTrack:
GVL.mover.MoveOnTrack(
commandFeedback:=GVL.commandFeedbacks[0]
,targetTrack:=GVL.tracks[1]
,targetPositionOnTrack:=150.0,constraint:=0
,options:=MoveOnTrackOption
);
GVL.mover2.MoveOnTrack(
commandFeedback:=GVL.commandFeedbacks[0]
,targetTrack:=GVL.tracks[1]
,targetPositionOnTrack:=350.0
,constraint:=0
,options:=MoveOnTrackOption
);
state := cStepCheckPosition;

cStepCheckPosition:
IF GVL.mover.MCTOPLC.SETONTRACK.SetPos >= 149.9
AND GVL.mover2.MCTOPLC.SETONTRACK.SetPos >= 349.9 THEN
  state:=cStepMover2JoinTrack2;
END_IF

cStepMover2JoinTrack2:
GVL.mover2.JoinTrack(0, GVL.tracks[2], 0, 0);
state := cStepCheckOnTrack2;

cStepCheckOnTrack2:
IF  GVL.mover2.MCTOPLC.STD.CommandMode=MC_PLANAR_MOVER_COMMAND_MODE.OnTrack THEN
  state := cStepMover2MOveOnTrack2;
END_IF
cStepMover2MOveOnTrack2:
GVL.mover2.MoveOnTrack(commandFeedback:=GVL.commandFeedbacks[0],GVL.tracks[2],targetPositionOnTrack:=250,constraint:=0,options:=MoveOnTrackOption);
//test—-
GVL.mover.MoveOnTrack(commandFeedback:=GVL.commandFeedbacks[0],GVL.tracks[2],targetPositionOnTrack:=250,constraint:=0,options:=MoveOnTrackOption);
TON(IN:=TRUE,PT:=T#1S);
IF TON.Q THEN
IF GVL.mover2.MCTOPLC.SETONTRACK.SetPos>=249.9
AND GVL2RoboDK.xplanar2.pos.ActPos.x>=450.0
AND GVL2RoboDK.xplanar2.pos.ActPos.y>=222.0
THEN
TON(IN:=FALSE);
state:=cStepRobotCanPickMover2;
END_IF
END_IF;

cStepRobotCanPickMover2:
GVL2RoboDK.canPick:=TRUE;
TON(in:=TRUE,PT:=T#0.2S);
IF TON.Q THEN
TON(IN:=FALSE);
state:=cStepWaitRobotDoneMover2;
END_IF

cStepWaitRobotDoneMover2:
IF GVL2RoboDK.Picked THEN
GVL2RoboDK.canPick:=FALSE;
state:=cStepMover2MoveBack;
END_IF;

cStepMover2MoveBack:
GVL.mover2.MoveOnTrack(commandFeedback:=GVL.commandFeedbacks[0],GVL.tracks[2],targetPositionOnTrack:=0,constraint:=0,options:=MoveOnTrackOption);
IF GVL.mover2.MCTOPLC.SETONTRACK.SetPos>=599.9 THEN
state:=cStepMover1JoinTrack2;
END_IF
  cStepMover1JoinTrack2:
  GVL.mover.JoinTrack(0, GVL.tracks[2], 0, 0);
IF  GVL.mover.MCTOPLC.STD.CommandMode=MC_PLANAR_MOVER_COMMAND_MODE.OnTrack THEN
      state := cStepMover1MoveOnTrack2;
    END_IF
  cStepMover1MoveOnTrack2:
  GVL.mover.MoveOnTrack(commandFeedback:=GVL.commandFeedbacks[0],GVL.tracks[2],targetPositionOnTrack:=250,constraint:=0,options:=MoveOnTrackOption);
IF GVL.mover.MCTOPLC.SETONTRACK.SetPos>=249.9
AND GVL2RoboDK.xplanar1.pos.ActPos.x>=450.0
AND GVL2RoboDK.xplanar1.pos.ActPos.y>=222.0
THEN
state:=cStepRobotCanPickMover1;
END_IF
  cStepRobotCanPickMover1:
  GVL2RoboDK.canPick:=TRUE;
state:=cStepWaitRobotDoneMover1;
  cStepWaitRobotDoneMover1:
IF GVL2RoboDK.Picked THEN
GVL2RoboDK.canPick:=FALSE;
state:=cStepMover1MoveBack;
END_IF
  cStepMover1MoveBack:
GVL.mover.MoveOnTrack(commandFeedback:=GVL.commandFeedbacks[0],GVL.tracks[2],targetPositionOnTrack:=0,constraint:=0,options:=MoveOnTrackOption);
IF GVL.mover.MCTOPLC.SETONTRACK.SetPos>=599.9 THEN
state:=cStepMover2MOveOnTrack2;
END_IF


END_CASE

GVL.mover.Update();
GVL.mover2.Update();
GVL.tracks[1].Update();
GVL.tracks[2].Update();

Result

Soure Code

以下のLinkからプロジェクトをダウンロードしてください。

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

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