How to Control Delta Robotic Arm by EtherCAT Motion Controller?

_Read Me at First: _

A. This article is written by Zmotion, original article is .

Generally, Delta robotic arm supports 3-4 axes (joint axis 1 + joint axis 2 + joint axis 3 + [end rotary axis 4]). For Zmotion motion controllers, not all controllers support Delta robot function, only the controller that is with suffix “R” can be applied in Delta robot application, for example, ZMC406R.

Then, here we take

For development:

*

B. Save Robot Structure Parameters into TABLE Register

While building the robotic arm connection, it needs to fill in mechanical parameters into TABLE arrays according to below sequence.

--Mechanical Structural Parameters of Delta Robotic Arm FRAME 12--

‘starting from TableNum, save robotic arm structural parameters shown in above form into Table.

TABLE(TableNum,Top_R,Under_R,Top_L,Under_L,OneCirPules_J1,OneCirPules_J2,OneCirPules_J3,Offset_X,Offset_Y,Offset_Z,OneCirPules_J4)

C. Set Parameters of Joint-Axis & Virtual-Axis

All axes’ axis types and pulse amounts (units) should be set correctly. UNITS means the number of pulses for the motor in 1° motion. But for virtual-axis, its “units” is unrelated to actual sending pulses, it is used to set motion precision. And it is recommended to set virtual-axis’ units in 1mm is 1000, which means the accuracy is 3 digits after decimal part.

'joint-axis setting

BASE(Axis_JList(0),Axis_JList(1),Axis_JList(2),Axis_JList(3))

'set pulse axis type (ATYPE) as 1, if it is bus type, set it as 65. 

ATYPE = 1,1,1,1 

UNITS = UnitsJList(0),UnitsJList(1),UnitsJList(2),UnitsJList(3)

'set joint-axis speed, acceleration (usually be 10 times of speed), deceleration (usually be 10 times of speed)

SPEED = SpeedJList(0),SpeedJList(1),SpeedJList(2),SpeedJList(3) 

ACCEL = ADSpeedJList(0),ADSpeedJList(1),ADSpeedJList(2),ADSpeedJList(3)

DECEL = ADSpeedJList(0),ADSpeedJList(1),ADSpeedJList(2),ADSpeedJList(3)

'S curve setting

SRAMP = SrampJ(0),SrampJ(1),SrampJ(2),SrampJ(3)

'virtual-axis setting

BASE(Axis_VList(0),Axis_VList(1),Axis_VList(2))

'set virtual axes type as 0

ATYPE = 0,0,0

'set virtual axes’ pulse amount (ATYPE) as 1000, which means the precision is after 3 digits decimal part

UNITS = 1000,1000,1000

'set virtual-axis speed, acceleration (usually be 10 times of speed), deceleration (usually be 10 times of speed)

SPEED = SpeedVList(0),SpeedVList(1),SpeedVList(2)

ACCEL = AccelV(0),AccelV(1),AccelV(2)

DECEL = DecelV(0),DecelV(1),DecelV(2)

'S curve setting

SRAMP = SrampV(0),SrampV(1),SrampV(2)

D. Move Each Joint-Axis to Planned Origin Position

While robotic arm algorithm is building, it needs one origin position as the reference. When Delta robot each joint-axis’ connection rods L1 are all in horizontal position, it is considered as joint origin position. In real robotic arm machine on site, generally there is one location pin, if it doesn’t have the location pin, it can adjust L1 to be horizontal by “level instrument”.

A. CONNREFRAME: Build Forward Kinematics Connection

--what is forward kinematics mode--

Associate virtual-axis coordinate with joint-axis coordinate, when joint-axis moves, virtual-axis will automatically move to corresponding position.

--how to use command--

CONNREFRAME(frame, tablenum, Axis_J1, Axis_J2, Axis_J3, Axis_J4)

Frame: the type of coordinate system. (for example, 12 means the robotic arm is 4-axis Delta robot, please refer to Zmotion robot command manual).

Tablenum: TABLE starting position that saves robotic arm structural parameters.

Axis_J1: the first joint-axis axis No.

Axis_J2: the second joint-axis axis No.

Axis_J3: the third joint-axis axis No.

Axis_J4: the fourth joint-axis axis No.

B. CONNFRAME: Build Inverse Kinematics Connection

--what is inverse kinematics mode--

Associate current joint-axis coordinate system demand position with virtual-axis coordinate system position. Please note the motion max speed of joint-axis coordinate system is limited by SPEED parameter. And when there is one alarm of joint-axis, the motion will be cancelled.

--how to use command--

CONNFRAME(frame, tablenum, Axis_Vx, Axis_Vy, Axis_Vz, Axis_J4)

Frame: the type of coordinate system. (for example, 12 means the robotic arm is 4-axis Delta robot, please refer to Zmotion robot command manual).

Tablenum: TABLE starting position that saves robotic arm structural parameters.

Axis_Vx: the first virtual-axis axis No.

Axis_Vy: the second virtual-axis axis No.

Axis_Vz: the third virtual-axis axis No.

Axis_J4: the fourth virtual-axis axis No.

2. How to Build Delta Robotic Arm Mode

(1) Forward Kinematics Connection

Take Delta robotic arm Frame 12 as the example. At first, save robotic arm structural parameters into Table in order, starting from one certain Table No., then select corresponding model axis list, at last, use CONNREFRAME command to build the forward kinematics mode.

For details of this command, please refer to “ZBasic Programming Manual”.

'save robotic arm parameters into Table array in order starting from TableNum

TABLE(TableNum,Top_R,Under_R,Top_L,Under_L,OneCirPules_J1,OneCirPules_J2,OneCirPules_J3,Offset_X,Offset_Y,Offset_Z,OneCirPules_J4)

'select axes

