FPGA (field programmable gate array) programmed for linuxcnc motion

[KINS]
JOINTS     = 6
KINEMATICS = trivkins coordinates=XYYZAB

[DISPLAY]
GEOMETRY = XYZAB

N0010 (Filename: 1e test ultracut.ngc)
N0020 (Post processor: LinuxCNC plasma - laser.scpost)
N0030 (Date: 05/28/2018)
N0040 G21 (Units: Metric)
N0050 G40 G90
N0060 F1 S1
N0070 G40 G64 P0.005 (machine follow path accuracy) 
N0080 M52 P1 (turn on adaptive feed)
N0090 (Part: 1e test ultracut)
N0100 (Operation: Outside Offset, 0, T3: plasma 0.95mm )
N0110 G00 Z10.0000
N0120 X68.2435 Y64.4213
N0130 Z3.0000
N0140 S45
N0150 G38.3 Z-100 F500 (height measuring with probe)
N0160 G10 L2 P1 Z-10 (correction to z height value)
N0170 G0 Z0 F500 (goto z start position)
N0180 M03 M65 P1 (M65 = turn off digital output P1 => output 1)
N0190 M66 P0 L3 Q5 (Q = wait for arc on => motion digital in)
N0200 G04 P0.5
N0210 G01 Z0.0000 F750
N0220 Y62.4213 F3000
N0230 G03 X68.2435 Y62.4213 I0.0000 J14.5250 F3000.0
N0240 G01 Y63.4213 F3000
N0250 M05 M64 P1 (M64 = turn on digital output 1 => remove offsets)
N0260 G00 Z10.0000
N0270 X21.6174 Y77.7952
N0280 Z3.0000
N0290 G38.3 Z-100 F500 (height measuring with probe)
N0300 G10 L2 P1 Z-10 (correction to z height value)
N0310 G0 Z0 F500 (goto z start position)
N0320 M03 M65 P1 (M65 = turn off digital output P1 => output 1)
N0330 M66 P0 L3 Q5 (Q = wait for arc on => motion digital in)
N0340 G04 P0.5
N0350 G01 Z0.0000 F750
N0360 Y75.7952 F3000
N0370 G03 X21.6174 Y75.7952 I0.0000 J2.0250 F3000.0
N0380 G01 Y76.7952 F3000
N0390 M05 M64 P1 (M64 = turn on digital output 1 => remove offsets)
N0400 G00 Z10.0000
N0410 X34.6832 Y10.5482
N0420 Z3.0000
N0430 G38.3 Z-100 F500 (height measuring with probe)
N0440 G10 L2 P1 Z-10 (correction to z height value)
N0450 G0 Z0 F500 (goto z start position)
N0460 M03 M65 P1 (M65 = turn off digital output P1 => output 1)
N0470 M66 P0 L3 Q5 (Q = wait for arc on => motion digital in)
N0480 G04 P0.5
N0490 G01 Z0.0000 F750
N0500 Y8.5482 F3000
N0510 G03 X34.6832 Y8.5482 I0.0000 J24.5250 F3000.0
N0520 G01 Y9.5482 F3000
N0530 M05 M64 P1 (M64 = turn on digital output 1 => remove offsets)
N0540 G00 Z10.0000
N0550 X102.4750 Y90.0000
N0560 Z3.0000
N0570 G38.3 Z-100 F500 (height measuring with probe)
N0580 G10 L2 P1 Z-10 (correction to z height value)
N0590 G0 Z0 F500 (goto z start position)
N0600 M03 M65 P1 (M65 = turn off digital output P1 => output 1)
N0610 M66 P0 L3 Q5 (Q = wait for arc on => motion digital in)
N0620 G04 P0.5
N0630 G01 Z0.0000 F750
N0640 X100.4750 F3000
N0650 Y10.0000
N0660 G02 X90.0000 Y-0.4750 I-10.4750 J0.0000 F3000.0
N0670 G01 X0.0000 F3000
N0680 G02 X-0.4750 Y0.0000 I0.0000 J0.4750 F3000.0
N0690 G01 Y90.0000 F3000
N0700 G02 X10.0000 Y100.4750 I10.4750 J0.0000 F3000.0
N0710 G01 X90.0000 F3000
N0720 G02 X100.4750 Y90.0000 I0.0000 J-10.4750 F3000.0
N0730 G01 X101.4750 F3000
N0740 M05 M64 P1 (M64 = turn on digital output 1 => remove offsets)
N0750 G00 Z10.0000
N0760 M52 P0 (stop adaptive)
N0770 M05 M30 (program end)

