#*******************
#  SPINDLE S
#*******************

# as I do use a 7i43 we will have to load the stuff for that
# you may need to add this to your needs
loadrt hostmot2
loadrt hm2_7i43 ioaddr=0xcc00 epp_wide=0 config="firmware=hm2/7i43/SVST4_4S.BIT num_encoders=2 num_pwmgens=2 num_stepgens=0" 
setp     hm2_7i43.0.pwmgen.pwm_frequency 100000
setp     hm2_7i43.0.pwmgen.pdm_frequency 100000
setp     hm2_7i43.0.watchdog.timeout_ns 10000000
addf     hm2_7i43.0.read servo-thread
addf     hm2_7i43.0.write         servo-thread
addf     hm2_7i43.0.pet_watchdog  servo-thread

# for closed loop we ned a PID control, so lets load it
loadrt pid num_chan=1
addf pid.0.do-pid-calcs servo-thread
setp   pid.0.Pgain     [SPINDLE_9]P
setp   pid.0.Igain     [SPINDLE_9]I
setp   pid.0.Dgain     [SPINDLE_9]D
setp   pid.0.bias      [SPINDLE_9]BIAS
setp   pid.0.FF0       [SPINDLE_9]FF0
setp   pid.0.FF1       [SPINDLE_9]FF1
setp   pid.0.FF2       [SPINDLE_9]FF2
setp   pid.0.deadband  [SPINDLE_9]DEADBAND
setp   pid.0.maxoutput [SPINDLE_9]MAX_OUTPUT

# ---PWM Generator signals/setup---
# no quadratur counting = 1 else 0
setp   hm2_7i43.0.pwmgen.01.output-type 1
# Set the spindle's top speed in RPM
setp   hm2_7i43.0.pwmgen.01.scale  3500

# ---Encoder feedback signals/setup---
setp    hm2_7i43.0.encoder.01.counter-mode 1
setp    hm2_7i43.0.encoder.01.filter 1
setp    hm2_7i43.0.encoder.01.index-invert 0
setp    hm2_7i43.0.encoder.01.index-mask 0
setp    hm2_7i43.0.encoder.01.index-mask-invert 0
setp    hm2_7i43.0.encoder.01.scale  [SPINDLE_9]INPUT_SCALE

# we need a scale for feedback to the pid
loadrt scale names=scale_rps_to_rpm
addf scale_rps_to_rpm servo-thread
setp scale_rps_to_rpm.gain 60

# ---setup spindle control signals---
net spindle-on           hm2_7i43.0.pwmgen.01.enable  <=  pid.0.enable  <=  motion.spindle-on
net spindle-vel-cmd      pid.0.command  <=  motion.spindle-speed-out
net spindle-output       pid.0.output  =>  hm2_7i43.0.pwmgen.01.value
net spindle-vel-fb-rps   hm2_7i43.0.encoder.01.velocity  =>  scale_rps_to_rpm.in
net spindle-vel-fb-rpm   scale_rps_to_rpm.out  =>  pid.0.feedback  
net spindle-vel-fb-rpm   motion.spindle-speed-in
net spindle-index-enable motion.spindle-index-enable  <=  hm2_7i43.0.encoder.01.index-enable

##############################
# spindle at speed detection #
##############################
loadrt near names=near_spindle_speed
addf near_spindle_speed servo-thread
# set the spindle speed inputs to agree if within 3%
setp near_spindle_speed.scale 1.03

# connect one input to the commanded spindle speed
# we declared above already 
# net spindle-vel-cmd      pid.0.command  <=  motion.spindle-speed-out
net spindle-vel-cmd     near_spindle_speed.in1

# and the other input to the encoder-measured spindle speed
# we declared above already 
# net spindle-vel-fb-rpm   scale_rps_to_rpm.out =>  pid.0.feedback  
net spindle-vel-fb-rpm  near_spindle_speed.in2

# connect the output to the spindle-at-speed input
net spindle-at-speed    motion.spindle-at-speed  <=  near_spindle_speed.out