Hallo and In the last tutorial I showed how to use TF5430 with RoboDK to simulate a Xplanar with the free movement operation.In this part, I will configure the system with Tracks and Add a Robot in our station.

Hope you like this!

Reference Link

Beckhoff#TwinCAT3 Play with Xplanar And TF5430 And RoboDK_Part1

Track?

Track is a user-defined path in your Xplanar project to allow these movers to move on this track( it can be line, circle), but also we can connect and join the mover in multiple tracks in the system.

Although we will not have a deep discussion of collision in this time,collision is avoided with multiple movers to make sure the distances between movers are not too close.

API

In this Part I will only explain the DUT and Function Block that used in this tutorial.

DUT

ST_JoinTrackOptions

The Option of JoinTrack command.

Name	Type	Default	Description
useOrientation	BOOL	True	the target　orientation is also reachedat the end of themovement

ST_LeaveTrackOptions

The Option of LeaveTrack command.

Name	Type	Default	Description
useOrientation	BOOL	True	the target　orientation is also reachedat the end of themovement

ST_MoveOnTrackOptions

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

Name	Type	Default	Description
gap	LREAL	200.0	Track上のMover間の最小距離
direction	MC_DIRECTION	MC_DIRECTION．mcDirectionNonModulo	Mover移動するの方向
additionalTurns	UDINT	0
moduloTolerance	LREAL	0.0

MC_PlanarMover

In the last tutorial PlanarMover object is explained, and I will show you the method for Track operation in here.This Function Block can not be directly called in the user program and please use the method of its instance.

JoinTrack

Join the mover to specific track and the mover can follow the track to move.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command
constraint	IPlcDynamicConstraint	The constraint of Planar
options	ST_JoinTrackOptions	The moving Option

VAR_INOUT

Name	Type	Description
targetTrack	MC_PlanarTrack	The Track that mover would like to join

LeaveTrack

operate the mover to leave the track and we can specify the position while the mover is left from Track.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command
constraint	IPlcDynamicConstraint	The constraint of Planar
options	ST_LeaveTrackOptions	The moving Option

VAR_INOUT

Name	Type	Description
targetPosition	PositionXYC	The Target position

MoveOnTrack

operate the mover to follow the track to create a movement.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command
targetTrack	MC_PlanarTrack	Target track for the movement. If none is specified, this defaultsto the current track.
targetPositionOnTrack	LREAL	Target position on the target track
constraint	IPlcDynamicConstraint	The constraint of Planar
options	ST_MoveOnTrackOptions	The moving Option

Enable

Enable the Planar Mover.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command

Update

This method must be triggered each cycle,to update the internal state of the object.

MC_PlanarTrack

Planar Movers object can follow the Track object to perform the movement and Planar Movers on the track automatically avoid collisions with each other.This Function Block can not be directly called in the user program and please use the method of its instance.

AppendLine

Append a line in the Planar Track.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command

VAR_INOUT

Name	Type	Description
start	PositionXYC	The start position of Line
end	PositionXYC	The end position of Line

StartFromTrack

Sets the other Planar Tracks endpoint as the start point of this Planar Track.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command

VAR_INOUT

Name	Type	Description
track	MC_PlanarTrack	The target Track

EndAtTrack

Appends a smooth transition from the end of this Planar Track to the other Planar Tracks start point.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command

VAR_INOUT

Name	Type	Description
track	MC_PlanarTrack	Planar Track Object

Enable

Enable the Planar Mover.

VAR_INPUT

Name	Type	Description
commandFeedback	MC_PlanarFeedback	The Feedback of the command

Update

Enable the Planar Mover.

AddTrack

Adds a track to the TrackTrail. The track should start at the end of the currently last track.

Name	Type	Description
track	MC_PlanarTrack	The tagert track that you would like to add

Planar mover command diagram

As I mentioned before in the last tutorial,there are 6 types of commands in Planar Mover.

