Mitsubishi CR750-Q Series - Sample Programs; Conveyer Tracking

21 Appendix
21.5. Sample Programs
21.5.1.
Conveyer Tracking
(1) A1.Prg
1 '## Ver.A3 ######################################
2 '# Program for calibration between tracking robot and conveyer
3 '# Program type : A1.prg
4 '# Date of creation/version : 2012.07.31 A3
5 '# COPYRIGHT : MITSUBISHI ELECTRIC CORPORATION.
6 '################################################
7 '(1) Register an encoder number to the X coordinate of the "PE" variable/
8 'Check the setting value
9 MECMAX=8 'The maximum encoder number value (for checking)
10 If PE.X<1 Or PE.X>MECMAX Then Error 9101 'Encoder number out of range
11 MENCNO=PE.X 'Acquire the encoder number
12 '(2) Attach a marking sticker on the conveyer upstream side/
13 '(3) Move the robot to the position right at the center of the attached sticker/
14 MX10EC1#=M_Enc(MENCNO) 'Acquire encoder data (first time)
15 PX10PS1=P_Zero 'Set all elements to ZERO
16 PX10PS1=P_Fbc(1) 'Acquire the current position (first time)
17 '(4) Raise the robot/
18 '(5) Move the sticker in the forward direction of the conveyer/
19 '(6) Move the robot to the position right at the center of the moved sticker/
20 MX10EC2#=M_Enc(MENCNO) 'Acquire encoder data (second time)
21 PX10PS2=P_Zero 'Set all elements to ZERO
22 PX10PS2=P_Fbc(1) 'Acquire the current position (second time)
23 '(7) Raise the robot/
24 '(8) Perform step operation until END/
25 GoSub *S10ENC 'P_ENCDLT calculation processing
26 P_EncDlt(MENCNO)=PY10ENC 'Store data in P_ENCDLT
27 End
28 '
29 '##### Processing for obtaining P_ENCDLT #####
30 'MX10EC1: Encoder data 1
31 'MX10EC2: Encoder data 2
32 'PX10PS1: Position 1
33 'PX10PS2: Position 2
34 'PY10ENC: P_ENCDLT value
35 *S10ENC
36 M10ED#=MX10EC2#-MX10EC1#
37 If M10ED#>800000000.0# Then M10ED#=M10ED#-1000000000.0#
38 If M10ED#<-800000000.0# Then M10ED#=M10ED#+1000000000.0#
39 PY10ENC.X=(PX10PS2.X-PX10PS1.X)/M10ED#
40 PY10ENC.Y=(PX10PS2.Y-PX10PS1.Y)/M10ED#
41 PY10ENC.Z=(PX10PS2.Z-PX10PS1.Z)/M10ED#
42 PY10ENC.A=(PX10PS2.A-PX10PS1.A)/M10ED#
43 PY10ENC.B=(PX10PS2.B-PX10PS1.B)/M10ED#
44 PY10ENC.C=(PX10PS2.C-PX10PS1.C)/M10ED#
45 PY10ENC.L1=(PX10PS2.L1-PX10PS1.L1)/M10ED#
46 PY10ENC.L2=(PX10PS2.L2-PX10PS1.L2)/M10ED#
47 Return
48 '
49 'This program “computes how much a robot moves per 1 pulse and stores the result in P_ENCDLT."
PE=(+1.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)(0,0)
PX10PS1=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)(0,0)
PX10PS2=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)(0,0)
PY10ENC=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)(0,0)
Sample Programs 21-125