blob: 83cb509f0b41ee9dff9b7290e8390981200ebb4d (
plain)
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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
|
#########################################################################
# Loading real-time modules #
#########################################################################
loadrt trivkins
loadrt hal_gm
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
loadrt pid num_chan=3
loadrt not
loadusr halmeter
#loadusr halscope -f
#########################################################################
# Adding functions #
#########################################################################
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf gm.0.read servo-thread -1
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 gm.0.write servo-thread -1
addf gm.0.RS485 servo-thread -1
addf not.0 servo-thread
#########################################################################
# Card mgt #
#########################################################################
setp gm.0.watchdog-enable 1
setp gm.0.watchdog-timeout-ns 3000000
net pwrEnable motion.motion-enabled => gm.0.power-enable
# create power fault loopback
net hw_fault gm.0.power-fault => not.0.in
net estop not.0.out => iocontrol.0.emc-enable-in
# connect enable signals for DAC and PID
net Xen axis.0.amp-enable-out => gm.0.dac.0.enable
net Xen => pid.0.enable
net Yen axis.1.amp-enable-out => gm.0.dac.1.enable
net Yen => pid.1.enable
net Zen axis.2.amp-enable-out => gm.0.dac.2.enable
net Zen => pid.2.enable
#########################################################################
# Encoder setup #
#########################################################################
# ################
# X [0] Axis
# ################
# encoder feedback
setp gm.0.encoder.0.counter-mode 0 #0:quad, 1:stepDir
setp gm.0.encoder.0.index-enable 0 #0:disable, 1:enable
setp gm.0.encoder.0.index-mode 1 #0:reset counter at index, 1:round position at index
setp gm.0.encoder.0.counts-per-rev 2000
setp gm.0.encoder.0.position-scale [AXIS_0]INPUT_SCALE
setp gm.0.encoder.0.min-speed-estimate 0.1
# ################
# Y [1] Axis
# ################
# encoder feedback
setp gm.0.encoder.1.counter-mode 0 #0:quad, 1:stepDir
setp gm.0.encoder.1.index-enable 0 #0:disable, 1:enable
setp gm.0.encoder.1.index-mode 1 #0:reset counter at index, 1:round position at index
setp gm.0.encoder.1.counts-per-rev 2000
setp gm.0.encoder.1.position-scale [AXIS_2]INPUT_SCALE
setp gm.0.encoder.1.min-speed-estimate 0.1
# ################
# Z [2] Axis
# ################
# encoder feedback
setp gm.0.encoder.2.counter-mode 0 #0:quad, 1:stepDir
setp gm.0.encoder.2.index-enable 0 #0:disable, 1:enable
setp gm.0.encoder.2.index-mode 1 #0:reset counter at index, 1:round position at index
setp gm.0.encoder.2.counts-per-rev 2000
setp gm.0.encoder.2.position-scale [AXIS_2]INPUT_SCALE
setp gm.0.encoder.2.min-speed-estimate 0.1
#########################################################################
# PID setup #
#########################################################################
# ################
# X [0] Axis
# ################
# set PID loop gains from inifile
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
setp pid.0.bias [AXIS_0]BIAS
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
setp pid.0.FF2 [AXIS_0]FF2
setp pid.0.deadband [AXIS_0]DEADBAND
setp pid.0.maxoutput [AXIS_0]MAX_OUTPUT
# ################
# Y [1] Axis
# ################
# set PID loop gains from inifile
setp pid.1.Pgain [AXIS_1]P
setp pid.1.Igain [AXIS_1]I
setp pid.1.Dgain [AXIS_1]D
setp pid.1.bias [AXIS_1]BIAS
setp pid.1.FF0 [AXIS_1]FF0
setp pid.1.FF1 [AXIS_1]FF1
setp pid.1.FF2 [AXIS_1]FF2
setp pid.1.deadband [AXIS_1]DEADBAND
setp pid.1.maxoutput [AXIS_1]MAX_OUTPUT
# ################
# Z [2] Axis
# ################
# set PID loop gains from inifile
setp pid.2.Pgain [AXIS_2]P
setp pid.2.Igain [AXIS_2]I
setp pid.2.Dgain [AXIS_2]D
setp pid.2.bias [AXIS_2]BIAS
setp pid.2.FF0 [AXIS_2]FF0
setp pid.2.FF1 [AXIS_2]FF1
setp pid.2.FF2 [AXIS_2]FF2
setp pid.2.deadband [AXIS_2]DEADBAND
setp pid.2.maxoutput [AXIS_2]MAX_OUTPUT
#########################################################################
# position reference and feedback connection #
#########################################################################
# connect encoder feedback to PID
net Xpos-fb gm.0.encoder.0.position => pid.0.feedback
net Ypos-fb gm.0.encoder.1.position => pid.1.feedback
net Zpos-fb gm.0.encoder.2.position => pid.2.feedback
# connect position commands from motion module to PID
net Xpos-cmd axis.0.motor-pos-cmd => pid.0.command
net Ypos-cmd axis.1.motor-pos-cmd => pid.1.command
net Zpos-cmd axis.2.motor-pos-cmd => pid.2.command
# connect PID output to DAC
net Xdac-cmd pid.0.output => gm.0.dac.0.value
net Ydac-cmd pid.1.output => gm.0.dac.1.value
net Zdac-cmd pid.2.output => gm.0.dac.2.value
# connect position feedback
net Xpos-cmd => axis.0.motor-pos-fb
net Ypos-cmd => axis.1.motor-pos-fb
net Zpos-cmd => axis.2.motor-pos-fb
#########################################################################
# homing and limit switches #
#########################################################################
net lim-sw-x-pos gm.0.axis.0.pos-lim-sw-in-not => axis.0.pos-lim-sw-in
net lim-sw-x-neg gm.0.axis.0.neg-lim-sw-in-not => axis.0.neg-lim-sw-in
net lim-sw-x-home gm.0.axis.0.home-sw-in-not => axis.0.home-sw-in
net lim-sw-y-pos gm.0.axis.1.pos-lim-sw-in-not => axis.1.pos-lim-sw-in
net lim-sw-y-neg gm.0.axis.1.neg-lim-sw-in-not => axis.1.neg-lim-sw-in
net lim-sw-y-home gm.0.axis.1.home-sw-in-not => axis.1.home-sw-in
net lim-sw-z-pos gm.0.axis.2.pos-lim-sw-in-not => axis.2.pos-lim-sw-in
net lim-sw-z-neg gm.0.axis.2.neg-lim-sw-in-not => axis.2.neg-lim-sw-in
net lim-sw-z-home gm.0.axis.2.home-sw-in-not => axis.2.home-sw-in
|
