# this file contains the HAL configuration for Roland's Mazak
#
# kinematics
loadrt [KINS]KINEMATICS
# first load the 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

# next load I/O drivers.  This retrofit uses three different I/O devices
#
# 1) MOTENC-Lite card, for analog outs to drives, encoder feedback, and
#     some digital I/O.
#
loadrt hal_motenc

# 2) AXIOM AX5214H card, for 48 digital I/O
#     we are using 32 inputs and 16 outputs, with the outputs on port
#     C, which can be converted to inputs 4 bits at a time.
#
loadrt hal_ax5214h cfg="0x220_iiooiioo"

# 3) Parallel Port, driving PMDX-122 card.  This provides a charge pump
#     type watchdog, and also provides a small number of inputs that
#     can be sampled at a higher rate.  The jogwheel comes in thru this
#     card and is counted in software.
#
loadrt hal_parport cfg="0x0378_in"

# 4) Weighted summer, driven by toolchanger slot position inputs
#     This adds together the 5 bits (with bit 4 being equal to 13)
#     and outputs the sum as an S32
#
loadrt weighted_sum wsum_sizes=5

# I/O Mapping - Physical I/O points to driver pins
# --------------------------------------------------------
# OPTO-22 board IO-1 input  module  0 is ax5214h.0.in-24
# OPTO-22 board IO-1 input  module  1 is ax5214h.0.in-25
#    "                  "                      "
# OPTO-22 board IO-1 input  module 14 is ax5214h.0.in-38
# OPTO-22 board IO-1 input  module 15 is ax5214h.0.in-39
# OPTO-22 board IO-1 output module 16 is ax5214h.0.out-40
# OPTO-22 board IO-1 output module 17 is ax5214h.0.out-41
#    "                  "                      "
# OPTO-22 board IO-1 output module 22 is ax5214h.0.out-46
# OPTO-22 board IO-1 output module 23 is ax5214h.0.out-47
# --------------------------------------------------------
# OPTO-22 board IO-2 input  module  0 is ax5214h.0.in-00
# OPTO-22 board IO-2 input  module  1 is ax5214h.0.in-01
#    "                  "                      "
# OPTO-22 board IO-2 input  module 14 is ax5214h.0.in-14
# OPTO-22 board IO-2 input  module 15 is ax5214h.0.in-15
# OPTO-22 board IO-2 output module 16 is ax5214h.0.out-16
# OPTO-22 board IO-2 output module 17 is ax5214h.0.out-17
#    "                  "                      "
# OPTO-22 board IO-2 output module 22 is ax5214h.0.out-22
# OPTO-22 board IO-2 output module 23 is ax5214h.0.out-23
# --------------------------------------------------------
# OPTO-22 board IO-3 output module  0 is motenc.3.out-15
# OPTO-22 board IO-3 output module  1 is motenc.3.out-14
#    "                  "                      "
# OPTO-22 board IO-3 output module  6 is motenc.3.out-01
# OPTO-22 board IO-3 output module  7 is motenc.3.out-00
# OPTO-22 board IO-3 input  module  8 is motenc.3.in-16
# OPTO-22 board IO-3 input  module  9 is motenc.3.in-17
#    "                  "                      "
# OPTO-22 board IO-3 input  module 22 is motenc.3.in-30
# OPTO-22 board IO-3 input  module 23 is motenc.3.in-31
# --------------------------------------------------------
# Breakout board IO-4 output chan  0 is motenc.3.out-00
# Breakout board IO-4 output chan  1 is motenc.3.out-01
#    "                  "                      "
# Breakout board IO-4 output chan  6 is motenc.3.out-06
# Breakout board IO-4 output chan  7 is motenc.3.out-07
# Breakout board IO-4 input  chan  0 is motenc.3.in-00
# Breakout board IO-4 input  chan  1 is motenc.3.in-01
#    "                  "                      "
# Breakout board IO-4 input  chan 14 is motenc.3.in-14
# Breakout board IO-4 input  chan 15 is motenc.3.in-15
# --------------------------------------------------------

