21 Appendix
sensor into the robot area
53 PVTR=(P_Zero/PWKPOS)*PTEACH 'Vectors specifying the center of gravity of the vision
sensor and grabbing position
54 If PVTR.X<-PCHK.X Or PVTR.X>PCHK.X Then Error 9110 'The calculation result is greatly different
from the theory value.
55 If PVTR.Y<-PCHK.Y Or PVTR.Y>PCHK.Y Then Error 9110
56 Return
57 '
58 '##### Condition setting #####
59 *S30CONST
60 MDX = P_EncDlt(MENCNO).X 'Amount of movement per pulse (X)
61 MDY = P_EncDlt(MENCNO).Y 'Amount of movement per pulse (Y)
62 MDZ = P_EncDlt(MENCNO).Z 'Amount of movement per pulse (Z)
63 MD = Sqr(MDX^2+MDY^2+MDZ^2) 'Calculation of the amount of movement per pulse
64 MEI#=Abs((MVSL-MWKL)/MD) 'Calculation of imaging start setting value
65 Return
66 '
67 '##### VS recognition check processing #####
68 *S40CHKS
69 *LVSCMD
70 *LWAIT
71 MEC# = M_Enc(MENCNO)
72 MEM#=MEC#-MEP# 'Subtract the previous encoder pulse value from the
current position of the encoder
73 If MEM# > 800000000.0# Then MEM# = MEM#-1000000000.0#
74 If MEM# < -800000000.0# Then MEM# = MEM#+1000000000.0#
75 If Abs(MEM#) > MEI# GoTo *LVSTRG 'Comparison between the amount of encoder movement
and the camera startup setting value
76 Dly 0.01
77 GoTo *LWAIT
78 *LVSTRG
79 MEP#=MEC# 'Set the encoder pulse current position to the previous
value
80 NVTrg #1, 5, MTR1#,MTR2#,MTR3#,MTR4#,MTR5#,MTR6#,MTR7#,MTR8# 'Imaging request +
encoder value acquisition
81 'Acquisition of recognition data
82 If M_NvOpen(1)<>1 Then Error 9100 'Communication error
83 EBRead #1,"",MNUM,PVS(1),PVS(2),PVS(3),PVS(4) 'Imaging request
84 If MNUM=0 Then GoTo *LVSCMD 'If no workpieces are recognized
85 If MNUM>4 Then MNUM=4 'Set the maximum number (4)
86 For M1=1 To MNUM 'Repeat for the number of workpieces recognized
87 MX(M1)=PVS(M1).X 'Data acquisition
88 MY(M1)=PVS(M1).Y
89 MT(M1)=PVS(M1).C
90 Next M1
91 GoSub *S60WRDAT 'Tracking data storage processing
92 Return
93 '
94 '##### Tracking data storage processing #####
95 *S60WRDAT
96 For M1=1 To MNUM 'Perform processing for the number of workpieces
recognized
97 PSW=P_Zero
98 PSW=PRBORG 'Virtually move the robot close to the vision sensor
99 PSW.X=PSW.X+MX(M1) 'Create the grabbing position
100 PSW.Y=PSW.Y+MY(M1)
101 PSW.C=PSW.C+MT(M1)
102 PRW=P_Zero
103 PRW=PSW*PVTR 'Compensate for the error in the calculation value
104 PRW.FL1=P_100(MWKNO).FL1
105 PRW.FL2=P_100(MWKNO).FL2
106 Select MENCNO
107 Case 1
108 TrWrt PRW, MTR1#, MWKNO,1,MENCNO 'Position, encoder value, model number, buffer number,
encoder number
Sample Programs 21-137
To Next Page
Loading...
