component thc "Torch Height Control";

description
"""
Torch Height Control
Mesa THC > Encoder > LinuxCNC THC component

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 inside the THC component.

The THCAD card sends a frequency at 0 volts so the scale offset parameter is
used to zero the calculated voltage.

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 => input pin
output pin => Plasma Torch Start Arc Contacts

HAL Plasma Connections
encoder.nn.velocity => thc.encoder-vel (tip voltage)
motion.spindle-on => output pin (start the arc)
thc.arc-ok <= motion.digital-in-00 <= input pin (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 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";

// 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 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, 80% = .8";
param rw float voltage_tol "The deviation of Tip Voltage before correction takes place";
param rw float correction_tol "Proportional offset, (volts_requested - volts)*thisnumber";

// Global Variables
variable float offset;
variable float correction_vel;//Changed from parameter
variable float last_z_in;

function _;

;;

#include "rtapi_math.h"

FUNCTION(_) {
    // convert encoder velocity to volts
    volts = (encoder_vel - scale_offset) * vel_scale;
    correction_vel = fabs((volts_requested - volts) * correction_tol);//Proportional offset based on voltage
    if(volts < 0){volts = 0;} // make sure volts is not negative

    if(enable){
        float min_velocity = requested_vel -(requested_vel*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 - voltage_tol) > volts_requested){
                offset -= correction_vel;
            }
            if((volts + voltage_tol) < volts_requested){
                offset += correction_vel;
            }
            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
    }
}