Cut Recovery Not Consistently Triggering

component fibercap "Convert BCL-AMP frequency to an appropriate level";

description """This component takes the BCL-Amplifier frequency input
and converts it to a 'voltage' value that plays nicely with Plasmac.

It also includes power level adjusting based on velocity and dynamic probing which
probes to a set capacitance value instead of touching the material.""";

/* INPUT PINS */
pin in  float bcl_amp_freq                  "BCL-AMP frequency IN";
pin in  float bcl_amp_freq_max                "Max frequency observed from BCL-AMP";  
pin in  float bcl_amp_freq_min                "Minimum frequency observed from BCL-AMP";
pin in  float capacitance_max                "Desired maximum capacitance value";
pin in  float capacitance_min                "Desired minimum capacitance value, usually 0";
pin in  float current_velocity                "Current machine velocity";    
pin in  float cut_capacitance               "Cut capacitance setting, from plasmac.cut-volts";
pin in  float cut_feed_rate                 "Cut velocity, from plasmac.cut-feed-rate";
pin in  float cut_height                    "Cut height, from plasmac.cut-height";
pin in  bit   enable_dynamic_probing   ;
pin in  float full_power_percent    ;
pin in  float lower_power            ;
pin in  float laser_max_power       ;
pin in  float nozzle_short_capacitance      "Desired capacitance to detect nozzle crashes";
pin in  bit ohmic_pierce_delay              "Delay for ohmic sense after torch-on to deal with slag hitting nozzle";
pin in  s32 plasmac_state                   "Plasmac state";
pin in  float probe_factor                  "Adjustment factor for use with dynamic probing";
pin in  bit torch_on                        "Plasmac Torch On signal";

/* OUTPUT PINS */
pin out float current_capacitance   ;
pin out float raw_freq                ;
pin out float laser_power              ;
pin out bit   ohmic_probe           ;

/* LOCAL VARIABLES */
variable float bcl_amp_range            ;
variable float capacitance_range        ;

function _;
author "Bryan LaRochelle";
license "GPL";
;;
#include <rtapi_math.h>

FUNCTION(_) {
    
    /* Scaling the frequency input to a preferred range */
    raw_freq = bcl_amp_freq;
    bcl_amp_range = (bcl_amp_freq_max - bcl_amp_freq_min);
    capacitance_range = (capacitance_max - capacitance_min);
    current_capacitance = (((bcl_amp_freq - bcl_amp_freq_min) * capacitance_range) / bcl_amp_range) + capacitance_min;
    
    /* Set the floor at 0 */
    if(current_capacitance < capacitance_min){
        current_capacitance = capacitance_min;
    }
        
    /* Dynamic Power */
    //if torch.on and motion.current-vel > some number, set power
    
    if(torch_on){
        if(current_velocity*60 > cut_feed_rate * (full_power_percent/100)){
            laser_power = laser_max_power;
        }
        else{
            laser_power = lower_power;
        }
    }


if(enable_dynamic_probing){
    
    /* Dynamic Probing */
    /* If probing, trigger at cut_capacitance instead of material */
    if(plasmac_state == 2){ //probe down state
        if(current_capacitance <= cut_capacitance + probe_factor){
            ohmic_probe = true;
        }
    }
}    

if(!enable_dynamic_probing || plasmac_state != 2){
    /* Catch nozzle tips any other time */
    if(((current_capacitance <= nozzle_short_capacitance + probe_factor) || (current_capacitance <= capacitance_min)) && (!torch_on || ohmic_pierce_delay)){
        ohmic_probe = true;
    }

    else{
        ohmic_probe = false;
    }
  }

}