96 lines
2.6 KiB
Plaintext
96 lines
2.6 KiB
Plaintext
# HAL config file for servos -- expanded from core_servo.hal
|
|
# for a full four axis setup
|
|
|
|
# create four position feedback signals
|
|
|
|
# connect position feedback to PID loop
|
|
net Xpos-fb => pid.0.feedback
|
|
net Ypos-fb => pid.1.feedback
|
|
net Zpos-fb => pid.2.feedback
|
|
net Apos-fb => pid.3.feedback
|
|
|
|
# connect position feedback to motion module
|
|
net Xpos-fb => joint.0.motor-pos-fb
|
|
net Ypos-fb => joint.1.motor-pos-fb
|
|
net Zpos-fb => joint.2.motor-pos-fb
|
|
net Apos-fb => joint.3.motor-pos-fb
|
|
|
|
# create PID to DAC output signals
|
|
|
|
# connect output signals to output of PID loops
|
|
net Xoutput <= pid.0.output
|
|
net Youtput <= pid.1.output
|
|
net Zoutput <= pid.2.output
|
|
net Aoutput <= pid.3.output
|
|
|
|
# set PID loop output limits to +/-1.00
|
|
setp pid.0.maxoutput [JOINT_0]PID_MAX_VEL
|
|
setp pid.1.maxoutput [JOINT_1]PID_MAX_VEL
|
|
setp pid.2.maxoutput [JOINT_2]PID_MAX_VEL
|
|
setp pid.3.maxoutput [JOINT_3]PID_MAX_VEL
|
|
|
|
# set PID loop gains
|
|
setp pid.0.Pgain [JOINT_0]P
|
|
setp pid.0.Igain [JOINT_0]I
|
|
setp pid.0.Dgain [JOINT_0]D
|
|
setp pid.0.bias [JOINT_0]BIAS
|
|
setp pid.0.FF0 [JOINT_0]FF0
|
|
setp pid.0.FF1 [JOINT_0]FF1
|
|
setp pid.0.FF2 [JOINT_0]FF2
|
|
setp pid.0.deadband [JOINT_0]DEADBAND
|
|
|
|
setp pid.1.Pgain [JOINT_1]P
|
|
setp pid.1.Igain [JOINT_1]I
|
|
setp pid.1.Dgain [JOINT_1]D
|
|
setp pid.1.bias [JOINT_1]BIAS
|
|
setp pid.1.FF0 [JOINT_1]FF0
|
|
setp pid.1.FF1 [JOINT_1]FF1
|
|
setp pid.1.FF2 [JOINT_1]FF2
|
|
setp pid.1.deadband [JOINT_1]DEADBAND
|
|
|
|
setp pid.2.Pgain [JOINT_2]P
|
|
setp pid.2.Igain [JOINT_2]I
|
|
setp pid.2.Dgain [JOINT_2]D
|
|
setp pid.2.bias [JOINT_2]BIAS
|
|
setp pid.2.FF0 [JOINT_2]FF0
|
|
setp pid.2.FF1 [JOINT_2]FF1
|
|
setp pid.2.FF2 [JOINT_2]FF2
|
|
setp pid.2.deadband [JOINT_2]DEADBAND
|
|
|
|
setp pid.3.Pgain [JOINT_3]P
|
|
setp pid.3.Igain [JOINT_3]I
|
|
setp pid.3.Dgain [JOINT_3]D
|
|
setp pid.3.bias [JOINT_3]BIAS
|
|
setp pid.3.FF0 [JOINT_3]FF0
|
|
setp pid.3.FF1 [JOINT_3]FF1
|
|
setp pid.3.FF2 [JOINT_3]FF2
|
|
setp pid.3.deadband [JOINT_3]DEADBAND
|
|
|
|
# create four position command signals
|
|
|
|
# connect position commands to motion controller
|
|
net Xpos-cmd <= joint.0.motor-pos-cmd
|
|
net Ypos-cmd <= joint.1.motor-pos-cmd
|
|
net Zpos-cmd <= joint.2.motor-pos-cmd
|
|
net Apos-cmd <= joint.3.motor-pos-cmd
|
|
|
|
# connect position commands to PID input
|
|
net Xpos-cmd => pid.0.command
|
|
net Ypos-cmd => pid.1.command
|
|
net Zpos-cmd => pid.2.command
|
|
net Apos-cmd => pid.3.command
|
|
|
|
# create bit signals to enable/disable the PID loops
|
|
|
|
# connect the signals to the motion controller
|
|
net Xenable <= joint.0.amp-enable-out
|
|
net Yenable <= joint.1.amp-enable-out
|
|
net Zenable <= joint.2.amp-enable-out
|
|
net Aenable <= joint.3.amp-enable-out
|
|
|
|
# connect the signals to the PID blocks
|
|
net Xenable => pid.0.enable
|
|
net Yenable => pid.1.enable
|
|
net Zenable => pid.2.enable
|
|
net Aenable => pid.3.enable
|