# Now we load some more HAL components:
#
# the rest of these components implement spindle speed
# scaling (for high/low shifting) and spindle orient

loadrt mux2 count=2
loadrt mux4 count=1
loadrt wcomp count=2
loadrt scale count=3
loadrt modmath mod_dir=2
loadrt charge_pump
loadrt tristate_bit
loadrt tristate_float count=3
loadrt conv_s32_float

# Software encoder counter, for the jogwheel, and possibly a future
# small manual encoder for a feedrate override knob.
#
loadrt encoder num_chan=1

# PID loops: 3 for axis control and one for spindle orient
#
loadrt pid num_chan=4

# classicladder for machine logic
# (load the realtime portion)
loadrt classicladder_rt numRungs=50 numBits=50 numWords=8 numTimers=20 numMonostables=10 numPhysInputs=50 numPhysOutputs=40 numArithmExpr=4 numSections=4

# invoke the user part of CL to silently load the program
loadusr -w classicladder --nogui demo_mazak.clp

# load debounce to handle the pushbuttons on the operator panels
loadrt debounce cfg="8"
setp debounce.0.delay 3

# -----------------------------------------------

# connect I/O driver functions to thread(s)
#
addf motenc.3.encoder-read          servo-thread
addf motenc.3.digital-in-read       servo-thread
addf ax5214h.0.read                 servo-thread
addf encoder.capture-position       servo-thread
addf motenc.3.adc-read              servo-thread

# convert IO to internally useful representations
addf scale.2                        servo-thread
# converter for tool magazine position
addf process_wsums                  servo-thread
addf mod-dir.0			    servo-thread
addf mod-dir.1			    servo-thread

# ladder logic is executed once all the inputs are read
#addf classicladder.0.refresh        servo-thread

# adaptive feedrate mux
addf mux2.1			    servo-thread
# now we run the motion controller
addf motion-command-handler         servo-thread
addf motion-controller              servo-thread

# ladder logic is executed once all the inputs are read
addf classicladder.0.refresh        servo-thread
# charge pump can run just about any time
addf charge-pump                    servo-thread

# pid calculations are done after the motion module
# has determined new position commands.
addf pid.0.do-pid-calcs             servo-thread
addf pid.1.do-pid-calcs             servo-thread
addf pid.2.do-pid-calcs             servo-thread

# spindle signal handling is done next
addf pid.3.do-pid-calcs             servo-thread
addf mux2.0                         servo-thread
addf scale.0                        servo-thread
addf scale.1                        servo-thread
addf mux4.0                         servo-thread
addf wcomp.0                        servo-thread
addf wcomp.1                        servo-thread
addf tristate-bit.0                 servo-thread

# misc stuff (jogwheel scale, tool number display)
addf tristate-float.0               servo-thread
addf tristate-float.1               servo-thread
addf tristate-float.2               servo-thread
addf conv-s32-float.0               servo-thread

# output drivers are loaded last
addf motenc.3.dac-write             servo-thread
addf motenc.3.digital-out-write     servo-thread
addf ax5214h.0.write                servo-thread
addf parport.0.write                servo-thread

# the base thread (fast thread) isn't needed for step pulse
# generation since this is a servo machine.  However we use
# it to sample the jogwheel signals and count them in software

addf parport.0.read                 base-thread
addf encoder.update-counters        base-thread

# -------------------------------------------------

# Next, create signals with meaningful names, and attach them to the
#  physical pins.  There are a lot of these, so they are broken up

# ---------------------------------------------------
# ESTOP and related signals
net external-estop-ok ax5214h.0.in-24
net gui-estop-ok iocontrol.0.user-enable-out
net main-estop-ok parport.0.pin-01-out
net main-estop-ok iocontrol.0.emc-enable-in
net main-estop-ok charge-pump.enable
net estop-reset iocontrol.0.user-request-enable
net charge-pump parport.0.pin-17-out
net charge-pump charge-pump.out


