cnc-club.ru

component thc "Torch Height Control";

description
"""
Torch Height Control
Mesa THC > Mesa 5i20 Encoder > EMC THC
The Mesa THC sends a pulse based on the scaled voltage detected.
A velocity is used from the encoder that is being driven by a frequency from
the Mesa THC.
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 "GPL";

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";
param rw float voltage_tol "The deviation of Tip Voltage before correction takes place";
param rw float correction_vel "The Velocity to move Z to correct";

// 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

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 + 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
}
}