Ethernet PokeysCNC LinuxCNC

component voorpaneel "Driver for DaBit Emco8 front panel";

option userspace yes;
option extra_link_args "-lPoKeys -lusb-1.0 -lm";

pin out bit isconnected		"true when the pokeys device is found and connected";	
pin out bit chgpump_out		"toggling charge pump output. May be used to check if the component is alive";

pin out bit jog_z_up		"Joystick z-up direction";
pin out bit jog_z_down		"Joystick z-down direction";
pin out bit jog_x_up		"Joystick x-up direction";
pin out bit jog_x_down		"Joystick x-down direction";
pin out bit jog_sel_speed1	"connect the jog-sel-speedX pins to gmoccapy jog-inc-X";
pin out bit jog_sel_speed2;
pin out bit jog_sel_speed3;
pin out bit jog_sel_speed4;

pin out signed wheel_jog_x_counts	"X axis handwheel jog counts"; 
pin out signed wheel_jog_z_counts	"Z axis handwheel jog counts";
pin out float wheel_jog_scale		"jogwheel scale, 1/0.1/0.01/0.001 machine units";

pin out float jogspeed_slider		"Jogspeed pot output, range 0.0-1.0. Connect to gmoccapy.jog-vel-value";
pin out float rapidspeed_slider		"Rapidspeed pot output, range 0.0-1.0. Connect to gmoccapy.rapid-vel-value";
pin out float feedspeed_slider		"feedspeed pot output, range 0.0-1.0. Connect to gmoccapy.feed-override-value";

license "GPL";
;;

#include "PoKeysLib.h"
#include <unistd.h>   /* UNIX standard function definitions */
#include "math.h"

#define POT_EPSILON 0.005
#define POT_AVGCNT 4

int first = 1;
sPoKeysDevice * dev=0;
int i=0, avgidx=0;
float avg_jog[POT_AVGCNT];
float avg_feed[POT_AVGCNT];
float avg_rapid[POT_AVGCNT];



void user_mainloop(void) 
{ 
    while(1){
	float ftmp;
       FOR_ALL_INSTS() {
	isconnected = (dev!=NULL);	
	if(first){
		dev = PK_ConnectToDeviceWSerial(23249, 1000);	/* Oh well, for personal use adding the serial in code is OK */
		if (dev) {
			PK_PinConfigurationGet(dev);
	        	PK_EncoderConfigurationGet(dev);
			usleep(50000);
			first=0;
		}
	} else if (dev) {
		/* Fetch values from Pokeys device */
		if (PK_EncoderValuesGet(dev) != PK_OK) { dev=NULL; }
		if (PK_DigitalIOGet(dev) != PK_OK) { dev=NULL; }
		if (PK_AnalogIOGet(dev) != PK_OK) { dev=NULL; }
		if (dev) {
			/* joystick pins. Note that indexes are one lower than the pokeys pin number */
			jog_z_up = dev->Pins[31].DigitalValueGet;
			jog_z_down = dev->Pins[32].DigitalValueGet;
			jog_x_up = dev->Pins[29].DigitalValueGet;
			jog_x_down = dev->Pins[30].DigitalValueGet;
			jog_sel_speed1 = dev->Pins[39].DigitalValueGet;
			jog_sel_speed2 = dev->Pins[38].DigitalValueGet;
			jog_sel_speed3 = dev->Pins[37].DigitalValueGet;
			jog_sel_speed4 = dev->Pins[36].DigitalValueGet;

			/* feed overrides. ain-4 is jogspeed, 5 is rapid, 6 is feed */
			ftmp=0; avg_jog[avgidx] = ((float)dev->Pins[44].AnalogValue) / 4095.0;			
			for (i=0;i<POT_AVGCNT;i++) ftmp+=(1.0/(float)POT_AVGCNT)*avg_jog[i];
			if (fabs(jogspeed_slider - ftmp) > POT_EPSILON) 
				jogspeed_slider = ftmp;
			
			ftmp=0; avg_rapid[avgidx] = ((float)dev->Pins[45].AnalogValue) / 4095.0;
			for (i=0;i<POT_AVGCNT;i++) ftmp+=(1.0/(float)POT_AVGCNT)*avg_rapid[i];
			if (fabs(rapidspeed_slider - ftmp) > POT_EPSILON) 
				rapidspeed_slider = ftmp;
				
			ftmp=0; avg_feed[avgidx]= ((float)dev->Pins[46].AnalogValue) / 4095.0;
			for (i=0;i<POT_AVGCNT;i++) ftmp+=(1.0/(float)POT_AVGCNT)*avg_feed[i];
			if (fabs(feedspeed_slider - ftmp) > POT_EPSILON) 
				feedspeed_slider = ftmp;
			if (++avgidx >= POT_AVGCNT) avgidx=0;

			/* handwheels */
			wheel_jog_x_counts = dev->Encoders[0].encoderValue;
			wheel_jog_z_counts = dev->Encoders[1].encoderValue;
			ftmp=0;
			if (dev->Pins[54].DigitalValueGet) ftmp += 1.0;
			else if (dev->Pins[52].DigitalValueGet) ftmp += 0.1;
			else if (dev->Pins[51].DigitalValueGet) ftmp += 0.01;
			else if (dev->Pins[50].DigitalValueGet) ftmp += 0.001;
			wheel_jog_scale = ftmp;
			
			//for(i=0;i<55;i++)dev->Pins[i].DigitalValueSet=out(i);     
			//PK_DigitalIOSet(dev); 
			chgpump_out = !chgpump_out;	
			usleep(50000);
		} 
	  } else {
	  	/* No device handle. Setup default values */
	    	jog_z_up = false;
		jog_z_down = false;
		jog_x_up = false;
		jog_x_down = false;
		jog_sel_speed1 = true;
		jog_sel_speed2 = false;			
		jog_sel_speed3 = false;
		jog_sel_speed4 = false;
		jogspeed_slider = 0.5;
		rapidspeed_slider = 0.5;
		feedspeed_slider = 0.5;
		/* Restart init */
	    	first = 1;
	  }
        }
    }

    exit(0);
}