forked from scottalford75/Remora
/
remora.hal
96 lines (62 loc) · 2.95 KB
/
remora.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
# load the real-time components
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadrt remora ctrl_type=v,p,p,p,p
loadrt PRUencoder names=encoderJ0,encoderJ1
loadrt pid names=j0pid
# estop loopback, SPI comms enable and feedback
net user-enable-out <= iocontrol.0.user-enable-out => remora.SPI-enable
net user-request-enable <= iocontrol.0.user-request-enable => remora.SPI-reset
net remora-status <= remora.SPI-status => iocontrol.0.emc-enable-in
# add the remora and motion functions to threads
addf remora.read servo-thread
addf PRUencoder.capture-position servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf j0pid.do-pid-calcs servo-thread
addf remora.update-freq servo-thread
addf remora.write servo-thread
# Joint 0 setup
setp remora.joint.0.scale [JOINT_0]SCALE
setp remora.joint.0.maxaccel [JOINT_0]STEPGEN_MAXACCEL
setp encoderJ0.position-scale [JOINT_0_ENCODER]SCALE
net j0enable <= joint.0.amp-enable-out => remora.joint.0.enable
net j0enable => j0pid.enable
net encoderJ0-count => encoderJ0.raw_count
net j0pos-fb <= encoderJ0.position => j0pid.feedback
net j0pos-fb => joint.0.motor-pos-fb
net j0pos-cmd <= joint.0.motor-pos-cmd => j0pid.command
net j0pid-output <= j0pid.output => remora.joint.0.vel-cmd
setp j0pid.Pgain [JOINT_0]P
setp j0pid.Igain [JOINT_0]I
setp j0pid.Dgain [JOINT_0]D
setp j0pid.bias [JOINT_0]BIAS
setp j0pid.FF0 [JOINT_0]FF0
setp j0pid.FF1 [JOINT_0]FF1
setp j0pid.FF2 [JOINT_0]FF2
setp j0pid.deadband [JOINT_0]DEADBAND
setp j0pid.maxoutput [JOINT_0]MAX_VELOCITY
# Joint 1 setup
setp remora.joint.1.scale [JOINT_1]SCALE
setp remora.joint.1.maxaccel [JOINT_1]STEPGEN_MAXACCEL
net j1pos-cmd <= joint.1.motor-pos-cmd => remora.joint.1.pos-cmd
net j1pos-fb <= remora.joint.1.pos-fb => joint.1.motor-pos-fb
net j1enable <= joint.1.amp-enable-out => remora.joint.1.enable
# Joint 2 setup
setp remora.joint.2.scale [JOINT_2]SCALE
setp remora.joint.2.maxaccel [JOINT_2]STEPGEN_MAXACCEL
net j2pos-cmd <= joint.2.motor-pos-cmd => remora.joint.2.pos-cmd
net j2pos-fb <= remora.joint.2.pos-fb => joint.2.motor-pos-fb
net j2enable <= joint.2.amp-enable-out => remora.joint.2.enable
# Joint 3 setup
setp remora.joint.3.scale [JOINT_3]SCALE
setp remora.joint.3.maxaccel [JOINT_3]STEPGEN_MAXACCEL
net j3pos-cmd <= joint.3.motor-pos-cmd => remora.joint.3.pos-cmd
net j3pos-fb <= remora.joint.3.pos-fb => joint.3.motor-pos-fb
net j3enable <= joint.3.amp-enable-out => remora.joint.3.enable
# Joint 4 setup
setp remora.joint.4.scale [JOINT_4]SCALE
setp remora.joint.4.maxaccel [JOINT_4]STEPGEN_MAXACCEL
net j4pos-cmd <= joint.4.motor-pos-cmd => remora.joint.4.pos-cmd
net j4pos-fb <= remora.joint.4.pos-fb => joint.4.motor-pos-fb
net j4enable <= joint.4.amp-enable-out => remora.joint.4.enable