component thc_pid "Torch Height Control";

description
"""
Torch Height Control
Mesa THC > Mesa 5i20 Encoder > EMC THC
The Mesa THC sends a frequency based on the voltage detected to the encoder.
The velocity from the encoder is converted to volts with the velocity scale
parameter.
The THCAD card sends a frequency at 0 volts so the scale offset parameter is
used to correct this.

Component Functions
If enabled and torch is on and X + Y velocity is within tolerance of set speed
allow the THC to offset the Z axis as needed to maintain voltage.

If enabled and torch is off and the Z axis is moving up remove any correction
at a rate not to exceed the rate of movement of the Z axis.

If enabled and torch is off and there is no correction
pass the Z position and feed back untouched.

If not enabled pass the Z position and feed back untouched.

Physical Connections
Plasma Torch Arc Voltage Signal => 6 x 487k 1% resistors => THC Arc Voltage In
THC Frequency Signal => Encoder #0, pin A (Input)
Plasma Torch Arc OK Signal => parport.0.pin-15-in
parport.0.pin-16-out => Plasma Torch Start Arc Contacts

HAL Plasma Connections
hm2_5i20.0.encoder.00.velocity => thc.encoder-vel (tip voltage)
motion.spindle-on => parport.0.pin-16-out (start the arc)
thc.arc-ok <= motion.digital-in-00 <= parport.0.pin-15-in (arc ok signal)

HAL Motion Connections
thc.requested-vel <= motion.requested-vel
thc.current-vel <= motion.current-vel

""";

author "John Thornton";

license "GPLv2 or greater";

option singleton yes;

// Input Pins
pin in float encoder_vel "Connect to hm2_5i20.0.encoder.00.velocity";
pin in float current_vel "Connect to motion.current-vel";
pin in float requested_vel "Connect to motion.requested-vel";
pin in float volts_requested "Tip Volts current_vel >= min_velocityequested (SP)";
pin in float vel_tol "Velocity Tolerance (Corner Lock)";
pin in float volts_limit "Maximum Voltage Correction Limit";
pin in bit torch_on "Connect to motion.spindle-on";
pin in bit arc_ok "Arc OK from Plasma Torch";
pin in bit enable "Enable the THC, if not enabled Z position is passed through";
pin in float z_pos_in "Z Motor Position Command in from axis.n.motor-pos-cmd";
pin in float p_gain "P Gain";

// Output Pins
pin out float z_pos_out "Z Motor Position Command Out";
pin out float z_fb_out "Z Position Feedback to Axis";
pin out float volts "The Calculated Volts";
pin out float offset_value "The Current Offset";
pin out bit vel_status "When the THC thinks we are at requested speed";

// Parameters
param rw float vel_scale "The scale to convert the Velocity signal to Volts";
param rw float scale_offset "The offset of the velocity input at 0 volts";
param rw float velocity_tol "The deviation percent from planned velocity";
param rw float voltage_tol "The deviation of Tip Voltage before correction takes place";

// Global Variables
variable float offset;
variable float last_z_in;

function _;

;;

#include "rtapi_math.h"

FUNCTION(_) {
    // convert encoder velocity to volts
    volts = (encoder_vel - scale_offset) * vel_scale;
    if(volts < 0){volts = 0;} // make sure volts is not negative
    offset_value = offset;

    if(enable){
        float min_velocity = requested_vel -(requested_vel*(1/velocity_tol));
        if(current_vel > 0 && current_vel >= min_velocity){vel_status = 1;}
        else {vel_status = 0;}

        if(torch_on && arc_ok && vel_status){ // allow correction
            //if(volts_requested - volts > volts_limit){
            //    volts = volts_requested - volts_limit;
            //}
            //else if(volts_requested + volts > volts_limit){
            //   volts = volts_requested +volts_limit;
            //}
            if (abs(volts_requested - volts) > voltage_tol) {
                offset += (volts_requested - volts) * p_gain;
            }
            last_z_in = 0;
        }
        if(!torch_on){ // remove any offset
            float z_diff;
            z_diff = z_pos_in - last_z_in;
            if(z_diff > 0 && offset != 0){ // torch is moving up
                if(offset > 0){ // positive offset
                    if(offset > z_diff){ // remove some
                        offset -= z_diff;
                    }
                    else {offset = 0;}
                }
                if(offset < 0){ // negative offset
                    if(offset < z_diff){ // remove some
                        offset += z_diff;
                    }
                    else {offset = 0;}
                }
            }
            last_z_in = z_pos_in;
        }
        z_pos_out = z_pos_in + offset;
        z_fb_out = z_pos_in; // keep axis motor position fb from being confused
    }
    if(!enable){
        z_pos_out = z_pos_in;
        z_fb_out = z_pos_in; // keep axis motor position fb from being confused
    }
}