function OnAbout(event)
   ctrl = event:GetTextCtrl()
   ctrl:AppendText("Grotius CNC Machines : plasma and laser post processor\n")
   ctrl:AppendText("\n")
   ctrl:AppendText("Modal G-codes and coordinates\n")
   ctrl:AppendText("Comments enclosed with ( and )\n")
   ctrl:AppendText("Incremental IJ\n")
   ctrl:AppendText("Laser outputs power as spindle speed\n")


end


--  made 23-5-2018



function OnInit()
   post.SetCommentChars ("()", "[]")  --make sure ( and ) characters do not appear in system text
   post.Text (" (Filename: ", fileName, ")\n")
   post.Text (" (Post processor: ", postName, ")\n")
   post.Text (" (Date: ", date, ")\n")
   if(scale == metric) then
      post.Text (" G21 (Units: Metric)\n") --metric mode
   else
      post.Text (" G20 (Units: Inches)\n") --inch mode
   end
   post.Text (" G40 G90\n F1 S1\n")
   post.Text (" G40 G64 P0.005 (machine follow path accuracy) \n") 
   post.Text (" M52 P1 (turn on adaptive feed)\n")  
   bigArcs = 1 --stitch arc segments together
   minArcSize = 0.05 --arcs smaller than this are converted to moves
end

function OnNewLine()
   post.Text ("N")
   post.Number (lineNumber, "0000")
   lineNumber = lineNumber + 10
end


function OnFinish()
   post.Text (" M52 P0 (stop adaptive)\n")
   post.Text (" M05 M30 (program end)\n")
end

function OnRapid()
   post.ModalText (" G00")
   post.ModalNumber (" X", endX * scale, "0.0000")
   post.ModalNumber (" Y", endY * scale, "0.0000")
   post.ModalNumber (" Z", (endZ + toolOffset) * scale, "0.0000")
   post.Eol()
end

function OnMove()
   post.ModalText (" G01")
   post.ModalNumber (" X", endX * scale, "0.0000")
   post.ModalNumber (" Y", endY * scale, "0.0000")
   post.ModalNumber (" Z", (endZ + toolOffset) * scale, "0.0000")
   post.ModalNumber (" F", feedRate * scale, "0.###")
   if(toolType == 8 or toolType == 9) then
      post.ModalNumber (" S", power, "0.###")
   end
   post.Eol()
end

function OnArc()
   if(arcAngle <0) then
      post.ModalText (" G03")
   else
      post.ModalText (" G02")
   end
   post.NonModalNumber (" X", endX * scale, "0.0000")
   post.NonModalNumber (" Y", endY * scale, "0.0000")
   post.ModalNumber (" Z", (endZ + toolOffset) * scale, "0.0000")
   post.Text (" I")
   post.Number ((arcCentreX - currentX) * scale, "0.0000")
   post.Text (" J")
   post.Number ((arcCentreY - currentY) * scale, "0.0000")
   post.ModalNumber (" F", feedRate * scale, "0.0###")
   post.Eol()
end


