component mpg " ";

description
"""
// compile :
// halcompile --compile mpg.comp
// sudo halcompile --install mpg.comp
// halfile load :
// loadrt	mpg
// setp 	mpg.0.scale 1000	for ilopwass function
// setp 	mpg.0.gain 0.01
// addf 	mpg.0 	servo-thread

/* comp to control more handwheels including ilowpass
	Add input 0 to 6 for shared S32 encoder data. 
	Master and slave inputs are used for axis selection.
	If master axis is on, master bits are forwarded, if master is off slave bits are forwarded.

	input0 S32 encoder input
	input1 S32 encoder input
	input2 S32 encoder input
	input3 S32 encoder input
	input4 S32 encoder input
	input5 S32 encoder input
	input6 S32 encoder input
	
	input7 float input master for jog increment
	input8 float input slave for jog increment
	
	input10 bit master
	input11 bit master
	input12 bit master
	input13 bit master
	input14 bit master
	input15 bit master
	input16 bit slave
	input17 bit slave
	input18 bit slave
	input19 bit slave
	input20 bit slave
	input21 bit slave	

	Output0 addition of input0 to 6 and iolowpass code inside
	Output1 bit = master input10 or if all master input zero output = slave input 16
	Output2 bit = master input11 or if all master input zero output = slave input 17
	Output3 bit = master input12 or if all master input zero output = slave input 18
	Output4 bit = master input13 or if all master input zero output = slave input 19
	Output5 bit = master input14 or if all master input zero output = slave input 20
	Output6 bit = master input15 or if all master input zero output = slave input 21
	Output7 float = output slected master or slave jog increments 
*/
""";
 
author "Chris@cnc";

license "GPL";

// Input pins

pin in s32 in0=0;
pin in s32 in1=0;
pin in s32 in2=0;
pin in s32 in3=0;
pin in s32 in4=0;
pin in s32 in5=0;
pin in s32 in6=0;
pin in float in7=0;
pin in float in8=0;
pin in bit in10=0;
pin in bit in11=0;
pin in bit in12=0;
pin in bit in13=0;
pin in bit in14=0;
pin in bit in15=0;
pin in bit in16=0;
pin in bit in17=0;
pin in bit in18=0;
pin in bit in19=0;
pin in bit in20=0;
pin in bit in21=0;

param rw float scale = 1;
param rw float gain = 1;

// Output pins
pin out s32 out0=0;
pin out bit out1=0;
pin out bit out2=0;
pin out bit out3=0;
pin out bit out4=0;
pin out bit out5=0;
pin out bit out6=0;
pin out float out7=0;

// Global variables
variable double value;

function _ "Update the output based on the input and parameters";

license "";
author "";
;;
#include <rtapi_math.h>

FUNCTION(_) {
    // addition of input0 to input6 * ilowpass
	value += ((in0 + in1 + in2 + in3 + in4 + in6)- value) * gain;
    out0 = (int)(rtapi_s64)floor((value * scale) + 0.5);

	// detect if input on master input 10-15 zero, slave input 17-21 will be forward, else master will be forward
	if((in10 + in11 + in12 + in13 + in14 + in15) == 0) {
		out1=in16;
		out2=in17;
		out3=in18;
		out4=in19;
		out5=in20;
		out6=in21;
		out7=in8;		
		}	

	else {
		out1=in10;
		out2=in11;
		out3=in12;
		out4=in13;
		out5=in14;
		out6=in15;
		out7=in7;
		}

}