# HAL config file for Pico Systems UPC board using Velocity Estimation
#

# set the PWM frequency to suit Pico's PWM amps (50KHz)
# users with other amps may need to change this
setp ppmc.0.pwm.00-03.freq 50000
# turn on bootstrapping for the MOSFET drivers
# this also depends on the amplifiers
setp ppmc.0.pwm.00.bootstrap TRUE
setp ppmc.0.pwm.01.bootstrap TRUE
setp ppmc.0.pwm.02.bootstrap TRUE
setp ppmc.0.pwm.03.bootstrap TRUE
# set max duty cycle to 95% for the MOSFET drives
# at 50KHz this corresponds to 1 us
setp ppmc.0.pwm.00.max-dc 0.95
setp ppmc.0.pwm.01.max-dc 0.95
setp ppmc.0.pwm.02.max-dc 0.95
setp ppmc.0.pwm.03.max-dc 0.95

setp ppmc.0.encoder.00.min-speed-estimate 0.001
setp ppmc.0.encoder.01.min-speed-estimate 0.001
setp ppmc.0.encoder.02.min-speed-estimate 0.001
setp ppmc.0.encoder.03.min-speed-estimate 0.001

# Here's the part that connects the encoder velocity to the PID
net Xfbderiv ppmc.0.encoder.00.velocity pid.0.feedback-deriv
net Yfbderiv ppmc.0.encoder.01.velocity pid.1.feedback-deriv
net Zfbderiv ppmc.0.encoder.02.velocity pid.2.feedback-deriv

# connect position feedback signals to encoders
linksp Xpos-fb <= ppmc.0.encoder.00.position
linksp Ypos-fb <= ppmc.0.encoder.01.position
linksp Zpos-fb <= ppmc.0.encoder.02.position
#linksp Apos-fb <= ppmc.0.encoder.03.position

# get feedback scaling from ini file
setp ppmc.0.encoder.00.scale [JOINT_0]INPUT_SCALE
setp ppmc.0.encoder.01.scale [JOINT_1]INPUT_SCALE
setp ppmc.0.encoder.02.scale [JOINT_2]INPUT_SCALE
setp ppmc.0.encoder.03.scale 6912

# connect PID output signals to PWM generators
linksp Xoutput => ppmc.0.pwm.00.value
linksp Youtput => ppmc.0.pwm.01.value
linksp Zoutput => ppmc.0.pwm.02.value

# connect axis enables to PWM generators
linksp Xenable => ppmc.0.pwm.00.enable
linksp Yenable => ppmc.0.pwm.01.enable
linksp Zenable => ppmc.0.pwm.02.enable

# set output scaling from ini file
setp ppmc.0.pwm.00.scale [JOINT_0]OUTPUT_SCALE
setp ppmc.0.pwm.01.scale [JOINT_1]OUTPUT_SCALE
setp ppmc.0.pwm.02.scale [JOINT_2]OUTPUT_SCALE

# add a couple of tuning test links
# if these are useful will want to add them to the other axes as well
# or make these setup with the tuning script
linkps ddt.0.in Xoutput
linkps ddt.1.in Xpos-fb

newsig spindle-sync bit
newsig spindle-index-en bit
linksp spindle-index-en => ppmc.0.encoder.03.index-enable

# hook up motion controller's sync output
linkps spindle.0.index-enable => spindle-index-en

# report rev count to motion controller
newsig spindle-pos float
linkps ppmc.0.encoder.03.position => spindle-pos
linksp spindle-pos => spindle.0.revs

# set up 4th PWM generator as a spindle speed control
newsig spindle-speed float
newsig spindle-pwm-cmd float
newsig spindle-pwm-filt float
linkps spindle.0.speed-out => spindle-speed
linksp spindle-speed => mult2.1.in0
setp   mult2.1.in1 0.0009018
linkps mult2.1.out => spindle-pwm-cmd
linksp spindle-pwm-cmd => lowpass.0.in
linkps lowpass.0.out => spindle-pwm-filt
linksp spindle-pwm-filt => ppmc.0.pwm.03.value
setp lowpass.0.gain 0.005

newsig spindle-enable bit
# SpindleFwd and SpindleRev come from univpwm_io.hal
linksp SpindleFwd => or2.0.in0
linksp SpindleRev => or2.0.in1
linkps or2.0.out => spindle-enable
linksp spindle-enable ppmc.0.pwm.03.enable
# spindle speed display
net spinraw    ppmc.0.encoder.03.delta conv-s32-float.0.in
net spinfloat  conv-s32-float.0.out mult2.0.in0
setp   mult2.0.in1 8.6805555
newsig SpindleRPM float
linkps mult2.0.out => SpindleRPM