# servo power supply control
net AP1 motenc.3.out-00
net AP2 motenc.3.out-01

# motion enable - this signal prevents the motion controller
# from starting unless everything is OK (comes from ladder)
net motion-enable motion.enable

# servo amp enable (only one, driven by axis 0)
net servo-enable motenc.3.out-02
net servo-enable joint.0.amp-enable-out

# servo amp fault signals
# the signals from the amps are actually "not running"
# they are asserted when the amp is faulted, OR just
# disabled.  So we use the inverse and call them "running"
net X-amp-running motenc.3.in-12-not
net Y-amp-running motenc.3.in-13-not
net Z-amp-running motenc.3.in-14-not
# need to release the Z-axis brake when running
net Z-amp-running motenc.3.out-15

# these are the real fault signals, and go to the motion
# controller, they are derived from the ones above by
# ladder logic
net X-amp-fault joint.0.amp-fault-in
net Y-amp-fault joint.1.amp-fault-in
net Z-amp-fault joint.2.amp-fault-in

# Limit switches
# (the switches are NC, and open when hit, so
# we invert the signals by using the -not input
# pin - the result is limit signals that are
# TRUE when the machine is on the limit.)
net X-lim-plus  motenc.3.in-00-not => joint.0.pos-lim-sw-in
net X-lim-minus motenc.3.in-01-not => joint.0.neg-lim-sw-in
net Y-lim-plus  motenc.3.in-02-not => joint.1.pos-lim-sw-in
net Y-lim-minus motenc.3.in-03-not => joint.1.neg-lim-sw-in
net Z-lim-plus  motenc.3.in-04-not => joint.2.pos-lim-sw-in
net Z-lim-minus motenc.3.in-05-not => joint.2.neg-lim-sw-in

# Home switches
# (the switches are NC, see note above)
net X-home motenc.3.in-08-not => joint.0.home-sw-in
net Y-home motenc.3.in-09-not => joint.1.home-sw-in
net Z-home motenc.3.in-10-not => joint.2.home-sw-in

# spindle related signals: "high level" signals
#   ready (from drive to PC)
#   run (from motion)
#   run (to drive)
#   at speed (from drive to PC)
#   orient command (to spindle control)
#   oriented status (from spindle control)
#   commanded speed (from EMC to control)
#   spindle current feedback (from drive to PC)

# spindle related signals: "internal" signal

# get spindle position from encoder
net sp-enc-pos motenc.3.enc-03-position
# scale spindle position to degrees
# 1440 cnts/rev = 4 cnts/degree
setp motenc.3.enc-03-scale -4.0

# get desired orient position from ini file
sets sp-orient-pos-cmd [SPINDLE]ORIENT_POSITION

# connect commanded and feedback positions to PID loop
net sp-orient-pos-cmd pid.3.command
net sp-enc-pos pid.3.feedback

# enable PID loop when in orient mode
net spindle-do-orient pid.3.enable

# tuning params for PID loop
setp pid.3.Pgain 1.0
setp pid.3.Igain 0.6
setp pid.3.Dgain 0.2
setp pid.3.deadband 0.4

# prevent integrator windup at the beginning of an orient
# after running the spindle for a long time.  The error is
# very high before the first index happens.
setp pid.3.maxerrorI 200.0

# limit outputs, we don't want to go fast during orient
setp pid.3.maxoutput 100

# check position error with window comparator
net sp-orient-pos-err pid.3.error
net sp-orient-pos-err wcomp.0.in
net sp-orient-pos-ok wcomp.0.out
# set a +/- 1 degree window
setp wcomp.0.min -1.0
setp wcomp.0.max  1.0

# output of loop is velocity for orientation
net sp-orient-rpm-cmd pid.3.output