BASE(Axis_Vx,Axis_Vy,Axis_Vz,Axis_J4)

'build robotic arm forward kinematics

CONNREFRAME(FrameType,TableNum,Axis_J1,Axis_J2,Axis_J3,Axis_J4)

If the building succeeds, virtual axes’ MTYPE (current motion type) will become 34, now, only joint-axis can be operated in joint-axis coordinate to adjust the robotic arm attitude. In ZDevelop (RTSys), there is one “manual” tool, we can select joint-axis No. to do point or inch (here, we select axis 0, axis 1, and axis 2 joint axes, and axis 3 terminal rotate axis). At this time, virtual axis will automatically calculate the position of end working place in the Cartesian coordinate system.

(2) Inverse Kinematics Connection

Take Delta robotic arm Frame 12 as the example. At first, save robotic arm structural parameters into Table in order, starting from one certain Table No., then select corresponding model axis list, at last, use CONNFRAME command to build the inverse kinematics mode.

For details of this command, please refer to “ZBasic Programming Manual”.

'save robotic arm parameters into Table array in order starting from TableNum

TABLE(TableNum,Top_R,Under_R,Top_L,Under_L,OneCirPules_J1,OneCirPules_J2,OneCirPules_J3,Offset_X,Offset_Y,Offset_Z,OneCirPules_J4)

'select axes

BASE(Axis_J1,Axis_J2,Axis_J3,Axis_J4) 

'build robotic arm forward kinematics

CONNFRAME(FrameType,TableNum,Axis_Vx,Axis_Vy,Axis_Vz,Axis_J4)

If the building succeeds, joint axes’ MTYPE (current motion type) will become 33. “manual” also can be used here. Now, only virtual-axis can be operated by processing technology commands. That is, the trajectory edited before moves in the Cartesian coordinate system (here, take virtual axis 10, 11, 12 as the example), joint axis will automatically calculate how to do joint motion in the joint coordinate system.

3. How to Operate?

(1) Edit the Program

ZDevelop (RTSys) supports hybrid programming among Basic, HMI, and PLC. This routine matches Basic with HMI. For HMI, it can select “control” to drag and place for interacting the interface.

A. Understand the Needs

--Condition--

Delta robotic arm builds the inverse kinematics, and take the origin position as the reference to determine one safe height.

--Basic Actions--

a. Delta moves up to above the fixed material in one certain safe height.

b. Delta moves down, opens OP port to get the material by “vacuum suction”.

c. Delay to move up to the safe height.

d. Move to fixed 2*2 disk position.

e. Down to disk position, close OP to place the material.

f. Repeat above, until the disk is full.

B. Process the Codes

'starting flag position 1

StartFlag = 1

LOCAL SafeHigh,i

'safe height

SafeHigh = DPOS(Axis_VList(2))-25

'row and colume

LOCAL Row,Col

'fixed material getting position

LOCAL SrcBorrow_X,SrcBorrow_Y,SrcBorrow_Z

SrcBorrow_X = 5

SrcBorrow_Y = 5

SrcBorrow_Z = SafeHigh-20

'select virtual axis

BASE(Axis_VList(0),Axis_VList(1),Axis_VList(2))

'set processing speed, acceleration, and deceleration

SPEED = SpeedVList(0)

ACCEL = AccelV(0)

DECEL = DecelV(0)

'open the continuous interpolation

MERGE = ON

'each on, temporary total numbers cleared 

TmpSum = 0

'move to the safe height (reference for origin position)

MOVEABS(0,0,SafeHigh)

FOR Col=1 TO 2

    FOR Row=1 TO 2

        'move to the position where is above the material placing position

        MOVEABS(SrcBorrow_X,SrcBorrow_Y,SafeHigh)

        'down to material getting place

        MOVEABS(SrcBorrow_X,SrcBorrow_Y,SrcBorrow_Z)

        'open OP to suck for getting the material

        MOVE_OP(8,ON)

        'delay

        MOVE_DELAY(300)

        'up to safe height

        MOVEABS(SrcBorrow_X,SrcBorrow_Y,SafeHigh)

        'move to the position where is above the disk that is for placing materials

        MOVEABS(-5*Row,5*Col,SafeHigh)

        'down to disk material placing position

        MOVEABS(-5*Row,5*Col,SrcBorrow_Z)

        'close OP to place the material

        MOVE_OP(8,OFF)

        'delay

        MOVE_DELAY(300)

        'up to safe height

        MOVEABS(-5*Row,5*Col,SafeHigh)

        'temporary total is added automatically

        TmpSum = TmpSum+1

    NEXT

NEXT

'wait until all axes stop

WAIT UNTIL IDLE(Axis_VList(0)) AND IDLE(Axis_VList(1)) AND IDLE(Axis_VList(2))

DELAY(10)

RAPIDSTOP(2)

DELAY(10)

'starting flag position 0

StartFlag = 0

(2) Debug & Analyze

A.Download the program into controller, then click “RTSys” – “Tool” – “Plugin” – “XPLC SCREEN”.

C. After forward building, open Zmotion Robotic Arm Solution “ZRobotView”, click “connect”, one “connect to controller” window appears, then select controller connection method (serial port or ethernet). Here, we select ethernet, it only needs to select controller IP, then click “connect”.

E. Open ZDevelop (RTSys) again, click *“SCOPE” *(oscilloscope). In the “scope” window, please select XYZ mode, that is, obverse the effect in 3D. For data source, select DPOS (demand position) here, and select axis No. of virtual axes. When all these parameters are set, click “stop” – “start” – “trigger”.

G. Return our programming software (ZDevelop / RTSys). In oscilloscope interface, processing effect is shown.

It can be seen Delta robotic arm always keeps moving up and down in one certain safe height in the process of getting and placing the material. In this way, real processing can be stable.