# SPINDLE Orient

setp hm2_5i25.0.encoder.05.counter-mode 0
setp hm2_5i25.0.encoder.05.filter 1
setp hm2_5i25.0.encoder.05.index-invert 0
setp hm2_5i25.0.encoder.05.index-mask 0
setp hm2_5i25.0.encoder.05.index-mask-invert 0
setp hm2_5i25.0.encoder.05.scale [SPINDLE_0]ENCODER_SCALE
setp hm2_5i25.0.pwmgen.05.output-type 1
setp hm2_5i25.0.pwmgen.05.scale [SPINDLE_0]OUTPUT_SCALE

loadrt limit2 names=spindle-ramp
loadrt near names=spindle-at-speed,spindle-at-pos
loadrt timedelay names=spindle-active-delay
loadrt orient names=spindle-orient
loadrt pid names=spindle-pid
loadrt mux2 names=spindle-pwm-switch
loadrt or2 count=0
loadrt not count=0
loadrt and2 count=0
loadrt offset count=0


addf spindle-ramp servo-thread
addf spindle-at-speed servo-thread
addf spindle-at-pos servo-thread
addf spindle-active-delay servo-thread
addf spindle-orient servo-thread
addf spindle-pid.do-pid-calcs servo-thread
addf spindle-pwm-switch servo-thread
addf or2.0 servo-thread
addf not.0 servo-thread
addf and2.0 servo-thread
addf offset.0.update-output servo-thread


setp spindle-pid.Pgain [SPINDLE_0]P
setp spindle-pid.Igain [SPINDLE_0]I
setp spindle-pid.Dgain [SPINDLE_0]D
setp spindle-pid.bias [SPINDLE_0]BIAS
setp spindle-pid.FF0 [SPINDLE_0]FF0
setp spindle-pid.FF1 [SPINDLE_0]FF1
setp spindle-pid.FF2 [SPINDLE_0]FF2
setp spindle-pid.deadband [SPINDLE_0]DEADBAND
setp spindle-pid.maxoutput [SPINDLE_0]MAX_OUTPUT
setp spindle-pid.error-previous-target true

setp spindle-ramp.maxv [SPINDLE_0]ACCELERATION
setp spindle-at-speed.difference [SPINDLE_0]MAX_ERROR
setp spindle-at-pos.difference 0.01
setp spindle-at-pos.in1 0
setp spindle-active-delay.on-delay 0
setp spindle-active-delay.off-delay [SPINDLE_0]OFF_DELAY

# orient mit motion verknuepfen
net orient-angle motion.spindle-orient-angle => spindle-orient.angle
net orient-mode motion.spindle-orient-mode => spindle-orient.mode
net orient-enable motion.spindle-orient => => and2.0.in1

# Position vom Encoder in den pid / orient und motion schieben
net spindle-pos => spindle-pid.feedback => spindle-orient.position
net spindle-pos <= motion.spindle-revs <= hm2_5i25.0.encoder.05.position

# Encodergeschwindigkeit U/sek in den near und motion schieben
net spindle-fb-rps motion.spindle-speed-in <= hm2_5i25.0.encoder.05.velocity => spindle-at-speed.in2

# Positionsvorgabe vom orient in den pid schieben
setp offset.0.offset 1
net spindle.otient-cmd spindle-orient.command => offset.0.in
net spindle-orient-cmd1 offset.0.out => spindle-pid.command

# Drehzahlvorgabe U/min aus motion in das limit2 schieben
net spindle-speed-rpm motion.spindle-speed-out => spindle-ramp.in

# Drehzahlvorgabe U/sek aus motion in das near schieben
net spindle-speed-rps motion.spindle-speed-out-rps => spindle-at-speed.in1

# Wenn Solldrehzahl gleich Istdrehzahl dann aus near das bit in motion schieben
net spindle-at-speed spindle-at-speed.out => motion.spindle-at-speed

# Wenn Sollpositon gleich Istpositon dann aus near das bit in motion schieben
net spindle-pos-err spindle-at-pos.in2 <= spindle-pid.error
net spindle-at-pos spindle-at-pos.out =>

net spindle-on0 motion.spindle-on => spindle-active-delay.in
net spindel-on1 spindle-active-delay.out => or2.0.in0 => not.0.in
net spindel-on2 and2.0.in0 <= not.0.out

# orient und pid aktivieren
net orient-active and2.0.out => or2.0.in1 => spindle-pwm-switch.sel
net orient-active => spindle-pid.enable => spindle-orient.enable

net spindle-enable or2.0.out => hm2_5i25.0.pwmgen.05.enable

# motion gibt Signal an Encoder das beim naechsten Z Signal auf 0 gestellt werden soll
net spindle-sync motion.spindle-index-enable => hm2_5i25.0.encoder.05.index-enable
sets spindle-sync 1

# pwm Signal entweder vom motion oder orient
net pwm-switch-in0 spindle-pwm-switch.in0 <= spindle-ramp.out
net pwm-switch-in1 spindle-pwm-switch.in1 <= spindle-pid.output
net pwm-switch-out spindle-pwm-switch.out => hm2_5i25.0.pwmgen.05.value


########## Bewegungsueberbrueckung um Schleppfehler zu vermeiden !!! ##########
net xpos-cmd axis.0.motor-pos-cmd => axis.0.motor-pos-fb
net ypos-cmd axis.1.motor-pos-cmd => axis.1.motor-pos-fb
net zpos-cmd axis.2.motor-pos-cmd => axis.2.motor-pos-fb
########## Bewegungsueberbrueckung ##########


net estop-out <= iocontrol.0.user-enable-out
net estop-out => iocontrol.0.emc-enable-in