# select between normal speed and orient speed
# based on do-orient command
net sp-orient-rpm-cmd mux2.0.in1
net spindle-rpm-cmd mux2.0.in0
net spindle-do-orient mux2.0.sel
# output of mux is desired spindle RPM
net sp-rpm-cmd mux2.0.out

# use scale blocks to calculate required motor RPM for
# both gears based on desired spindle RPM
net sp-rpm-cmd scale.0.in
net sp-mtr-high-rpm-cmd scale.0.out
setp scale.0.gain [SPINDLE]HIGH_GEAR_RATIO
net sp-rpm-cmd scale.1.in
net sp-mtr-low-rpm-cmd scale.1.out
setp scale.1.gain [SPINDLE]LOW_GEAR_RATIO

# select either high or low speed based on current gear ratio,
# unless shifting, then select a low speed for meshing the gears
net sp-mtr-high-rpm-cmd mux4.0.in0
net sp-mtr-low-rpm-cmd mux4.0.in1
net sp-mtr-mesh-rpm-cmd mux4.0.in2
net sp-mtr-mesh-rpm-cmd mux4.0.in3
net sp-in-low-gear mux4.0.sel0
net sp-shifting mux4.0.sel1
# output of mux is desired motor RPM
net sp-mtr-rpm-cmd mux4.0.out
# set meshing speed
sets sp-mtr-mesh-rpm-cmd 15

# link the final motor command to the DAC
net sp-mtr-rpm-cmd motenc.3.dac-03-value
# set scaling - 10V = 4500RPM at the motor
setp motenc.3.dac-03-gain -0.002222
# correct for offset, it causes drift and hunting
setp motenc.3.dac-03-offset -8

# connect other signals to drive
net spindle-ready motenc.3.in-15
net spindle-drive-run motenc.3.out-03
net sp-at-speed motenc.3.in-11
net spindle-amps motenc.3.adc-03-value

# connect signals to gearbox controls
# need to add a spindle run command
# and a rotating speed I think
net sp-engage-high-gear ax5214h.0.out-21
net sp-engage-low-gear ax5214h.0.out-20
net sp-in-high-gear ax5214h.0.in-34
net sp-in-neutral ax5214h.0.in-35
net sp-in-low-gear ax5214h.0.in-36

# use scale block to convert spindle position in degrees
# to revolutions for rigid tapping
net sp-enc-pos scale.2.in
net sp-pos-revs scale.2.out
setp scale.2.gain 0.002777777777777
net sp-pos-revs spindle.0.revs

# rayh begins to screw it up with help from his friends
# connect iocontrol signals for spindle run and speed
# these work as long as a spindle command is being output.
net spindle-run-request spindle.0.on
net spindle-rpm-cmd spindle.0.speed-out

# end of spindle control

# ioControl exports some tool pins
# iocontrol.0.tool-prepare
# iocontrol.0.tool-prep-number
# and expects iocontrol.0.tool-prepared (when the tool prep. is done)
# iocontrol.0.tool-change (output)
# and expects iocontrol.0.tool-changed (when tool changed)
# it also exports iocontrol.0.tool-number, which is the current tool
# in the spindle and is used when its time to return that tool to
# the tool magazine
net tool-prepare iocontrol.0.tool-prepare
net tool-prepared iocontrol.0.tool-prepared
net tool-change iocontrol.0.tool-change
net tool-changed iocontrol.0.tool-changed
net tool-requested-number iocontrol.0.tool-prep-number
net tool-current-number iocontrol.0.tool-number

# misc control
# tool change location push button operators
net magazine-index-pbs ax5214h.0.in-07
net worklight-pbs ax5214h.0.in-08
net tool-load-pbs ax5214h.0.in-09
net tool-unload-pbs ax5214h.0.in-10

# add front panel buttons here
net tool-unclamp-pbs ax5214h.0.in-39
net feed-hold-pbs ax5214h.0.in-14
net cycle-start-pbs ax5214h.0.in-25