OnTrack
LeavingTrack
JoiningTrack
ExternalSetpointGeneration
CRotationFreeMovement/-
FreeMovement

JoiningTrack

In JoiningTrack Mode, mover will not receive and execute any command, and the Mover will shift the status to OnTrack after the JoinTrack command is finished.

OnTrack

The mover will be in OnTrack Mode after the JoinTrack command is finished.

In OnTrack mode, MoveOnTrack Command can be received to execute an on track movement, or leave the Track by using LeaveTrack Command.

And also, the mover will shift to CRotationOnTrack mode while a moveC command is received.

LeavingTrack

In the LeavingTrack mode the mover will not receive any command and it will shift to FreeMovement mode after LeaveTrack command is Finished.

Planar track operation modes

There are 4 modes in the Planar Track to show the track status.

Moving
Standing
Configuring
Uninitialized.

Moving

In the Moving Mode, It means that movers are moving on the track, and the Track status will shift from standing mode to moving mode. After the movement is finished, the track status will shift back to Standing mode automatically.

The mover can not execute the commands like JoinTrack/LeaveTrack while in Moving Mode.

Standing

In Standing Mode the mover can use the Travel command like JoinTrack/LeaveTrack/MoveOnTrack.

Configuring

In the Configuring mode, it means that at least one Mover is executing the LeaveTrack Command or JoinTrack Command to affect the Track object. The status will shift to Configuring while the first mover is joining/leaving the Track, and Shift to Standing mode while all movers ‘ commands are finished.

Uninitialized

In the uninitiated mode the movers can not be used – The reason may be geometric is not configured and the user should set up all geometric information before Track is enabled, and the track will shift to Standing mode automatically.

Basic Setup

We will start to set up the hardware configuration first.

Add Planar Object

Go to Motion>your mc project >Axes>Groups Right Click and Add New Item.

Choose Planar Track and OK.

Planar Track group is created.

Click the Track object and we can set up the parameter for the Track.

ANd then Go to Group1>McToPlc>STD >Right Click and Change Link.

Choose the TRACK object that is inside your user-program.

Add One More Mover

insert one more Mover inside your MC Project.

Set Mover Init Position

The initial position of Movers can be configured here. Click the Mover1.

Go to the Parameter(init) tab and set the Initial position as 0 in x,y,z,a,b,c.

次はMover2です。

Go to the Parameter(init) tab and set the Initial position as 240in x and 0 in y,z,a,b,c.

Implementation-1

We will set the first Implementation that only has Track 1 as Start point=0.0 and End Point as 400,0.The Xplanar1 and Xplanar2 will move based on the 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

We will Initiate the Track>Set the start point and end point of the Track>Enable the Track>Enable the movers>operate the movement.

Result

Implementation-2

In the second Implementation we have 2 Tracks, mover1 and mover2 will follow the Track1 to perform the movement to the end of End point in Point1, and joint Track2 and Stop the movers in front of the Robot>Perform the Pick up Program of Robot> Go Back.

Reference Link

http://soup01.com/en/category/robot/robodk-en/

RoboDK#Install in ubuntu

RoboDK#Start Program from TwinCAT By OPCUA

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

RoboDK

In the Second Implementation a Robot is inserted in out station , and a simple hardshark flow is used in the below.TwinCAT3 will pass a CanPick=True signal to RoboDK side, and RoboDK will execute the Robot Program.

A Picked=True Signal is sent back to TwinCAT3 while the Robot Program is finished, and TwinCAT3 will Turn off the CanPick to False, Finally RoboDK Side will also turn off the Picked signal to false.

Configuration

As with the last tutorial, we will add a new plate and rename these 2 plates as xplanar1 and xplanar2.

And then insert the robot.

Create a simple Robot Program.

Python

In the Python script, the Flow is inserted inside.

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

Now we can configure the TwinCAT3 Side.

Confugaraton

In this Implementation we have 2 tracks, please add one more Planar Track inside your MC Project.

And Link it into your user program.

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();