75 lines
2.2 KiB
Plaintext
Executable File
75 lines
2.2 KiB
Plaintext
Executable File
# core HAL config file for servos
|
|
|
|
# first load the core RT modules that will be needed
|
|
# kinematics
|
|
loadrt [KINS]KINEMATICS
|
|
# motion controller, get name and thread periods from INI file
|
|
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
|
|
# 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 joint.0.motor-pos-fb => pid.0.feedback
|
|
net Ypos-fb joint.1.motor-pos-fb => pid.1.feedback
|
|
net Zpos-fb joint.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 [JOINT_0]MAX_VELOCITY
|
|
setp pid.1.maxoutput [JOINT_1]MAX_VELOCITY
|
|
setp pid.2.maxoutput [JOINT_2]MAX_VELOCITY
|
|
|
|
# set PID loop gains from INI file
|
|
|
|
# the values below come from the INI
|
|
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
|
|
# deadband should be just over 1 count
|
|
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
|
|
# deadband should be just over 1 count
|
|
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
|
|
# deadband should be just over 1 count
|
|
setp pid.2.deadband [JOINT_2]DEADBAND
|
|
|
|
# position command signals
|
|
net Xpos-cmd joint.0.motor-pos-cmd => pid.0.command
|
|
net Ypos-cmd joint.1.motor-pos-cmd => pid.1.command
|
|
net Zpos-cmd joint.2.motor-pos-cmd => pid.2.command
|
|
|
|
# joint enable signals
|
|
net Xenable joint.0.amp-enable-out => pid.0.enable
|
|
net Yenable joint.1.amp-enable-out => pid.1.enable
|
|
net Zenable joint.2.amp-enable-out => pid.2.enable
|