# hydraulic pump
net hydraulic-pump-run motenc.3.out-14
net hydraulic-pump-running ax5214h.0.in-05

# hydraulic actuator prox switches
# tool load-unload arm
net tool-unloaded ax5214h.0.in-38
net tool-loaded ax5214h.0.in-15

# double or intermediate arm
net arm-retracted ax5214h.0.in-29
net arm-extended ax5214h.0.in-30
# FIXME!!  This sensor is broken, so we are using a delay
# instead - 2 seconds after the cylinder is actuated, we
# assume that we have reached the desired position and 
# set the signal true.  The commented out line below is
# the proper source for the signal.  But for now, it is
# driven by classicladder output number 27 (see ladder
# section near end of file).
#linksp arm-at-0/180 ax5214h.0.in-26
net arm-at-0/60 ax5214h.0.in-27
net arm-at-60 ax5214h.0.in-28
net arm-at-180 ax5214h.0.in-31

# tool drawbar
net tool-clamped ax5214h.0.in-33
net tool-unclamped ax5214h.0.in-32

# tool magazine or carousel with 0-24 positions
net magazine-in-position ax5214h.0.in-37
net magazine-not-in-position ax5214h.0.in-37-not
net magazine-not-in-position wsum.0.hold
net magazine-position-0 ax5214h.0.in-00
net magazine-position-0 wsum.0.bit.0.in
net magazine-position-1 ax5214h.0.in-01
net magazine-position-1 wsum.0.bit.1.in
net magazine-position-2 ax5214h.0.in-02
net magazine-position-2 wsum.0.bit.2.in
net magazine-position-3 ax5214h.0.in-03
net magazine-position-3 wsum.0.bit.3.in
net magazine-position-4 ax5214h.0.in-04
net magazine-position-4 wsum.0.bit.4.in
# set the bit weight for bit 4 to 13 instead of the default 16
setp wsum.0.bit.4.weight 13
# add a signal for magazine position
net magazine-position wsum.0.sum
# add a temporary signal to rotate by one.

# hydraulic valves
net arm-extend ax5214h.0.out-47
net arm-retract ax5214h.0.out-46
net arm-60cw ax5214h.0.out-45
net arm-180ccw ax5214h.0.out-44
net tool-load ax5214h.0.out-43
net tool-unload ax5214h.0.out-42
net magazine-forward ax5214h.0.out-41
net magazine-reverse ax5214h.0.out-40
net tool-unclamp ax5214h.0.out-23
net head-unclamp ax5214h.0.out-22

# other solenoids and such
net spindle-air-blast ax5214h.0.out-19
net work-air-blast ax5214h.0.out-18
net mist-coolant ax5214h.0.out-17

net coolant-flood iocontrol.0.coolant-flood ax5214h.0.out-16

# magazine indexing
net magazine-position mod-dir.0.actual
net tool-requested-number mod-dir.0.desired
net tool-requested-match mod-dir.0.on-target

net magazine-position mod-dir.1.actual
net tool-current-number mod-dir.1.desired
net tool-current-match mod-dir.1.on-target

setp mod-dir.0.max-num [EMCIO]TOOL_TURRET_MAX
setp mod-dir.0.wrap [EMCIO]TOOL_TURRET_WRAP
setp mod-dir.1.max-num [EMCIO]TOOL_TURRET_MAX
setp mod-dir.1.wrap [EMCIO]TOOL_TURRET_WRAP
# direction to move to fetch new tool
net magazine-fwd-req-fetch mod-dir.0.up
net magazine-rev-req-fetch mod-dir.0.down
# direction to move to store previous tool
net magazine-fwd-req-store mod-dir.1.up
net magazine-rev-req-store mod-dir.1.down

