Coordinate Calculation - Mitsubishi Electric MELFA BFP-A3614 Manual

(3) Coordinate calculation

 Operation details
Search for a hole on the XY plane. If a hole is found, the XY coordinates of the center of the hole are
figured.
 Program example
1 '----------------------------------------------
2 ' Appendix 6 Reference Case (3) Coordinate calculation
3 '----------------------------------------------
4 'Control Mode ( 0 ) setting
5 P_FsStf0=(+0.00,+0.00,+1.00,+0.00,+0.00,+0.00)(0,0)
6 P_FsDmp0=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)(0,0)
7 P_FsMod0=(+0.00,+0.00,+2.00,+0.00,+0.00,+0.00)(0,0)
8 M_FsCod0=1
9 '
10 ' Control Feature ( 0 ) setting
11 P_FsGn0=(+0.00,+0.00,+40.00,+0.00,+0.00,+0.00)(0,0)
12 P_FsFLm0=(+20.00,+20.00,+5.00,+5.00,+5.00,+5.00)(0,0)
13 P_FsFCd0=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)(0,0)
14 '
15 Def Act 1,P_FsLmtR.Z=1 GoTo *PCEN,S
16 'direction becomes smaller than force detection setting value.
17 P2=P1
18 P2.X=P2.X+100
19 Fsc On,0,0,1 'Enable the force sense control with control Mode=0, Control Feature =0, and offset cancel is enabled.
20 Mvs P1
21 '
22 Spd 5
23 Act 1=1
24 For M1=1 To 10 'Search in 5 mm intervals in the Y-direction. Force is lost in the Z-axis direction if the robot axis
25 Mvs P1
Search for a hole
'Move the axis to search start position (Move it to an approximately 10 mm lower position in the
Z-axis direction from the search plane, and then pushes in the Z-direction).
drops into a hole, and therefore an interrupt is entered.
Move in X and Y direction
and calculate the center
position by contact position.
.
'Stiffness coefficient (Specify Z-axis component as 1 [N/mm].)
'Damping coefficient (Not specified.)
'Force sense control type (Specify stiffness control in Z-axis
direction.)
'Force sense coordinate system (Specify XYZ coordinate system.)
'Gain (Specify Z-axis direction as 40 [10-3 mm/N].)
'Force detection setting value (Specify Z-axis direction as
5 [N].)
'Force control (Not specified.)
'Define so that interrupt processing is performed if the Z-axis
'Search for the width of 100 mm in X-direction.
-177-
(Continue to the next page →)