-
Notifications
You must be signed in to change notification settings - Fork 16
/
Copy pathcia402.comp
310 lines (261 loc) · 10.7 KB
/
cia402.comp
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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
component cia402 "HAL CiA402 Drive interface layer";
//
// Copyright (C) 2019 Dominik Braun <[email protected]>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
author "Dominik Braun";
license "GPL";
description """
HAL Interface for CiA402 Devices,
this component acts as a glue layer between hardware to Hal modules like Ethercat, CAN-Bus or others.\n
It translates raw IO Data from the PDOs to the common linuxcnc Hal pin structure and has build in logic\n
for the CiA402 State Control, feedback handling, external homing and build in scaling functions.\n
It delivers two functions: read_all and write_all.\n
The concept of integration in the correspondending task should be as following: \n
################### ########### ######### ############ #####################\n
#HARDWARE INPUT # # CiA402 # #Motion # # CiA402 # # Hardware Output #\n
# like #-->>--#read_all #-->>>--#Pids #-->--#write_all #-->>--# like #\n
#Ethercat read_all# # etc. # # # # # # Ethercat write_all#\n
################### ########### ######### ############ #####################\n
\n
Hal Example:\n
#########\n
# Setup\n
#########\n
loadrt [KINS]KINEMATICS\n
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS\n
(loadusr -W lcec_conf ethercat-conf.xml)\n
loadrt lcec\n
loadrt cia402 count=3\n
loadrt pid names=x-pid,y-pid,z-pid\n
##########################\n
# Functions servo-thread\n
##########################\n
addf lcec.read-all servo-thread\n
addf cia402.0.read-all servo-thread\n
addf cia402.1.read-all servo-thread\n
addf cia402.2.read-all servo-thread\n
addf motion/ PIDs / PCL / etc .\n
addf cia402.0.write-all servo-thread\n
addf cia402.1.write-all servo-thread\n
addf cia402.2.write-all servo-thread\n
addf lcec.write-all servo-thread\n
#########################################\n
#nets .....\n
By default the component is set to CSP Mode,\n
CSV Mode could be selected with an:\n
setp cia402.0.csp-mode 0 in hal.\n
Mode changing in runtime is currently not supported, to avoid\n
unwanted behaviour.\n
For using the servo drives internal homing procedure configure your\n
joint homing to Home on Index Pulse only and connect the components\n
home Input to the index-enable Pin:\n
HOME_SEARCH_VEL = 0.0\n
HOME_LATCH_VEL = 0.2 (Any value but zero, the homing speed is predetermined by the drives configured speed\n
HOME_USE_INDEX = TRUE\n
If you are using PIDs, don't forget to connect the PIDs index-enable pin.\n
Even though this component exports many pins, you can choose which functions you want to use:\n
If you would like to use the CiA State Machine, connect Statusword and Controlword.\n
For single use of the scaling function connect only the fb and cmd pins from position or velocity.\n
If no Drives homing is needed, let the Pins unconnected.""";
//CiA IOs
//inputs from drive
pin in unsigned statusword "Drives CiA402 Statusword, index 0x6041";
pin in signed opmode_display "Drives Modes of Operation feedback register, index 0x6061";
pin in signed drv_actual_position "Drives actual Position, index 0x6064";
pin in signed drv_actual_velocity "Drives actual Velocity, index 0x606C";
//outputs to drive
pin out unsigned controlword "Drives CIA402 Controlword, index 0x6040";
pin out signed opmode "Drives Modes of Operation Input, index 0x6060";
pin out signed drv_target_position "Position command to the drive, index 0x607A";
pin out signed drv_target_velocity "Velocity command to the drive, index 0x60FF";
//Control IOs
pin in bit enable "true enables the Drive";
pin in float pos_cmd "target Position, from Motion or PID";
pin in float velocity_cmd "target Velocity, from Motion or PID";
pin out float pos_fb "Position feedback, scaled";
pin out float velocity_fb "Velocity feedback scaled";
pin out bit drv_fault "true indicates an Drive Error";
//Homing IOs
pin io bit home "true starts the Drives internal home procedure, connect this to joint.x.index-enable pin";
pin out bit stat_homed "true indicates that the Drive is internaly homed";
pin out bit stat_homing "true indicates that the Drives homing procedure is running";
//Auxilary IOs
pin out bit stat_switchon_ready "Drive in CiA State: ready to switch on";
pin out bit stat_switched_on "Drive in CiA State: switched on";
pin out bit stat_op_enabled "Drive in CiA State: Operation enabled";
pin out bit stat_voltage_enabled "Drive in CiA State: switched on, Voltage enabled";
pin out bit stat_fault "Drive hast the fault bit set";
pin out bit stat_quick_stop "Drive in State: Quick Stop";
pin out bit stat_switchon_disabled "Drive in CiA State: Switch on disabled";
pin out bit stat_warning "Drive has the warning bit set";
pin out bit stat_remote "Drive in State remote / bus operation";
pin out bit stat_target_reached "Drive has reached target position / velocity";
pin out bit opmode_no_mode "Drive has no operation Mode set";
pin out bit opmode_homing "Drive in Mode Homing";
pin out bit opmode_cyclic_position "Drive in Mode Cyclic synchronus Position";
pin out bit opmode_cyclic_velocity "Drive in Mode Cyclic synchronus Velocity";
pin in bit fault_reset "true, sends fault reset command to the Drive";
//parameters
param rw float pos_scale "increments per machine unit";
param rw float velo_scale "velocity in machine units per 1 Motor revolution";
param rw bit auto_fault_reset "true resets an actual Drive Fault automatically at the next enable Signal";
param rw bit csp_mode "true= CS Position Mode, false= CS Velocity Mode, is only recognized at Linuxcnc Startup, default true";
//internals
variable float pos_scale_old;
variable float velo_scale_old;
variable bool enable_old;
variable bool stat_homed_old;
variable bool stat_fault_old;
variable double pos_scale_rcpt;
variable double velo_scale_rcpt;
variable bool pos_mode;
variable bool init_pos_mode = 0;
variable long auto_fault_reset_delay;
//declare functions
function read_all;
function write_all;
option extra_setup yes;
;;
//constants
#define FAULT_AUTORESET_DELAY_NS 100000000LL
#define OPMODE_CYCLIC_POSITION 8
#define OPMODE_CYCLIC_VELOCITY 9
#define OPMODE_HOMING 6
#define OPMODE_NONE 0
EXTRA_SETUP () {
// initialize variables
pos_scale = 1.0;
pos_scale_old = pos_scale + 1.0;
pos_scale_rcpt = 1.0;
velo_scale = 1.0;
velo_scale_old = velo_scale + 1.0;
velo_scale_rcpt = 1.0;
auto_fault_reset = 1;
csp_mode = 1;
return 0;
}
void check_scales(hal_float_t *scale,float *scale_old, double *scale_rcpt) {
// check for change in scale value
if (*scale != *scale_old) {
// scale value has changed, test and update it
if ((*scale < 1e-20) && (*scale > -1e-20)) {
// value too small, divide by zero is a bad thing
*scale = 1.0;
}
// save new scale to detect future changes
*scale_old = *scale;
// we actually want the reciprocal
*scale_rcpt = 1.0 / *scale;
}
}
FUNCTION(read_all) {
// check for change in scale value
check_scales(&pos_scale, &pos_scale_old, &pos_scale_rcpt);
check_scales(&velo_scale, &velo_scale_old, &velo_scale_rcpt);
// read position feedback
pos_fb = ((double)drv_actual_position) * pos_scale_rcpt;
// read velocity feedback
velocity_fb = ((double)drv_actual_velocity) * velo_scale_rcpt;
// read Modes of Operation
opmode_no_mode = (opmode_display == OPMODE_NONE);
opmode_homing = (opmode_display == OPMODE_HOMING);
opmode_cyclic_velocity = (opmode_display == OPMODE_CYCLIC_VELOCITY);
opmode_cyclic_position = (opmode_display == OPMODE_CYCLIC_POSITION);
// read status
stat_switchon_ready = (statusword >> 0) & 1;
stat_switched_on = (statusword >> 1) & 1;
stat_op_enabled = (statusword >> 2) & 1;
stat_fault = (statusword >> 3) & 1;
stat_voltage_enabled = (statusword >> 4) & 1;
stat_quick_stop = (statusword >> 5) & 1;
stat_switchon_disabled = (statusword >> 6) & 1;
stat_warning = (statusword >> 7) & 1;
stat_remote = (statusword >> 9) & 1;
if (opmode_cyclic_position || opmode_cyclic_velocity) {
stat_target_reached = (statusword >> 10) & 1;
} else {
stat_target_reached = 0;
}
//home states
if (opmode_homing) {
stat_homed = ((statusword >> 10) & 1) && ((statusword >> 12) & 1);
stat_homing = !stat_homed && !((statusword >> 10) & 1);
}
// update fault output
if (auto_fault_reset_delay > 0) {
auto_fault_reset_delay -= period;
drv_fault = 0;
} else {
drv_fault = stat_fault && enable;
}
}
FUNCTION(write_all) {
int enable_edge;
//init opmode
if (!init_pos_mode) {
pos_mode = csp_mode;
init_pos_mode = 1;
}
// detect enable edge
enable_edge = enable && !enable_old;
enable_old = enable;
// write control register
controlword = (1 << 2); // quick stop is not supported
if (stat_fault) {
home = 0;
if (fault_reset) {
controlword |= (1 << 7); // fault reset
}
if (auto_fault_reset && enable_edge) {
auto_fault_reset_delay = FAULT_AUTORESET_DELAY_NS;
controlword |= (1 << 7); // fault reset
}
} else {
if (enable) {
controlword |= (1 << 1); // enable voltage
if (stat_switchon_ready) {
controlword |= (1 << 0); // switch on
if (stat_switched_on) {
controlword |= (1 << 3); // enable op
}
}
}
}
// write position command
drv_target_position = (int32_t) (pos_cmd * pos_scale);
// write velocity command
drv_target_velocity = (int32_t) (velocity_cmd * velo_scale);
// reset home command
if (home && (stat_homed && !stat_homed_old) && opmode_homing) {
home = 0;
}
stat_homed_old = stat_homed;
// OP Mode
// set to position mode
if (stat_voltage_enabled && !home ) {
opmode = OPMODE_CYCLIC_POSITION;
}
// set velo mode
if (stat_voltage_enabled && !pos_mode && !home) {
opmode = OPMODE_CYCLIC_VELOCITY;
}
// mode Home and start homing
if (home) {
opmode = OPMODE_HOMING;
controlword |= (home << 4);
}
}