blob: 571224738f32378891c11df13bec65c17d6683a5 (
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
|
# core HAL config file for steppers
# 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 base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
# stepper module
loadrt stepgen step_type=0,0,0,0
# hook its functions to realtime threads
addf stepgen.make-pulses base-thread
addf stepgen.capture-position servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf stepgen.update-freq servo-thread
# connect position commands from motion module to step generator
net Xpos-cmd <= axis.0.motor-pos-cmd
net Xpos-cmd => stepgen.0.position-cmd
net Ypos-cmd <= axis.1.motor-pos-cmd
net Ypos-cmd => stepgen.1.position-cmd
net Zpos-cmd <= axis.2.motor-pos-cmd
net Zpos-cmd => stepgen.2.position-cmd
net Apos-cmd <= axis.3.motor-pos-cmd
net Apos-cmd => stepgen.3.position-cmd
# connect position feedback from step generators
# to motion module
net Xpos-fb <= stepgen.0.position-fb
net Xpos-fb => axis.0.motor-pos-fb
net Ypos-fb <= stepgen.1.position-fb
net Ypos-fb => axis.1.motor-pos-fb
net Zpos-fb <= stepgen.2.position-fb
net Zpos-fb => axis.2.motor-pos-fb
net Apos-fb <= stepgen.3.position-fb
net Apos-fb => axis.3.motor-pos-fb
# send the position commands thru differentiators to
# generate velocity and accel signals
# first 8 differentiators
loadrt ddt count=8
# link the differentiator functions into the code
addf ddt.0 servo-thread
addf ddt.1 servo-thread
addf ddt.2 servo-thread
addf ddt.3 servo-thread
addf ddt.4 servo-thread
addf ddt.5 servo-thread
addf ddt.6 servo-thread
addf ddt.7 servo-thread
# define the signals, and hook them up
net Xpos-cmd => ddt.0.in
net Xvel <= ddt.0.out
net Xvel => ddt.1.in
net Xacc <= ddt.1.out
net Ypos-cmd => ddt.2.in
net Yvel <= ddt.2.out
net Yvel => ddt.3.in
net Yacc <= ddt.3.out
net Zpos-cmd => ddt.4.in
net Zvel <= ddt.4.out
net Zvel => ddt.5.in
net Zacc <= ddt.5.out
net Apos-cmd => ddt.6.in
net Avel <= ddt.6.out
net Avel => ddt.7.in
net Aacc <= ddt.7.out
# connect enable signals for step generators
net Xen <= axis.0.amp-enable-out
net Xen => stepgen.0.enable
net Yen <= axis.1.amp-enable-out
net Yen => stepgen.1.enable
net Zen <= axis.2.amp-enable-out
net Zen => stepgen.2.enable
net Aen <= axis.3.amp-enable-out
net Aen => stepgen.3.enable
# connect signals to step pulse generator outputs
net Xstep stepgen.0.step
net Xdir stepgen.0.dir
net Ystep stepgen.1.step
net Ydir stepgen.1.dir
net Zstep stepgen.2.step
net Zdir stepgen.2.dir
net Astep stepgen.3.step
net Adir stepgen.3.dir
# set stepgen module scaling - get values from ini file
setp stepgen.0.position-scale [AXIS_0]SCALE
setp stepgen.1.position-scale [AXIS_1]SCALE
setp stepgen.2.position-scale [AXIS_2]SCALE
setp stepgen.3.position-scale [AXIS_3]SCALE
# set stepgen module accel limits - get values from ini file
setp stepgen.0.maxaccel [AXIS_0]STEPGEN_MAXACCEL
setp stepgen.1.maxaccel [AXIS_1]STEPGEN_MAXACCEL
setp stepgen.2.maxaccel [AXIS_2]STEPGEN_MAXACCEL
setp stepgen.3.maxaccel [AXIS_3]STEPGEN_MAXACCEL
|