# set encoder latch enable TRUE so encoder count "wraps" during toolchanges
# and resets under motion controller command for rigid tapping
net sp-index-enable motenc.3.enc-03-index-enable
net sp-index-enable tristate-bit.0.out
setp tristate-bit.0.in 1
net tool-change tristate-bit.0.enable
net sp-index-enable spindle.0.index-enable

# jogwheel signals
setp encoder.0.x4-mode 0
net jogwheel-phA parport.0.pin-12-in
net jogwheel-phB parport.0.pin-11-in
# route signals to software encoder counter
net jogwheel-phA encoder.0.phase-A

net jogwheel-phB encoder.0.phase-B
# jogwheel output
net jogwheel-counts encoder.0.counts
# need to release the Z-axis brake when running
net Z-amp-running motenc.3.out-15

# feedhold, uses G50 adaptive feed input
sets zero 0.0
sets one 1.0
net adaptive-feed mux2.1.out
net adaptive-feed motion.adaptive-feed
net zero mux2.1.in1
net one mux2.1.in0
net feed-hold mux2.1.sel


# -----------------------------------------------------
# encoders - signals and scaling
#
# position in counts
net X-enc-counts motenc.3.enc-00-count
net Y-enc-counts motenc.3.enc-01-count
net Z-enc-counts motenc.3.enc-02-count

# scaling to get inches (scale comes from ini file)
setp motenc.3.enc-00-scale [JOINT_0]INPUT_SCALE
setp motenc.3.enc-01-scale [JOINT_1]INPUT_SCALE
setp motenc.3.enc-02-scale [JOINT_2]INPUT_SCALE

# position in inches
net X-enc-pos motenc.3.enc-00-position
net Y-enc-pos motenc.3.enc-01-position
net Z-enc-pos motenc.3.enc-02-position

# index pulses
net X-index-enable motenc.3.enc-00-index-enable joint.0.index-enable
net Y-index-enable motenc.3.enc-01-index-enable joint.1.index-enable
net Z-index-enable motenc.3.enc-02-index-enable joint.2.index-enable

# -----------------------------------------------------
# DACs - output to servo amps
#
net X-volts motenc.3.dac-00-value
net Y-volts motenc.3.dac-01-value
net Z-volts motenc.3.dac-02-value
# get scale and offset from the ini file
setp motenc.3.dac-00-gain [JOINT_0]OUTPUT_SCALE
setp motenc.3.dac-01-gain [JOINT_1]OUTPUT_SCALE
setp motenc.3.dac-02-gain [JOINT_2]OUTPUT_SCALE
setp motenc.3.dac-00-offset [JOINT_0]OUTPUT_OFFSET
setp motenc.3.dac-01-offset [JOINT_1]OUTPUT_OFFSET
setp motenc.3.dac-02-offset [JOINT_2]OUTPUT_OFFSET

# -----------------------------------------------------
# ADCs - servo amp current feedback
#
net X-amps motenc.3.adc-00-value
net Y-amps motenc.3.adc-01-value
net Z-amps motenc.3.adc-02-value
# set scale and offset (need to calibrate this)
setp motenc.3.adc-00-gain 1.0
setp motenc.3.adc-01-gain 1.0
setp motenc.3.adc-02-gain 1.0
setp motenc.3.adc-00-offset 0.0
setp motenc.3.adc-01-offset 0.0
setp motenc.3.adc-02-offset 0.0

# -----------------------------------------------------
# PIDs - position control
#

# signals for position command
# hook the motion controller outputs to the position command
net X-pos-cmd joint.0.motor-pos-cmd
net Y-pos-cmd joint.1.motor-pos-cmd
net Z-pos-cmd joint.2.motor-pos-cmd
# and to the PID inputs
net X-pos-cmd pid.0.command
net Y-pos-cmd pid.1.command
net Z-pos-cmd pid.2.command

# hook encoders to PID feedback
net X-enc-pos pid.0.feedback
net Y-enc-pos pid.1.feedback
net Z-enc-pos pid.2.feedback
# and to motion controller
net X-enc-pos joint.0.motor-pos-fb
net Y-enc-pos joint.1.motor-pos-fb
net Z-enc-pos joint.2.motor-pos-fb

