/
LcncMill.hal
123 lines (97 loc) · 4.24 KB
/
LcncMill.hal
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
# load the realtime components
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS num_aio=13
loadrt Lcnc ipaddr="192.168.2.50" udpport="1234"
loadrt pid num_chan=3
loadusr -W hal_manualtoolchange
loadrt debounce cfg=3
loadrt not
loadrt and2
# estop loopback
net user-enable-out <= iocontrol.0.user-enable-out => Lcnc.00.enable
net user-request-enable <= iocontrol.0.user-request-enable => Lcnc.00.enable-request
net Lcnc-status <= Lcnc.00.enabled => and2.0.in0
net Drvr-alarm-n <= Lcnc.00.input.03.in-n => and2.0.in1 # spostato su J5-4
net Lcnc-and-drv-ok <= and2.0.out => iocontrol.0.emc-enable-in
net Lcnc-and-drv-ok => Lcnc.00.output.06.out #drivers power enable
net Lcnc-and-drv-ok => not.0.in
net Lcnc-status-n <= not.0.out
net Lcnc-status-n => Lcnc.00.stepgen.00.reset
net Lcnc-status-n => Lcnc.00.stepgen.01.reset
net Lcnc-status-n => Lcnc.00.stepgen.02.reset
# add Lcnc and motion functions to threads
addf Lcnc.00.update servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf debounce.0 servo-thread
addf pid.0.do-pid-calcs servo-thread
addf pid.1.do-pid-calcs servo-thread
addf pid.2.do-pid-calcs servo-thread
addf not.0 servo-thread
addf and2.0 servo-thread
# stepgen setup, times in ns
setp Lcnc.00.stepgen-dir_width 1000
setp Lcnc.00.stepgen-setup_time 1000
setp Lcnc.00.stepgen-step_space 500
setp Lcnc.00.stepgen-step_width 500
# Joint 0 setup
setp Lcnc.00.stepgen.00.scale [JOINT_0]SCALE
setp Lcnc.00.stepgen.00.acc_lim [JOINT_0]STEPGEN_MAXACCEL
setp pid.0.FF1 1.0
setp pid.0.Pgain 200
setp pid.0.Igain 20
setp pid.0.Dgain 0
setp pid.0.deadband 0.01
net J0enable-out <= joint.0.amp-enable-out
net J0enable-out => pid.0.enable
net J0enable-out => Lcnc.00.stepgen.00.enable
net j0mot-pos-cmd <= joint.0.motor-pos-cmd => pid.0.command
net j0pos-fb <= Lcnc.00.stepgen.00.pos-fb => pid.0.feedback joint.0.motor-pos-fb
net pid0out pid.0.output => Lcnc.00.stepgen.00.vel-cmd
# Joint 1 setup
setp Lcnc.00.stepgen.01.scale [JOINT_1]SCALE
setp Lcnc.00.stepgen.01.acc_lim [JOINT_1]STEPGEN_MAXACCEL
setp pid.1.FF1 1.0
setp pid.1.Pgain 200
setp pid.1.Igain 20
setp pid.1.Dgain 0
setp pid.1.deadband 0.01
net J1enable-out <= joint.1.amp-enable-out
net J1enable-out => pid.1.enable
net J1enable-out => Lcnc.00.stepgen.01.enable
net j1mot-pos-cmd <= joint.1.motor-pos-cmd => pid.1.command
net j1pos-fb <= Lcnc.00.stepgen.01.pos-fb => pid.1.feedback joint.1.motor-pos-fb
net pid1out pid.1.output => Lcnc.00.stepgen.01.vel-cmd
# Joint 2 setup
setp Lcnc.00.stepgen.02.scale [JOINT_2]SCALE
setp Lcnc.00.stepgen.02.acc_lim [JOINT_2]STEPGEN_MAXACCEL
setp pid.2.FF1 1.0
setp pid.2.Pgain 200
setp pid.2.Igain 20
setp pid.2.Dgain 0
setp pid.2.deadband 0.01
net J2enable-out <= joint.2.amp-enable-out
net J2enable-out => pid.2.enable
net J2enable-out => Lcnc.00.stepgen.02.enable
net j2mot-pos-cmd <= joint.2.motor-pos-cmd => pid.2.command
net j2pos-fb <= Lcnc.00.stepgen.02.pos-fb => pid.2.feedback joint.2.motor-pos-fb
net pid2out pid.2.output => Lcnc.00.stepgen.02.vel-cmd
# end-stops
net X-stop Lcnc.00.input.00.in-n => joint.0.home-sw-in
net Y-stop Lcnc.00.input.01.in-n => joint.1.home-sw-in
net Z-stop Lcnc.00.input.02.in-n => joint.2.home-sw-in
# outputs
net cool-flood iocontrol.0.coolant-flood => Lcnc.00.output.04.out
net cool-mist iocontrol.0.coolant-mist => Lcnc.00.output.05.out
# spindle setup
setp Lcnc.00.pwm.00.freq 5000
setp Lcnc.00.pwm.00.scale 1500
setp Lcnc.00.pwm.00.enable true
net spindle-speed-cmd spindle.0.speed-out-abs => Lcnc.00.pwm.00.value
net spindle-enable spindle.0.on => Lcnc.00.output.01.out
net spindle-dir spindle.0.reverse => Lcnc.00.output.00.out
# toolchange setup
net tool-change iocontrol.0.tool-change => hal_manualtoolchange.change
net tool-changed iocontrol.0.tool-changed => hal_manualtoolchange.changed
net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared