control axis.N.motor-pos-cmd for Z height control

#*******************
#  AXIS Z
#*******************

setp   pid.z.Pgain     [AXIS_2]P
setp   pid.z.Igain     [AXIS_2]I
setp   pid.z.Dgain     [AXIS_2]D
setp   pid.z.bias      [AXIS_2]BIAS
setp   pid.z.FF0       [AXIS_2]FF0
setp   pid.z.FF1       [AXIS_2]FF1
setp   pid.z.FF2       [AXIS_2]FF2
setp   pid.z.deadband  [AXIS_2]DEADBAND
setp   pid.z.maxoutput [AXIS_2]MAX_OUTPUT

net z-index-enable  <=>  pid.z.index-enable
net z-enable       => pid.z.enable
net z-output       => pid.z.output
net z-pos-cmd      => pid.z.command
net z-vel-fb       => pid.z.feedback-deriv
net z-pos-fb       => pid.z.feedback

# ---PWM Generator signals/setup---

setp   hm2_5i25.0.7i77.0.1.analogout2-scalemax  [AXIS_2]OUTPUT_SCALE
setp   hm2_5i25.0.7i77.0.1.analogout2-minlim    [AXIS_2]OUTPUT_MIN_LIMIT
setp   hm2_5i25.0.7i77.0.1.analogout2-maxlim    [AXIS_2]OUTPUT_MAX_LIMIT

net z-output                             => hm2_5i25.0.7i77.0.1.analogout2
#net z-pos-cmd    axis.2.motor-pos-cmd
net z-enable     axis.2.amp-enable-out

# ---Encoder feedback signals/setup---

setp    hm2_5i25.0.encoder.02.counter-mode 0
setp    hm2_5i25.0.encoder.02.filter 1
setp    hm2_5i25.0.encoder.02.index-invert 0
setp    hm2_5i25.0.encoder.02.index-mask 0
setp    hm2_5i25.0.encoder.02.index-mask-invert 0
setp    hm2_5i25.0.encoder.02.scale  [AXIS_2]ENCODER_SCALE

net z-pos-fb               <=  hm2_5i25.0.encoder.02.position
net z-vel-fb               <=  hm2_5i25.0.encoder.02.velocity
net z-pos-fb           =>  axis.2.motor-pos-fb
net z-index-enable    axis.2.index-enable  <=>  hm2_5i25.0.encoder.02.index-enable
net z-pos-rawcounts        <=  hm2_5i25.0.encoder.02.rawcounts

#Nozzle height control

#enable the nhc
net LigthGreen => nhc.Enable
#Input commanded position
net ZPosCmdIn axis.2.motor-pos-cmd => nhc.ZPosCmdIn
#Input button for increase the offset
net Mud1Button => nhc.OffsetPlus
#Input button for decrease the offset
net Mud2Button => nhc.OffsetMinus
#Output commanded position + offset if exists
net z-pos-cmd  <= nhc.ZPosCmdOut 
#Output feedback position
#net ZFeedbackOut <= nhc.ZFeedbackOut 

component nhc "Nozzle Height Control";

description 
"""
Nozzle Height Control

""";
 
author "jtc";

license "GPLv2 or greater";

option singleton yes;

// Input Pins
pin in bit Enable "Enable the THC, if not Enabled Z position is passed through";
pin in float ZPosCmdIn "Z Motor Position Command in from axis.n.motor-pos-cmd";
pin in bit OffsetPlus "increases the Offset of the nozzle";
pin in bit OffsetMinus "decrases the Offset of the nozzle";

// Output Pins
pin out float ZPosCmdOut "Z Motor Position Command Out";
pin out float ZFeedbackOut "Z Position Feedback to Axis";


// Global Variables
variable float Offset=0;

function _;

;;

#include "rtapi_math.h"

FUNCTION(_) {
	
	if(OffsetPlus && Enable){ //increases the offset
		Offset+=0.004;
	}
	if(OffsetMinus && Enable){ //decreases the offset
		Offset-=0.004;
	}
      
    if(Enable){
        ZPosCmdOut = ZPosCmdIn + Offset;
        ZFeedbackOut = ZPosCmdIn; // keep axis motor position fb from being confused
    }
    else{	// !Enable
        ZPosCmdOut = ZPosCmdIn;
        ZFeedbackOut = ZPosCmdIn; // keep axis motor position fb from being confused
    }
}