function OnPenDown()
   if (preheat > 0.001) then
      post.ModalText (" G00")
      post.ModalNumber (" Z", cutHeight * scale, "0.0000")
      post.Text ("\n G04 P")
      post.Number (preheat,"0.###")
      post.Eol()
   end
   post.ModalText (" G00")
   post.ModalNumber (" Z", pierceHeight * scale, "0.0000")
   post.ModalNumber (" S", power, "0.###")
   post.Text ("\n G38.3 Z-100 F500 (height measuring with probe)\n") 
   post.Text ("\n G10 L20 P1 Z-10 (correction to z height value)\n") 
   post.Text ("\n G0 Z0 F500 (goto z start position)\n") 
   post.Text ("\n M03 M65 P1 (M65 = turn off digital output P1 => output 1)\n") 
   post.Text ("\n M66 P0 L3 Q5 (Q = wait for arc on => motion digital in)\n")
   
   
   if (pierceDelay > 0.001) then
      post.Text (" G04 P")
      post.Number (pierceDelay,"0.###")
      post.Eol()
   end
end


function OnPenUp()
   post.Text (" M05 M64 P1 (M64 = turn on digital output 1 => remove offsets)\n")
   if (endDelay > 0) then
      post.Text (" G04 P")
      post.Number (endDelay,"0.###")
      post.Eol()
   end
end

function OnNewOperation()
   post.Text (" (Operation: ", operationName, ")\n")
end

function OnComment()
  post.Text(" (",commentText,")\n")
end


function OnNewPart()
   post.Text(" (Part: ",partName,")\n");
end


function OnDrill()
   OnRapid()
   OnPenDown()
   endZ = drillZ
   OnMove()
   OnPenUp()
   endZ = safeZ
   OnRapid()
end

if(torch_on && arc_control && !arc_ok){ // pause machine
            pause_machine = 1;
        }
        if(!torch_on){ // pause machine
            pause_machine = 0;
        }

component thcud "Torch Height Control Up/Down Input";

description 
"""
Torch Height Control
This THC takes either an up or a down input from a 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.

Typical Physical Connections using a Parallel Port
.br
Parallel Pin 12 <= THC controller Plasma Up
.br
Parallel Pin 13 <= THC controller Plasma Down
.br
Parallel Pin 15  <= Plasma Torch Arc Ok Signal
.br
Parallel Pin 16 => Plasma Torch Start Arc Contacts

HAL Plasma Connections
.br
net torch-up thcud.torch-up <= parport.0.pin-12-in
.br
net torch-down thcud.torch-down <= parport.0.pin-13-in
.br
net torch-on motion.spindle-on => parport.0.pin-16-out (start the arc)
.br
net arc-ok thcud.arc-ok <= motion.digital-in-00 <= parport.0.pin-15-in (arc ok signal)

HAL Motion Connections
.br
net requested-vel thcud.requested-vel <= motion.requested-vel
.br
net current-vel thcud.current-vel <= motion.current-vel

Pyvcp Connections
In the xml file you need something like:

  <pyvcp>
  <checkbutton>
    <text>"THC Enable"</text>
    <halpin>"thc-enable"</halpin>
  </checkbutton>
  </pyvcp>

Connect the Pyvcp pins in the postgui.hal file like this:

net thc-enable thcud.enable <= pyvcp.thc-enable


""";
 
author "The real John Thornton, i think it is him B) ";

license "GPLv2 or greater";

option singleton yes;

// Input Pins
pin in bit torch_up "Connect to an input pin";
pin in bit torch_down "Connect to input pin";
pin in float current_vel "Connect to motion.current-vel";
pin in float requested_vel "Connect to motion.requested-vel";
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 bit arc_control "M100 & M101 linking arc lost";

// 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 cur_offset "The Current Offset";
pin out bit vel_status "When the THC thinks we are at requested speed";
pin out bit removing_offset "Pin for testing";
pin out bit pause_machine "Pin for pausing";

// Parameters
param rw float velocity_tol "The deviation percent from planned velocity";
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(_) {
    if(enable){

        if(torch_on && arc_control && !arc_ok){ // pause machine
            pause_machine = 1;
        }
        if(!torch_on){ // pause machine
            pause_machine = 0;
        }

        float min_velocity = requested_vel -(requested_vel*(velocity_tol*0.01));
        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(torch_down){
                offset -= correction_vel;
            }
            if(torch_up){
                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
                removing_offset = 1;
                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;}
                }
            }
            else {removing_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
    }




}