Spindle Orient and Rigid Tapping without an Encoder (M19, G33.1)

component stepgenencoder "use a stepgen position-fb output as an encoder";
pin in float position-in;
pin io bit index-enable;
pin in bit reset;
pin out float position-out;
variable float offset = 0;
variable float offset_frac = 0;
variable float whole_revs = 0;
function _ fp;
license "GPL";
;;
#include <rtapi_math.h>
FUNCTION(_) {
    float whole_revs_old;

    whole_revs_old = whole_revs;
    whole_revs = floor(position_in - offset);

    if (index_enable && whole_revs != whole_revs_old) {
        offset = floor(position_in - offset_frac) + offset_frac;
        whole_revs = 0;
        index_enable = false;
    }

    if (reset) {
        offset = position_in;
        offset_frac = floor(position_in);
        whole_revs = 0;
    }

    position_out = position_in - offset;
}

# snip
loadrt pid names=pid.s-pos
loadrt orient
loadrt stepgenencoder
loadrt mux2 names=mux2.s
loadrt near names=near.s-vel,near.s-brake
loadrt and2 names=and2.s-brake
loadrt scale names=scale.s-vel

addf hm2_7i76e.0.read          servo-thread
# snip
addf pid.s-pos.do-pid-calcs   servo-thread
addf orient.0                 servo-thread
addf stepgenencoder.0         servo-thread
addf near.s-vel               servo-thread
addf near.s-brake             servo-thread
addf and2.s-brake             servo-thread
addf scale.s-vel              servo-thread
addf mux2.s                   servo-thread
addf hm2_7i76e.0.write         servo-thread

# snip

# --- spindle velocity stuff ---

setp scale.s-vel.gain 60              # rpm = 60 * rps
setp near.s-vel.difference 1          # spindle-at-speed is within 1rps (i.e. within 60rpm)
setp near.s-brake.difference 0.1      # can apply brake when spindle rps < 0.1 (i.e. rpm < 6)
setp near.s-brake.in2 0               # brake should be applied near 0rps / 0rpm

net spindle-vel           hm2_7i76e.0.stepgen.04.velocity-fb  => spindle.0.speed-in near.s-vel.in1 near.s-brake.in1 scale.s-vel.in
net spindle-vel-rpm       scale.s-vel.out
net spindle-vel-stepgen   mux2.s.out                          => hm2_7i76e.0.stepgen.04.velocity-cmd
net spindle-vel-cmd       spindle.0.speed-out-rps             => near.s-vel.in2 mux2.s.in0
net spindle-vel-cmd-rpm   spindle.0.speed-out
net spindle-at-speed      near.s-vel.out                      => spindle.0.at-speed
net spindle-can-brake     near.s-brake.out                    => and2.s-brake.in0
net spindle-want-brake    spindle.0.brake                     => and2.s-brake.in1
net spindle-brake         and2.s-brake.out


# --- spindle position stuff ---

net spindle-index-enable  spindle.0.index-enable             <=> stepgenencoder.0.index-enable
net spindle-pos-raw       hm2_7i76e.0.stepgen.04.position-fb  => stepgenencoder.0.position-in  
net spindle-pos           stepgenencoder.0.position-out       => spindle.0.revs pid.s-pos.feedback orient.0.position
net spindle-pos-cmd       orient.0.command                    => pid.s-pos.command
net spindle-angle         spindle.0.orient-angle              => orient.0.angle
net spindle-pos-pid       pid.s-pos.output                    => mux2.s.in1
net spindle-in-pos        orient.0.is-oriented                => spindle.0.is-oriented


# --- switch between modes ---

net spindle-velocity-mode spindle.0.on
net spindle-orient-mode   spindle.0.orient                    => orient.0.enable pid.s-pos.enable mux2.s.sel
net spindle-orient-dir    spindle.0.orient-mode               => orient.0.mode


# -- stepgen setup ---

setp   hm2_7i76e.0.stepgen.04.dirsetup        [SPINDLE]DIRSETUP
setp   hm2_7i76e.0.stepgen.04.dirhold         [SPINDLE]DIRHOLD
setp   hm2_7i76e.0.stepgen.04.steplen         [SPINDLE]STEPLEN
setp   hm2_7i76e.0.stepgen.04.stepspace       [SPINDLE]STEPSPACE
setp   hm2_7i76e.0.stepgen.04.position-scale  [SPINDLE]STEP_SCALE
setp   hm2_7i76e.0.stepgen.04.step_type       2 # quadrature output
setp   hm2_7i76e.0.stepgen.04.control-type    1 # velocity mode
setp   hm2_7i76e.0.stepgen.04.maxaccel        [SPINDLE]MAX_ACCELERATION
setp   hm2_7i76e.0.stepgen.04.maxvel          [SPINDLE]MAX_VELOCITY

# keep the stepgen always on so that the spindle always respects MAX_ACCELERATION and doesn't try to stop instantly
# (this probably means that the physical drive enable should be attached to the estop signal for safety)
setp   hm2_7i76e.0.stepgen.04.enable           true


# --- setup the position PID ---

setp   pid.s-pos.Pgain     [SPINDLE]P_POS
setp   pid.s-pos.Igain     [SPINDLE]I_POS
setp   pid.s-pos.Dgain     [SPINDLE]D_POS
setp   pid.s-pos.bias      [SPINDLE]BIAS_POS
setp   pid.s-pos.FF0       [SPINDLE]FF0_POS
setp   pid.s-pos.FF1       [SPINDLE]FF1_POS
setp   pid.s-pos.FF2       [SPINDLE]FF2_POS
setp   pid.s-pos.deadband  [SPINDLE]DEADBAND_POS
setp   pid.s-pos.maxoutput [SPINDLE]MAX_OUTPUT_POS
setp   pid.s-pos.error-previous-target true
# This setting is to limit bogus stepgen
# velocity corrections caused by position
# feedback sample time jitter.
setp   pid.s-pos.maxerror 0.012700