Transcript Driving value Kinematics with MPiec Controllers Hosted by: Doug Meyer Sr. Project Engineer Yaskawa America Inc PP.MPIEC-01 | Rev 1.00 | Date: 02/20/2012 | ©2012 Yaskawa America, Inc.

Driving value
Kinematics
with MPiec
Controllers
Hosted by:
Doug Meyer
Sr. Project Engineer
Yaskawa America Inc
PP.MPIEC-01 | Rev 1.00 | Date: 02/20/2012 | ©2012 Yaskawa America, Inc. All rights reserved.
Driving value
Welcome to Yaskawa America’s
Training Café Express
•
To make this Café enjoyable for all, please follow these tips.
– Please do not put us on hold. Others will hear the hold music.
– Do not use a speaker phone. Background noise can be heard.
– We welcome comments and questions.
You can type questions into the “Chat” window. Please send to ‘All Panelists’
– Questions not answered during the Café can be e-mailed to
[email protected] or can be entered into the survey sent to you at the end
of the class.
PP.YEA-M.01 | Rev 1.00 | Date: 03/31/2011 | ©2010 Yaskawa America, Inc. All rights reserved.
Challenge #1
2-axis ‘Delta-2’ Planar Robot




Pick and place action
100-120 cycles per min
Constant pickup location
Create a product stack
Move along a Path to avoid
abrupt corners and improve
cycle time

z
Keep path referenced in X-Z
cartesian space
x
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Page 3
Delta-2 Approach
Benefits over traditional Gantry



Higher speeds at endeffector
Fixed motor locations
Stability and rigidity
Multiple mechanical
arrangements and
permutations are possible
z
x
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Page 4
iec
Solution
Sigma-5 motors with Absolute Feeedback

No homing required on power-up
MP2300Siec Controller


1-16 axes networked servo control
Up to 6 Virtual axes
MotionWorks IEC Professional



IEC61131-3
User-defined Function Blocks
User-defined DataTypes
Page 5
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Application Support Tools
Kinematic_Toolbox_v200


Delta_2_1_Control
FourBar_1_Control
Gantry_Toolbox_v201

XY_MoveAbsolute

PathGenerator

MovePath
Math_Toolbox_v201

ATAN2
» ArcTan2 function avoids discontinuities of regular ArcTan function at
vertical asymptotes
YMotion Firmware Library

Y_DirectControl
» Allows continuous streaming of position, velocity or torque targets to a
motion axis
Page 6
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Implementation Method – A 5-Step Process
1) Define Virtual Axes in user coordinate system (X,Z)
2) Use Forward Kinematic equations to establish initial
position of the virtual axes
3) Enable all axes
4) Create motion on the virtual axes in (X,Z) cartesian
space
5) Use Inverse Kinematic equations to transfer the motion
to the real axes in (Θ1,Θ2) mechanism space
Page 7
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
How to Implement
Add Kinematic Toolbox
as User Library




Available on
Yaskawa Website
Free download
Currently contains
2 kinematic
mechanisms
More to follow
www.yaskawa.com/iectb
Page 8
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
How to Implement
One Function Block
Inserted into a POU!





Enables the Axis
Clears Alarms
Sets Zero Position
for Mechanism
All Kinematics
inside
Just supply
information about
the mechanism and
the application
Page 9
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
How to Implement
Two parts of the
implementation


Run the Kinematic
Function Block
Create motion on the
virtuals in Cartesian space
Example Code


Delta2_1_EC_v200.zwt
Website Document
Number EC.MWIEC.31
Page 10
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Inside the Function Block
Page 11
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
F
Model
L5
C
Givens:
d
L1=L2
L3=L4
L4+L5
L4
L3
Θ4
Θ3
D
B
Note:
Shown Inverted from normal
usage so as to align with linkage
coordinate system
L2
P
z’
Motors located at pts A and E
Q
Θ2
d
Coordinate system origin at Pt A
A
L1
Θ1
x’
E
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Page 12
Method
Prior to enabling servos

Forward Kinematics
» Establish (x’,z’) position of linkage based on (Θ1,Θ2) absolute
motor positions

Coordinate system transformation
» Translate (x’,z’) positions of the linkage into (X,Z) positions of end
effector

Set positions of Virtual Axes in (X,Z) space
Page 13
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Forward Kinematics
Custom Function Block