# hook PID outputs to DACs
net X-volts pid.0.output
net Y-volts pid.1.output
net Z-volts pid.2.output

# use 'servo-enable' to enable PID blocks
# need to release the Z-axis brake when running
net Z-amp-running motenc.3.out-15

net servo-enable pid.0.enable
net servo-enable pid.1.enable
net servo-enable pid.2.enable

# get tuning params from ini file
setp pid.0.deadband [JOINT_0]DEADBAND
setp pid.0.Pgain [JOINT_0]PGAIN
setp pid.0.Igain [JOINT_0]IGAIN
setp pid.0.Dgain [JOINT_0]DGAIN
setp pid.0.FF0 [JOINT_0]FF0
setp pid.0.FF1 [JOINT_0]FF1
setp pid.0.FF2 [JOINT_0]FF2
setp pid.0.bias [JOINT_0]BIAS
setp pid.1.deadband [JOINT_1]DEADBAND
setp pid.1.Pgain [JOINT_1]PGAIN
setp pid.1.Igain [JOINT_1]IGAIN
setp pid.1.Dgain [JOINT_1]DGAIN
setp pid.1.FF0 [JOINT_1]FF0
setp pid.1.FF1 [JOINT_1]FF1
setp pid.1.FF2 [JOINT_1]FF2
setp pid.1.bias [JOINT_1]BIAS
setp pid.2.deadband [JOINT_2]DEADBAND
setp pid.2.Pgain [JOINT_2]PGAIN
setp pid.2.Igain [JOINT_2]IGAIN
setp pid.2.Dgain [JOINT_2]DGAIN
setp pid.2.FF0 [JOINT_2]FF0
setp pid.2.FF1 [JOINT_2]FF1
setp pid.2.FF2 [JOINT_2]FF2
setp pid.2.bias [JOINT_2]BIAS
# get maximum (and minimum) output volts from ini file
setp pid.0.maxoutput [JOINT_0]MAX_OUTPUT
setp pid.1.maxoutput [JOINT_1]MAX_OUTPUT
setp pid.2.maxoutput [JOINT_2]MAX_OUTPUT

# LADDER LOGIC!!!
#
# Classic ladder doesn't let you use meaningful names, so this
# will be the magic decoder ring

# INPUTS to CL
# I0 = the GUI estop "button" isn't pressed
net gui-estop-ok classicladder.0.in-00
# I1 = servo-enable, used to mask amp faults when not enabled
net servo-enable classicladder.0.in-01
# I2 thru I4, amp running signal (FALSE when faulted OR disabled)
net X-amp-running classicladder.0.in-02
net Y-amp-running classicladder.0.in-03
net Z-amp-running classicladder.0.in-04
net spindle-use-low-gear classicladder.0.in-05
net sp-in-low-gear classicladder.0.in-06
net sp-in-high-gear classicladder.0.in-07
net sp-in-neutral classicladder.0.in-08
net sp-at-speed classicladder.0.in-09
net hydraulic-pump-running classicladder.0.in-10
net magazine-in-position classicladder.0.in-11
net magazine-index-pbs classicladder.0.in-12
net tool-load-pbs classicladder.0.in-13
net tool-loaded classicladder.0.in-14
net tool-unload-pbs classicladder.0.in-15
net tool-unloaded classicladder.0.in-16
net tool-unclamp-pbs classicladder.0.in-17
# based on difference between current position and
# the slot for the tool currently in the spindle
net magazine-fwd-req-store classicladder.0.in-18
net magazine-rev-req-store classicladder.0.in-19
# I20 = means the external estop chain is OK
net external-estop-ok classicladder.0.in-20
# I21 = estop-reset, pulsed to reset the estop relay
net estop-reset classicladder.0.in-21
net feed-hold-pbs classicladder.0.in-22
net spindle-run-request classicladder.0.in-23
net sp-orient-pos-ok classicladder.0.in-24
net tool-requested-match classicladder.0.in-25
net tool-prepare classicladder.0.in-26
# based on difference between current position and EMC
# requested position (from a T block)
net magazine-fwd-req-fetch classicladder.0.in-27
net magazine-rev-req-fetch classicladder.0.in-28
# tool changer proxes
net arm-at-0/180 classicladder.0.in-29
net arm-at-0/60 classicladder.0.in-30
net arm-at-180 classicladder.0.in-31
net arm-at-60 classicladder.0.in-32
net arm-extended classicladder.0.in-33
net arm-retracted classicladder.0.in-34
net tool-change classicladder.0.in-35
net tool-clamped classicladder.0.in-36
net tool-unclamped classicladder.0.in-37
net tool-current-match classicladder.0.in-38



