Added linuxcnc test config before video release
This commit is contained in:
Hakan Bastedt
285
linuxcnc/sim.gmoccapy.lathe_configs/hallib/servo_sim.hal
Executable file
285
linuxcnc/sim.gmoccapy.lathe_configs/hallib/servo_sim.hal
Executable file
@@ -0,0 +1,285 @@
|
||||
# HAL config file for simulated servo machine
|
||||
|
||||
# first load all the RT modules that will be needed
|
||||
# kinematics
|
||||
loadrt [KINS]KINEMATICS
|
||||
# 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=[KINS]JOINTS
|
||||
# PID module, for three PID loops
|
||||
loadrt pid names=pid_x,pid_y,pid_z
|
||||
# 6 differentiators (for velocity and accel sigs)
|
||||
loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv
|
||||
# three scale blocks (to simulate motor and leadscrew scaling)
|
||||
loadrt scale names=scale_x,scale_y,scale_z
|
||||
# three lowpass filters (to simulate motor inertia), and nine
|
||||
loadrt lowpass names=lowpass_x,lowpass_y,lowpass_z
|
||||
# window comparators (to simulate limit and home switches)
|
||||
loadrt wcomp names=wcomp_xneg,wcomp_xpos,wcomp_xhome,wcomp_yneg,wcomp_ypos,wcomp_yhome,wcomp_zneg,wcomp_zpos,wcomp_zhome
|
||||
|
||||
# simulated encoders
|
||||
loadrt sim_encoder names=sim_encoder_px,sim_encoder_py,sim_encoder_pz
|
||||
# software encoder counters, 3 for feedback, 3 for actual axis pos
|
||||
loadrt encoder names=encoder_px,encoder_py,encoder_pz,encoder_x,encoder_y,encoder_z
|
||||
# add encoder counter and simulator functions to high speed thread
|
||||
addf sim-encoder.make-pulses base-thread
|
||||
addf encoder.update-counters base-thread
|
||||
|
||||
# add all required functions to servo thread
|
||||
addf encoder.capture-position servo-thread
|
||||
addf wcomp_xneg servo-thread
|
||||
addf wcomp_xpos servo-thread
|
||||
addf wcomp_xhome servo-thread
|
||||
addf wcomp_yneg servo-thread
|
||||
addf wcomp_ypos servo-thread
|
||||
addf wcomp_yhome servo-thread
|
||||
addf wcomp_zneg servo-thread
|
||||
addf wcomp_zpos servo-thread
|
||||
addf wcomp_zhome servo-thread
|
||||
addf motion-command-handler servo-thread
|
||||
addf motion-controller servo-thread
|
||||
addf pid_x.do-pid-calcs servo-thread
|
||||
addf pid_y.do-pid-calcs servo-thread
|
||||
addf pid_z.do-pid-calcs servo-thread
|
||||
addf scale_x servo-thread
|
||||
addf scale_y servo-thread
|
||||
addf scale_z servo-thread
|
||||
addf lowpass_x servo-thread
|
||||
addf lowpass_y servo-thread
|
||||
addf lowpass_z servo-thread
|
||||
addf sim-encoder.update-speed servo-thread
|
||||
|
||||
# link the differentiator functions into the code
|
||||
addf ddt_x servo-thread
|
||||
addf ddt_xv servo-thread
|
||||
addf ddt_y servo-thread
|
||||
addf ddt_yv servo-thread
|
||||
addf ddt_z servo-thread
|
||||
addf ddt_zv servo-thread
|
||||
|
||||
# get position feedback from encoder module
|
||||
# connect position feedback to PID loop and motion module
|
||||
net Xpos-fb encoder_px.position => pid_x.feedback joint.0.motor-pos-fb
|
||||
net Ypos-fb encoder_py.position => pid_y.feedback joint.1.motor-pos-fb
|
||||
net Zpos-fb encoder_pz.position => pid_z.feedback joint.2.motor-pos-fb
|
||||
|
||||
# set position feedback scaling
|
||||
setp encoder_px.position-scale [JOINT_0]INPUT_SCALE
|
||||
setp encoder_py.position-scale [JOINT_1]INPUT_SCALE
|
||||
setp encoder_pz.position-scale [JOINT_2]INPUT_SCALE
|
||||
|
||||
# connect encoder index-enables for homing on index
|
||||
net Xindex-enable encoder_px.index-enable <=> joint.0.index-enable pid_x.index-enable
|
||||
net Yindex-enable encoder_py.index-enable <=> joint.1.index-enable
|
||||
net Zindex-enable encoder_pz.index-enable <=> joint.2.index-enable
|
||||
|
||||
# connect position commands from motion controller to PID input
|
||||
net Xpos-cmd <= joint.0.motor-pos-cmd => pid_x.command
|
||||
net Ypos-cmd <= joint.1.motor-pos-cmd => pid_y.command
|
||||
net Zpos-cmd <= joint.2.motor-pos-cmd => pid_z.command
|
||||
|
||||
# connect motion controller enables to PID blocks
|
||||
net Xenable joint.0.amp-enable-out => pid_x.enable
|
||||
net Yenable joint.1.amp-enable-out => pid_y.enable
|
||||
net Zenable joint.2.amp-enable-out => pid_z.enable
|
||||
|
||||
# connect PID loops to scale blocks that translate to motor revs per sec
|
||||
net Xoutput pid_x.output => scale_x.in
|
||||
net Youtput pid_y.output => scale_y.in
|
||||
net Zoutput pid_z.output => scale_z.in
|
||||
|
||||
# set scaling, number of motor revs needed to
|
||||
# travel one inch
|
||||
setp scale_x.gain [JOINT_0]DRIVE_RATIO
|
||||
setp scale_y.gain [JOINT_1]DRIVE_RATIO
|
||||
setp scale_z.gain [JOINT_2]DRIVE_RATIO
|
||||
|
||||
# motor speed command sigs come from scale blocks
|
||||
# motor speed commands go thru lowpass filters
|
||||
# to simulate motor inertia
|
||||
net Xmtr-cmd scale_x.out => lowpass_x.in
|
||||
net Ymtr-cmd scale_y.out => lowpass_y.in
|
||||
net Zmtr-cmd scale_z.out => lowpass_z.in
|
||||
|
||||
# set "inertia" here, probably by trial and error
|
||||
setp lowpass_x.gain 0.1
|
||||
setp lowpass_y.gain 0.1
|
||||
setp lowpass_z.gain 0.1
|
||||
|
||||
# "actual" motor speed signals
|
||||
# output of lowpass is simulated motor speed
|
||||
# speed goes to simulated encoders
|
||||
net Xmtr-spd lowpass_x.out => sim_encoder_px.speed
|
||||
net Ymtr-spd lowpass_y.out => sim_encoder_py.speed
|
||||
net Zmtr-spd lowpass_z.out => sim_encoder_pz.speed
|
||||
|
||||
# set simulated encoder scaling
|
||||
setp sim_encoder_px.ppr [JOINT_0]MOTOR_PPR
|
||||
setp sim_encoder_py.ppr [JOINT_1]MOTOR_PPR
|
||||
setp sim_encoder_pz.ppr [JOINT_2]MOTOR_PPR
|
||||
|
||||
# simulated encoder output signals
|
||||
# connect them up
|
||||
net XphA sim_encoder_px.phase-A => encoder_px.phase-A
|
||||
net XphB sim_encoder_px.phase-B => encoder_px.phase-B
|
||||
net XphZ sim_encoder_px.phase-Z => encoder_px.phase-Z
|
||||
net YphA sim_encoder_py.phase-A => encoder_py.phase-A
|
||||
net YphB sim_encoder_py.phase-B => encoder_py.phase-B
|
||||
net YphZ sim_encoder_py.phase-Z => encoder_py.phase-Z
|
||||
net ZphA sim_encoder_pz.phase-A => encoder_pz.phase-A
|
||||
net ZphB sim_encoder_pz.phase-B => encoder_pz.phase-B
|
||||
net ZphZ sim_encoder_pz.phase-Z => encoder_pz.phase-Z
|
||||
|
||||
# set PID loop output limits to max velocity
|
||||
setp pid_x.maxoutput [JOINT_0]MAX_VELOCITY
|
||||
setp pid_y.maxoutput [JOINT_1]MAX_VELOCITY
|
||||
setp pid_z.maxoutput [JOINT_2]MAX_VELOCITY
|
||||
|
||||
# set PID loop gains
|
||||
# NOTE: eventually these will be non-zero values as
|
||||
# needed to tune the performance of each axis. The
|
||||
# initial values shown here are extremely conservative
|
||||
# to prevent unexpected behavior. After this file
|
||||
# has been "executed" by halcmd, the gains can be
|
||||
# interactively adjusted using commands like
|
||||
# "halcmd setp pid.<channel>.Pgain <value>"
|
||||
# Once the axis has been tuned to your satisfaction,
|
||||
# do "halcmd show param | grep pid" to get a listing
|
||||
# of the tuning parameters, and enter those values here.
|
||||
|
||||
# the values below come from the INI
|
||||
setp pid_x.Pgain [JOINT_0]PGAIN
|
||||
setp pid_x.Igain [JOINT_0]IGAIN
|
||||
setp pid_x.Dgain [JOINT_0]DGAIN
|
||||
setp pid_x.bias [JOINT_0]BIAS
|
||||
setp pid_x.FF0 [JOINT_0]FF0
|
||||
setp pid_x.FF1 [JOINT_0]FF1
|
||||
setp pid_x.FF2 [JOINT_0]FF2
|
||||
# deadband should be just over 1 count
|
||||
setp pid_x.deadband [JOINT_0]DEADBAND
|
||||
|
||||
setp pid_y.Pgain [JOINT_1]PGAIN
|
||||
setp pid_y.Igain [JOINT_1]IGAIN
|
||||
setp pid_y.Dgain [JOINT_1]DGAIN
|
||||
setp pid_y.bias [JOINT_1]BIAS
|
||||
setp pid_y.FF0 [JOINT_1]FF0
|
||||
setp pid_y.FF1 [JOINT_1]FF1
|
||||
setp pid_y.FF2 [JOINT_1]FF2
|
||||
# deadband should be just over 1 count
|
||||
setp pid_y.deadband [JOINT_1]DEADBAND
|
||||
|
||||
setp pid_z.Pgain [JOINT_2]PGAIN
|
||||
setp pid_z.Igain [JOINT_2]IGAIN
|
||||
setp pid_z.Dgain [JOINT_2]DGAIN
|
||||
setp pid_z.bias [JOINT_2]BIAS
|
||||
setp pid_z.FF0 [JOINT_2]FF0
|
||||
setp pid_z.FF1 [JOINT_2]FF1
|
||||
setp pid_z.FF2 [JOINT_2]FF2
|
||||
# deadband should be just over 1 count
|
||||
setp pid_z.deadband [JOINT_2]DEADBAND
|
||||
|
||||
# send the position commands thru differentiators to
|
||||
# generate velocity and accel signals
|
||||
net Xvel ddt_x.out => ddt_xv.in
|
||||
net Xacc <= ddt_xv.out
|
||||
net Yvel ddt_y.out => ddt_yv.in
|
||||
net Yacc <= ddt_yv.out
|
||||
net Zvel ddt_z.out => ddt_zv.in
|
||||
net Zacc <= ddt_zv.out
|
||||
|
||||
# estop loopback
|
||||
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
|
||||
|
||||
# create signals for tool loading loopback
|
||||
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
|
||||
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
|
||||
|
||||
net xflt => joint.0.amp-fault-in
|
||||
net yflt => joint.1.amp-fault-in
|
||||
net zflt => joint.2.amp-fault-in
|
||||
|
||||
# a second set of encoder counters keeps track of position
|
||||
net XphA => encoder_x.phase-A
|
||||
net XphB => encoder_x.phase-B
|
||||
net YphA => encoder_y.phase-A
|
||||
net YphB => encoder_y.phase-B
|
||||
net ZphA => encoder_z.phase-A
|
||||
net ZphB => encoder_z.phase-B
|
||||
|
||||
setp encoder_x.position-scale [JOINT_0]INPUT_SCALE
|
||||
setp encoder_y.position-scale [JOINT_1]INPUT_SCALE
|
||||
setp encoder_z.position-scale [JOINT_2]INPUT_SCALE
|
||||
|
||||
# connect "actual" position from encoders
|
||||
# to window comparators
|
||||
net Xaxis-pos encoder_x.position => wcomp_xneg.in wcomp_xpos.in wcomp_xhome.in
|
||||
net Yaxis-pos encoder_y.position => wcomp_yneg.in wcomp_ypos.in wcomp_yhome.in
|
||||
net Zaxis-pos encoder_z.position => wcomp_zneg.in wcomp_zpos.in wcomp_zhome.in
|
||||
|
||||
# connect simulated switch outputs to motion controller
|
||||
net Xminlim wcomp_xneg.out => joint.0.neg-lim-sw-in
|
||||
net Xmaxlim wcomp_xpos.out => joint.0.pos-lim-sw-in
|
||||
net Xhome wcomp_xhome.out => joint.0.home-sw-in
|
||||
|
||||
net Yminlim wcomp_yneg.out => joint.1.neg-lim-sw-in
|
||||
net Ymaxlim wcomp_ypos.out => joint.1.pos-lim-sw-in
|
||||
net Yhome wcomp_yhome.out => joint.1.home-sw-in
|
||||
|
||||
net Zminlim wcomp_zneg.out => joint.2.neg-lim-sw-in
|
||||
net Zmaxlim wcomp_zpos.out => joint.2.pos-lim-sw-in
|
||||
net Zhome wcomp_zhome.out => joint.2.home-sw-in
|
||||
|
||||
# configure the points at which the simulated switches trip
|
||||
# X axis first
|
||||
# min limit switch
|
||||
setp wcomp_xneg.max [JOINT_0]MIN_HARD_LIMIT
|
||||
setp wcomp_xneg.min [JOINT_0]MIN_HARD_LIMIT_RELEASE
|
||||
# max limit switch
|
||||
setp wcomp_xpos.min [JOINT_0]MAX_HARD_LIMIT
|
||||
setp wcomp_xpos.max [JOINT_0]MAX_HARD_LIMIT_RELEASE
|
||||
# home switch
|
||||
setp wcomp_xhome.min [JOINT_0]HOME_SW_MIN
|
||||
setp wcomp_xhome.max [JOINT_0]HOME_SW_MAX
|
||||
|
||||
# Y axis
|
||||
# min limit switch
|
||||
setp wcomp_yneg.max [JOINT_1]MIN_HARD_LIMIT
|
||||
setp wcomp_yneg.min [JOINT_1]MIN_HARD_LIMIT_RELEASE
|
||||
# max limit switch
|
||||
setp wcomp_ypos.min [JOINT_1]MAX_HARD_LIMIT
|
||||
setp wcomp_ypos.max [JOINT_1]MAX_HARD_LIMIT_RELEASE
|
||||
# home switch
|
||||
setp wcomp_yhome.min [JOINT_1]HOME_SW_MIN
|
||||
setp wcomp_yhome.max [JOINT_1]HOME_SW_MAX
|
||||
|
||||
# Z axis
|
||||
# min limit switch
|
||||
setp wcomp_zneg.max [JOINT_2]MIN_HARD_LIMIT
|
||||
setp wcomp_zneg.min [JOINT_2]MIN_HARD_LIMIT_RELEASE
|
||||
# max limit switch
|
||||
setp wcomp_zpos.min [JOINT_2]MAX_HARD_LIMIT
|
||||
setp wcomp_zpos.max [JOINT_2]MAX_HARD_LIMIT_RELEASE
|
||||
# home switch
|
||||
setp wcomp_zhome.min [JOINT_2]HOME_SW_MIN
|
||||
setp wcomp_zhome.max [JOINT_2]HOME_SW_MAX
|
||||
|
||||
# Configure fake probing
|
||||
loadrt sphereprobe names=sphereprobe
|
||||
|
||||
addf sphereprobe base-thread 2
|
||||
|
||||
setp sphereprobe.cx -2811 # this is where it ends up after homing
|
||||
setp sphereprobe.cz -6000 # this is where it ends up after homing
|
||||
setp sphereprobe.r 5000 # 5/6 inch
|
||||
net px encoder_px.rawcounts => sphereprobe.px
|
||||
net py encoder_py.rawcounts => sphereprobe.py
|
||||
net pz encoder_pz.rawcounts => sphereprobe.pz
|
||||
net probe-out sphereprobe.probe-out => motion.probe-input
|
||||
net probe-out => encoder_px.latch-input encoder_py.latch-input
|
||||
net probe-out => encoder_pz.latch-input
|
||||
|
||||
setp encoder_px.latch-rising 1
|
||||
setp encoder_px.latch-falling 1
|
||||
setp encoder_py.latch-rising 1
|
||||
setp encoder_py.latch-falling 1
|
||||
setp encoder_pz.latch-rising 1
|
||||
setp encoder_pz.latch-falling 1
|
||||
Reference in New Issue
Block a user