-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathcia402.hal
138 lines (107 loc) · 4.6 KB
/
cia402.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
124
125
126
127
128
129
130
131
132
133
134
135
136
###########################################################
#
# CIA 402 example snippet Hal
#
###########################################################
###########################################################
# Setup
###########################################################
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadusr -W ./lcec_conf ethercat-conf.xml
loadrt lcec
loadrt cia402 count=3
loadrt pid names=s-pid,0-pid,1-pid,2-pid
###########################################################
# Functions servo-thread
###########################################################
addf lcec.read-all servo-thread
addf cia402.0.read-all servo-thread
addf cia402.1.read-all servo-thread
addf cia402.2.read-all servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf s-pid.do-pid-calcs servo-thread
addf 0-pid.do-pid-calcs servo-thread
addf 1-pid.do-pid-calcs servo-thread
addf 2-pid.do-pid-calcs servo-thread
addf cia402.0.write-all servo-thread
addf cia402.1.write-all servo-thread
addf cia402.2.write-all servo-thread
addf lcec.write-all servo-thread
#########################################
#nets
#########################################
net emc-enable => iocontrol.0.emc-enable-in
sets emc-enable 1
#config
#
# Joint 0
#
setp cia402.0.csp-mode 1
setp cia402.0.pos-scale 800
#from servo(ethercat) to cia402
net 0-statusword lcec.0.0.cia-statusword => cia402.0.statusword
net 0-opmode-display lcec.0.0.opmode-display => cia402.0.opmode-display
net 0-drv-act-pos lcec.0.0.actual-position => cia402.0.drv-actual-position
net 0-drv-act-velo lcec.0.0.actual-velocity => cia402.0.drv-actual-velocity
#from motion to cia
net 0-home-index <= joint.0.index-enable => cia402.0.home
net 0-enable <= joint.0.amp-enable-out => cia402.0.enable
net 0-amp-fault => joint.0.amp-fault-in <= cia402.0.drv-fault
net 0-pos-cmd <= joint.0.motor-pos-cmd => cia402.0.pos-cmd
net 0-pos-fb => joint.0.motor-pos-fb <= cia402.0.pos-fb
#net 0-home lcec.0.0.in-5 => joint.0.home-sw-in
#from cia402 to servo(ethercat)
net 0-controlword cia402.0.controlword => lcec.0.0.cia-controlword
net 0-modes-of-operation cia402.0.opmode => lcec.0.0.opmode
net 0-drv-target-pos cia402.0.drv-target-position => lcec.0.0.target-position
net 0-drv-target-velo cia402.0.drv-target-velocity => lcec.0.0.target-velocity
#
# Joint 1
#
setp cia402.1.csp-mode 1
setp cia402.1.pos-scale 800
#from servo(ethercat) to cia402
net 1-statusword lcec.0.1.cia-statusword => cia402.1.statusword
net 1-opmode-display lcec.0.1.opmode-display => cia402.1.opmode-display
net 1-drv-act-pos lcec.0.1.actual-position => cia402.1.drv-actual-position
net 1-drv-act-velo lcec.0.1.actual-velocity => cia402.1.drv-actual-velocity
#from cia402 to servo(ethercat)
net 1-controlword cia402.1.controlword => lcec.0.1.cia-controlword
net 1-modes-of-operation cia402.1.opmode => lcec.0.1.opmode
net 1-drv-target-pos cia402.1.drv-target-position => lcec.0.1.target-position
net 1-drv-target-velo cia402.1.drv-target-velocity => lcec.0.1.target-velocity
#from motion to cia
net 1-enable <= joint.1.amp-enable-out => cia402.1.enable
net 1-amp-fault => joint.1.amp-fault-in <= cia402.1.drv-fault
net 1-pos-cmd <= joint.1.motor-pos-cmd => cia402.1.pos-cmd
net 1-pos-fb => joint.1.motor-pos-fb <= cia402.1.pos-fb
#
# Joint 2
#
setp cia402.2.csp-mode 1
setp cia402.2.pos-scale 2000
#from servo(ethercat) to cia402
net 2-statusword lcec.0.2.cia-statusword => cia402.2.statusword
net 2-opmode-display lcec.0.2.opmode-display => cia402.2.opmode-display
net 2-drv-act-pos lcec.0.2.actual-position => cia402.2.drv-actual-position
net 2-drv-act-velo lcec.0.2.actual-velocity => cia402.2.drv-actual-velocity
#from cia402 to servo(ethercat)
net 2-controlword cia402.2.controlword => lcec.0.2.cia-controlword
net 2-modes-of-operation cia402.2.opmode => lcec.0.2.opmode
net 2-drv-target-pos cia402.2.drv-target-position => lcec.0.2.target-position
net 2-drv-target-velo cia402.2.drv-target-velocity => lcec.0.2.target-velocity
#from motion to cia
net 2-enable <= joint.2.amp-enable-out => cia402.2.enable
net 2-amp-fault => joint.2.amp-fault-in <= cia402.2.drv-fault
net 2-pos-cmd <= joint.2.motor-pos-cmd => cia402.2.pos-cmd
net 2-pos-fb => joint.2.motor-pos-fb <= cia402.2.pos-fb
#
# Slave 3 I/O module - RTEC1616
#
#Outputs
#M3,M4,M5
net spindle-on spindle.0.on => lcec.0.3.out-01
net mist-on iocontrol.0.coolant-mist => lcec.0.3.out-02
net coolant-on => iocontrol.0.coolant-flood => lcec.0.3.out-03