blob: a4f1b12959ae561b7dc52a2be59ead208c8de5e6 (
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
|
# load realtime modules
# kinematics
loadrt trivkins
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
loadrt hal_parport cfg="0x378"
loadrt encoder num_chan=2
loadrt pid num_chan=2
loadrt pwmgen output_type=1,1
loadrt ddt count=4
loadrt constant count=1
# define the order of execution for RT code
addf parport.0.read base-thread
addf encoder.update-counters base-thread
addf pwmgen.make-pulses base-thread
addf parport.0.write base-thread
addf encoder.capture-position servo-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 constant.0 servo-thread
addf pwmgen.update servo-thread
addf ddt.0 servo-thread
addf ddt.1 servo-thread
addf ddt.2 servo-thread
addf ddt.3 servo-thread
# hook stuff together
setp encoder.0.position-scale [AXIS_0]INPUT_SCALE
setp encoder.1.position-scale [AXIS_1]INPUT_SCALE
net enc0A encoder.0.phase-A <= parport.0.pin-10-in
net enc0B encoder.0.phase-B <= parport.0.pin-11-in
net enc1A encoder.1.phase-A <= parport.0.pin-12-in
net enc1B encoder.1.phase-B <= parport.0.pin-13-in
net Xpos-fb <= encoder.0.position
net Ypos-fb <= encoder.1.position
net Xpos-fb => pid.0.feedback
net Ypos-fb => pid.1.feedback
net Xpos-fb => axis.0.motor-pos-fb
net Ypos-fb => axis.1.motor-pos-fb
net Xvel-cmd <= pid.0.output
net Yvel-cmd <= pid.1.output
net Xvel-cmd => pwmgen.0.value
net Yvel-cmd => pwmgen.1.value
net Xpwm pwmgen.0.pwm
net Ypwm pwmgen.1.pwm
net Xdir <= pwmgen.0.dir
net Ydir <= pwmgen.1.dir
net Xdir => parport.0.pin-02-out
net Xdir => parport.0.pin-04-out
setp parport.0.pin-04-out-invert TRUE
net Xpwm => parport.0.pin-03-out
net Ydir => parport.0.pin-05-out
net Ydir => parport.0.pin-07-out
setp parport.0.pin-07-out-invert TRUE
net Ypwm => parport.0.pin-06-out
setp pid.0.maxoutput 1.0
setp pid.1.maxoutput 1.0
# 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
net Xpos-cmd <= axis.0.motor-pos-cmd
net Ypos-cmd <= axis.1.motor-pos-cmd
net Xpos-cmd => pid.0.command
net Ypos-cmd => pid.1.command
net Xenable <= axis.0.amp-enable-out
net Yenable <= axis.1.amp-enable-out
net Xenable => pid.0.enable
net Xenable => pwmgen.0.enable
net Yenable => pid.1.enable
net Yenable => pwmgen.1.enable
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
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 tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
|