# OUTPUTS from CL
# Q0 = AP1, first stage power up (applies power thru resistors)
net AP1 classicladder.0.out-00
# Q1 = AP2, second stage (bypasses resistors)
net AP2 classicladder.0.out-01
# Q2 thru Q4, amp faulted signal (ENABLED and NOT RUNNING)
net X-amp-fault classicladder.0.out-02
net Y-amp-fault classicladder.0.out-03
net Z-amp-fault classicladder.0.out-04
# Q5 is motion enable (after chain)
net motion-enable classicladder.0.out-05
# Q6 and 7 are the gear shift outputs
net sp-engage-low-gear classicladder.0.out-06
net sp-engage-high-gear classicladder.0.out-07
# Q8 indicates that a shift is in progress
net sp-shifting classicladder.0.out-08
net hydraulic-pump-run classicladder.0.out-09
net magazine-forward classicladder.0.out-10
net magazine-reverse classicladder.0.out-11
net tool-load classicladder.0.out-12
net tool-unload classicladder.0.out-13
net arm-extend classicladder.0.out-14
net arm-retract classicladder.0.out-15
net arm-60cw classicladder.0.out-16
net arm-180ccw classicladder.0.out-17
net tool-unclamp classicladder.0.out-18
net head-unclamp classicladder.0.out-19
net tool-prepared classicladder.0.out-20
net tool-changed classicladder.0.out-21
# Q22 indicates that the estop ladder rung (latch) is ok
net main-estop-ok classicladder.0.out-22
net spindle-drive-run classicladder.0.out-23
net spindle-at-speed classicladder.0.out-24
net spindle-oriented classicladder.0.out-25
net feed-hold classicladder.0.out-26
# FIXME! this is a temporary replacement for a bad sensor
net arm-at-0/180 classicladder.0.out-27
net spindle-do-orient classicladder.0.out-28

# CL internals (not HAL data, just here for documentation
#
# T0 = delay from estop OK to AP1
# T1 = delay from AP1 to AP2
# T2 = delay to allow servo amps to respond to enable
# T3 = delay between AP2 close and motion enable
# T4 = delay before shifting out of gear
# T5 = delay before shifting into gear
# T6 = oneshot for tool magazine advance
# T7 = hydraulic pump delay
# T8 = at-speed delay
# T9 = oriented delay
# T10 = bad prox delay


# B0 = delayed servo-enable, for fault masking
# B1 = five second initialization signal
# B2 = gear shifting in progress
# B3 = ok to shift into neutral
# B4 = ok to shift into gear
# B5 = magazine-index delay
# B6 = magazine stop delay
# B7 = tool load virtual
# B8 = tool unload virtual
# B9 = arm extend virtual
# B10 = arm retract virtual
# B11 = arm arm-60cw virtual
# B12 = arm arm 180ccw virtual
# B13 = arm tool unclamp virtual
# B14 = tool change preconditions ready
# B15 = cycle midpoint
# B16 = tool cycle start