Uses abs position of motors at
Pts A and E (Θ1 and Θ2 are
known)
Geometry and Trigonometry
Θ4 can only be calculated after
PtC is known
Used to establish position of Pt F
prior to enabling the real axes.
A = (0,0)
E = (d,0)
D = (d + L1CosΘ1, L1SinΘ1)
B = (L2CosΘ2, L2SinΘ2)
C = Intersection of Circles centered at B and D
F = (d + L1CosΘ1 + L45CosΘ4, L1SinΘ1 + L45SinΘ4) = (Xf, Zf)
Page 14
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Coordinate System Fwd Translation to X,Z
(0,0)
X = Xoffset - X’
Z Offset
Z = Zoffset - Z’ - Zef
z
F
(x’,z’)  (x,z+zef)
Zef End-effector offset
(x,z)
(0,0)
x
X offset
Page 15
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Creating Movement on Virtual Axes (X,Z)
Application code creates moves in X,Z space using
XY_MoveAbsolute, PathGenerator and Move_Path function
blocks found in the Gantry_Toolbox. See Example Code
Delta2_1_EC_v200.zwt.
Page 16
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Method to Define Paths
Pre-Calculate path segments using a customized Path
Segment Calculator FB in a lower speed task
Define Segments in absolute X,Z space
Generate path segment structure
Ex. Path ‘Drop3’ moves from pickup point to drop off
point for layer 3.
(X4,Z4)



Seg4
Seg3
(X3,Z3)
(X5,Z5)
Seg5
(X6,Z6)
z
Seg2
(X2,Z2)
Seg1
(X1,Z1)
x
Page 17
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Segment calculation
Straight Line definition



Straight lines can be defined by 2 absolute points and
by setting Resolution equal to zero.
Starting point is assumed to be current position
Specify total number of segments when finished with
segment definition
Drop3Segs.Data[2].SegmentType := TB_Pattern#StraightLine;
Drop3Segs.Data[2].Xcoord := X3;
Drop3Segs.Data[2].Ycoord := Z3;
Drop3Segs.Data[2].Resolution := REAL#0.0;
Drop3Segs.LastSegment := INT#4;
Page 18
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Segment calculation
Arc definition


Arcs can be defined given the start and end points,
absolute starting angle and traversed angle
If the angles are unknown, use Calculate_Angles
function given the points, radius and direction
(ArcDefinitionMode = INT#1) OR center point and
direction of the arc (INT#0).
Drop3Segs.Data[1].SegmentType := TB_Pattern#Arc;
Drop3Segs.Data[1].Radius := R1;
Drop3Segs.Data[1].Resolution := REAL#0.1;
Calculate_Angles_1(X1:=X1,X2:=X2,Y1:=Z1,Y2:=Z2,Radius:=R1,Execute:= TRUE,
ArcDefinitionMode:=INT#1,Direction:=BOOL#0); (* 0=CW, 1=CCW *)
Drop3Segs.Data[1].StartAngle := Calculate_Angles_1.StartAngle;
Drop3Segs.Data[1].TraversedAngle := Calculate_Angles_1.TraversedAngle;
Page 19
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Path Segment Calculator
A customized segment calculator block is used to create
the path segments from user-provided datapoints.


Provide array of data points
Creates a segment structure to be fed to the
PathGenerator
Page 20
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
PathGenerator
PathGenerator function block turns segment data into a
predefined path on the prescribed axes


Provide segment structure
Returns Path structure and PathID number to use with
Move_Path
Page 21
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Move the Virtual Axes in X,Z space
Pre-Defined Paths can be executed with Move_Path
Page 22
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Move the Virtual Axes in X,Z space
Normal pt-to-pt moves can be done with
MC_MoveAbsolute or XY_MoveAbsolute on the
virtuals defined in a Gantry structure
Page 23
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Inverse Kinematics, 3 Steps
1) Translate Commanded Position of virtual axes in (X,Z)
into Commanded Position of PtF in (x’,z’)
Page 25
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Coordinate System Translation to x’,z’
(0,0)
x’ = Xoffset - X
Z Offset
z’ = Zoffset - Z - Zef
z
F
(x’,z’)  (x,z+zef)
Zef End-effector offset
(x,z)
(0,0)
x
X offset
Page 26
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Inverse Kinematics, 3 Steps
2) Feed Commanded position of PtF into an Inverse
Kinematics function block
Page 27
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Inverse Kinematics
Custom Function Block



Uses desired (Xf,Zf) position of
Virtual Axes
Geometry and Trigonometry
transformation
Used to calculate commanded
position of real axes.
Θ1 = P - Q
Θ4 = ATAN2[(Zf – L1SinΘ1)/(Xf – L1CosΘ1 – d)]
Xc = Xf – L5CosΘ4
Zc = Zf – L5SinΘ4
Θ2= ATAN2(Zc/Xc) + Cos-1[(L22+Xc2+Zc2-L32) /
(2L2(Xc2+Zc2)1/2)]
Page 28
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Inverse Kinematics, 3 Steps
3) Feed the resulting commanded position values for
Theta1 and Theta2 into a Y_DirectControl block for
each axis.

