blob: 476cf68db3bbc816424fb26cce472286e5c0f360 (
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
|
# core HAL config file for servos
# first load the core RT modules that will be needed
# kinematics
loadrt trivkins
# motion controller, get name and thread periods from ini file
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
# PID module, for three PID loops
loadrt pid num_chan=3
# hook functions to realtime thread
addf motion-command-handler servo-thread
addf motion-controller 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
# connect position feedback
net Xpos-fb axis.0.motor-pos-fb => pid.0.feedback
net Ypos-fb axis.1.motor-pos-fb => pid.1.feedback
net Zpos-fb axis.2.motor-pos-fb => pid.2.feedback
# create PID to DAC output signals
net Xoutput <= pid.0.output
net Youtput <= pid.1.output
net Zoutput <= pid.2.output
# set PID loop output limits to +/-1.00
setp pid.0.maxoutput [AXIS_0]MAX_VELOCITY
setp pid.1.maxoutput [AXIS_1]MAX_VELOCITY
setp pid.2.maxoutput [AXIS_2]MAX_VELOCITY
# set PID loop gains from inifile
# the values below come from the ini
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
# deadband should be just over 1 count
setp pid.0.deadband [AXIS_0]DEADBAND
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
# deadband should be just over 1 count
setp pid.1.deadband [AXIS_1]DEADBAND
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
# deadband should be just over 1 count
setp pid.2.deadband [AXIS_2]DEADBAND
# position command signals
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
# axis enable signals
net Xenable axis.0.amp-enable-out => pid.0.enable
net Yenable axis.1.amp-enable-out => pid.1.enable
net Zenable axis.2.amp-enable-out => pid.2.enable
|
