Mitsubishi Electric MELFA BFP-A3614, Manual

Page: 136 / 212

-125-
( → Continuation of program example in Exercise 1-1 Hand insertion and pulling out operation)
(// End of program example of Exercise 1-1 Hand insertion and pulling out operation)
37 PB.Z=PHome.Z
38 Mov PB
39 Fine 0.1,P
40 Mvs P2
41 Fine 0
42 Dly 0.2
43 Fsc On, 0, 0, 1 'Start force sense control
44 *Loop1
45 M_Timer(2)=0
46 PB=P_Fbc
47 *Loop2
48 Dly 0.1
49 PNow=P_Fbc
50 M1=Zone2(PNow,PB,PB,0.2)
51 If ((M_Timer(2) > 5000) And (M1=1)) Then *Fin1
52 If M_Timer(2) <= 5000 Then *Loop2 Else *Loop1
53 *Fin1
54 Fsc Off
55 Dly 0.1
56 Mjudge=P_Fbc.Z 'Insert judgment condition (Z)
57 '-----------------------
58 '----- Withdraw -----
59 P_FsFCd0 = (+0.00,+0.00,-10.00,+0.00,+0.00,+0.00)(0,0)
'Force command
60 Dly 0.2
61 Fsc On, 0, 0, 0 'Start force sense control
62 *Loop3
63 Dly 0.1
64 If P_Fbc.Z < P2.Z Then *Loop3
65 Fsc Off
66 Dly 0.1
67 Mvs P_Fbc+(+0.00,+0.00,+30.00)
68 Mov PHome
69 Hlt
70 End
PHome=(+270.00,+0.00,+320.00,+180.00,+0.00,+180.00,+0.00,+0.00)(7,0)
P2=(+351.07,+38.03,+107.52,-180.00,+0.00,-180.00)(7,0)