Y_DirectControl is found in the Y_Motion Firmware
Library
Page 29
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Stream Command to Real Axes
Y_DirectControl Block



From YMotion firmware library
Sub-Interpolation Filter
smoothes the profile
More sub-interpolation at Servo
Page 30
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Choose Valid XZ Coordinate
System for Virtual offsets



All points must be within
physical range of motion
In general, operate below
the ‘knees’ of the
mechanism
Leave margin to avoid full
extension
Z Offset
Set Limits on Range of Motion
z
(0,0)
Approximate range of motion
x
X offset
Page 31
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
C
Set Limits on Range of Motion
Prevent Multiple solutions to
Inverse Kinematic equations


L4
L3
Choose good starting point
and path
Stay away from points that
create ‘locked legs’
condition
» LenAC approaches L2+L3 (or
Θ2= Θ3)
» LenEC approaches L1+L4 (or
Θ1= Θ4)
Θ3
Θ4
B
D
L2
L1
z’
Θ2
Θ1
x’
A
E
d
Page 32
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Set Limits on Range of Motion
Prevent Linkage overlaps


Set Min and Max value for Θ1
Set Min and Max value for Θ2
F
z’
B
L2
L5
L3
Θ2
C
x’
A
d
Θ1
E
L4
L1
Θ3
D
Page 33
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Set Limits on Range of Motion
Prevent Linkage overlaps (linkage compression)

Θ4 – Θ3 approaches 180 deg
L5
L3
B
z’
L4
C
Θ4
Θ3
D
Θ2
L1
L2
Θ1
x’
A
E
d
Page 34
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Set Limits on Range of Motion
Enter Givens and Limits in Initialization POU
Page 35
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Example Video
To see an example of Yaskawa Kinematic Toolbox in
action on a real machine, view the video available on
YouTube at http://ez.com/delta2
This video shows a pick and place arm in action on a
Blisterpack Thermoformer machine at PackExpo 2011.
Page 36
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Challenge #2
2-axis ‘4-Bar’ Planar Robot



Pick and place action
Concentric DD motors
Independent Z axis
Improve repeatability,
throughput and uptime



Target +/- 12 micron
repeatability
Faster moves - Double
production rate to 4000
substrates per day
Path is referenced in X-Y
Cartesian space
Page 37
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Method
Process is the same






Create Forward Kinematic equations
Translate to working coordinate system through offsets
Create motion on virtual axes in XY space
Translate virtual positions into mechanism coordinate
space
Perform Inverse Kinematics on mechanism endpoint to
create motor commands
Stream motor position commands to real axes using
Y_DirectControl block.
Page 38
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Model
Advantages of 4-bar
linkage



Less moving inertia
since motor controlling
L2 is at origin of L1
instead of end of L1
Greater stability of L2
FB assumes a
parallelogram structure
4
M
Page 39
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Kinematics
Page 40
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Kinematics
Page 41
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Example Code
FourBar_1_EC_v200.zwt

Available at Yaskawa .com Document Number
EC.MWIEC.32
Page 42
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Example Video
To see an example of Yaskawa Kinematic Toolbox in
action for the Four-Bar Linkage, view the video
available on YouTube at http://ez.com/fourbar
This video shows an arm in action moving around the
motion envelope at 2g acceleration.
Page 43
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Questions and Answers
Page 44
Doc:PP.MPIEC.01 Date: 02/20/2012 | © 2012 Yaskawa America, Inc. All rights reserved.
Driving value
Thank You
PP.MPIEC.01 | Rev 1.00 | Date: 03/31/2011 | ©2010 Yaskawa America, Inc. All rights reserved.