// Initialization File
// Added 11/30/2016
// 	4 Axis with Estop support
// 12.30.2016  Added external button support
//	FEED HOLD, CYCLE START, ESTOP, HALT, SAFE Z, SET Z ZERO.
// 1.5.2017 Added MPG Support. 1000 pulse per rev encoder
// 1.28.2017 Added user button support for Laser and Zero DRO's
// 4.3.2020 Starteds working on FRO/SSO control
//          SSO and FRO selection works - Need to add switches
//          Home use as Dec, Reset used as Inc. Need to add limits
// 4.2.2023 Corrected issue with drive enable not working correctly
// 			Konnect output is 53 not 48
//          Removed ClearBit (DRIVEENABLE) located before forever loop
//          Rewrote the ESTOP handeling routine to include Drive enable SET and RESET.
//
//			Started working on an enhanced method of zeroing each axis to work offset. Just an experiment at this time


#include "KMotionDef.h"
#include "PC-DSP.h"
#define TMP 10 		// Spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"

//Define G0 Max Speeds
// based on 5mm per rev ball screws and 2000 pulses per rev
// stepper settings
#define xVelocity 42333 // = 250 ipm
#define yVelocity 42333 // = 250 ipm
#define zVelocity 42333 // = 250 ipm
#define aVelocity 50800 // = 300 ipm

//Define G0 Accelerations
#define xAccel 240000
#define yAccel 240000
#define zAccel 240000
#define aAccel 240000

//Define G0 Jerks
#define xJerk 4e+006
#define yJerk 4e+006
#define zJerk 4e+006
#define aJerk 4e+006

//Konnect Board Inputs
//Axis Limit Switchs
#define XLimitSwitchNegBit 1024
#define XLimitSwitchPosBit 1025
#define YLimitSwitchNegBit 1027
#define YLimitSwitchPosBit 1028
#define ZLimitSwitchNegBit 1029
#define ZLimitSwitchPosBit 1030

// Hard Wired Buttons
#define SELECTX1 1032	// MPG Mulitpier Switch
#define SELECTX10 1033
#define SELECTX100 1034

#define SELECTX 1035	// MPG Axis Switch
#define SELECTY 1036
#define SELECTZ 1037
#define SELECTA 1038

#define CYCLESTARTBIT 1039 // User Buttons
#define FEEDHOLDBIT 1040
#define HALTBIT 1041
#define SAFEZBIT 1042
#define ZERODROBIT 1043
#define SETZBIT 1044
#define LASERBUTTON 1045
#define HOME 1046
#define RESET 1047
#define ESTOP 1048	// MPG Estop Button
#define FROSELECT 1052  // FRO TEST  *****
#define SSOSELECT 1053  //  FRO TEST ******

//MPG Encoder
#define QA 1049		// MPG Encoder A+ Signal
#define QB 1050		// MPG Encoder B+ Signal

// Konnect Board Outputs
#define AXISLEDBIT 49
#define LASERON 52
#define DRIVEENABLE 45

// MPG Smoothing Filter
#define TAU 0.35 	// Max smoothing factor
#define MINTAU 0.12 	// min smoothing factor
#define RATE 1.1 	// Smoothing factor rate of change per second after no mpg movement
#define FINAL_TIME 1.0 	// Set final dest after this amount of time with no change

// MPG Scaling for ppm
#define CPI_X -25.375	// multiply factor by counts per inch divided by 100
#define CPI_Y -25.325
#define CPI_Z -25.400
#define CPI_A -8.33333

// Defines offset between laser crosshair and spindle zero in the X axis
#define LASER_OFFSET -2.8341

//          Defines for FRO Test  *******
#define CHANGE_TOL 0.02  // only update if change is more than this
#define CHANGE_TIME 0.05  // don't update more often then this time

// Define Work Offset Test ********************************************************************
#define Xaxis 0
#define Yaxis 2
#define Zaxis 3
#define Aaxis 4

// Function prototypes for compiler
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *last, int *lastsolid);

// State variables for switch debouncing
int cyclestartlast=0,cyclestartlastsolid=-1,cyclestartcount=0;
int feedholdlast=0,feedholdlastsolid=-1,feedholdcount=0;
int haltlast=0,haltlastsolid=-1,haltcount=0;
int estoplast=0,estoplastsolid=-1,estopcount=0;
int resetlast=0,resetlastsolid=-1,resetcount=0;
int homelast=0,homelastsolid=-1,homecount=0;
int safezlast=0,safezlastsolid=-1,safezcount=0;
int zerodrolast=0,zerodrolastsolid=-1,zerodrocount=0;
int setzlast=0,setzlastsolid=-1,setzcount=0;
int laserlast=0,laserlastsolid=-1,lasercount=0;

//  WOrk Offset Test
int FixtureIndex;
double NewOriginOffsetX,OriginOffsetX,AxisOffsetX,OriginZeroX;
double NewOriginOffsetY,OriginOffsetY,AxisOffsetY,OriginZeroY;
double NewOriginOffsetZ,OriginOffsetZ,AxisOffsetZ,OriginZeroZ;
double NewOriginOffsetA,OriginOffsetA,AxisOffsetA,OriginZeroA;
double Machinex,Machiney,Machinez,Machinea,Machineb,Machinec;
double DROx, DROy, DROz, DROa, DROb, DROc;

// FRO TEST *******
int FROTemp,SSOTemp;
int frolast=0,frolastsolid=-1,frocount=0;
int ssolast=0,ssolastsolid=-1,ssocount=0;

double LastFRO=-1;
double LastFROTime=0;
double LastSSO=-1;
double LastSSOTime=0;

int main()
{
	KStepPresent=TRUE;      // enable KSTEP input multiplexing
	FPGA(KAN_TRIG_REG)=4;  	// Mux PWM0 to JP7 Pin5 IO 44 for KSTEP 

	FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80;  // set polarity and pulse length to 4us
	
	ClearStopImmediately();
	
	ch0->InputMode=NO_INPUT_MODE;
	ch0->OutputMode=STEP_DIR_MODE;
	ch0->Vel=40000;
	ch0->Accel=200000;
	ch0->Jerk=4e+06;
	ch0->P=0;
	ch0->I=0.01;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=200;
	ch0->MaxErr=1e+06;
	ch0->MaxOutput=200;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=0;
	ch0->OutputChan0=8;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x110;
	ch0->LimitSwitchNegBit=1024;
	ch0->LimitSwitchPosBit=1025;
	ch0->SoftLimitPos=1e+30;
	ch0->SoftLimitNeg=-1e+30;
	ch0->InputGain0=1;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_OFF;
	ch0->BacklashAmount=0;
	ch0->BacklashRate=0;
	ch0->invDistPerCycle=1;
	ch0->Lead=0;
	ch0->MaxFollowingError=1000000000;
	ch0->StepperAmplitude=20;

	ch0->iir[0].B0=1;
	ch0->iir[0].B1=0;
	ch0->iir[0].B2=0;
	ch0->iir[0].A1=0;
	ch0->iir[0].A2=0;

	ch0->iir[1].B0=1;
	ch0->iir[1].B1=0;
	ch0->iir[1].B2=0;
	ch0->iir[1].A1=0;
	ch0->iir[1].A2=0;

	ch0->iir[2].B0=0.000769;
	ch0->iir[2].B1=0.001538;
	ch0->iir[2].B2=0.000769;
	ch0->iir[2].A1=1.92076;
	ch0->iir[2].A2=-0.923833;
    EnableAxisDest(0,0);

	ch1->InputMode=NO_INPUT_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=40000;
	ch1->Accel=200000;
	ch1->Jerk=4e+06;
	ch1->P=0;
	ch1->I=0.01;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=200;
	ch1->MaxErr=1e+06;
	ch1->MaxOutput=200;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=0;
	ch1->InputChan1=0;
	ch1->OutputChan0=9;
	ch1->OutputChan1=0;
	ch1->MasterAxis=0;
	ch1->LimitSwitchOptions=0x110;
	ch1->LimitSwitchNegBit=47;
	ch1->LimitSwitchPosBit=47;
	ch1->SoftLimitPos=1e+30;
	ch1->SoftLimitNeg=-1e+30;
	ch1->InputGain0=1;
	ch1->InputGain1=1;
	ch1->InputOffset0=0;
	ch1->InputOffset1=0;
	ch1->OutputGain=1;
	ch1->OutputOffset=0;
	ch1->SlaveGain=1;
	ch1->BacklashMode=BACKLASH_OFF;
	ch1->BacklashAmount=0;
	ch1->BacklashRate=0;
	ch1->invDistPerCycle=1;
	ch1->Lead=0;
	ch1->MaxFollowingError=1000000000;
	ch1->StepperAmplitude=20;

	ch1->iir[0].B0=1;
	ch1->iir[0].B1=0;
	ch1->iir[0].B2=0;
	ch1->iir[0].A1=0;
	ch1->iir[0].A2=0;

	ch1->iir[1].B0=1;
	ch1->iir[1].B1=0;
	ch1->iir[1].B2=0;
	ch1->iir[1].A1=0;
	ch1->iir[1].A2=0;

	ch1->iir[2].B0=0.000769;
	ch1->iir[2].B1=0.001538;
	ch1->iir[2].B2=0.000769;
	ch1->iir[2].A1=1.92076;
	ch1->iir[2].A2=-0.923833;
    EnableAxisDest(1,0);

	// Define Y Axis
	ch2->InputMode=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=40000;
	ch2->Accel=400000;
	ch2->Jerk=4e+06;
	ch2->P=0;
	ch2->I=0.01;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+06;
	ch2->MaxOutput=200;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=10;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x110;
	ch2->LimitSwitchNegBit=1027;
	ch2->LimitSwitchPosBit=1028;
	ch2->SoftLimitPos=1e+09;
	ch2->SoftLimitNeg=-1e+09;
	ch2->InputGain0=1;
	ch2->InputGain1=1;
	ch2->InputOffset0=0;
	ch2->InputOffset1=0;
	ch2->OutputGain=1;
	ch2->OutputOffset=0;
	ch2->SlaveGain=1;
	ch2->BacklashMode=BACKLASH_OFF;
	ch2->BacklashAmount=0;
	ch2->BacklashRate=0;
	ch2->invDistPerCycle=1;
	ch2->Lead=0;
	ch2->MaxFollowingError=1000000000;
	ch2->StepperAmplitude=20;

	ch2->iir[0].B0=1;
	ch2->iir[0].B1=0;
	ch2->iir[0].B2=0;
	ch2->iir[0].A1=0;
	ch2->iir[0].A2=0;

	ch2->iir[1].B0=1;
	ch2->iir[1].B1=0;
	ch2->iir[1].B2=0;
	ch2->iir[1].A1=0;
	ch2->iir[1].A2=0;

	ch2->iir[2].B0=0.000769;
	ch2->iir[2].B1=0.001538;
	ch2->iir[2].B2=0.000769;
	ch2->iir[2].A1=1.92081;
	ch2->iir[2].A2=-0.923885;

	EnableAxisDest(2,0);
	// End Y Axis


	// Define Z Axis
	ch3->InputMode=NO_INPUT_MODE;
	ch3->OutputMode=STEP_DIR_MODE;
	ch3->Vel=40000;
	ch3->Accel=400000;
	ch3->Jerk=4e+06;
	ch3->P=0;
	ch3->I=0.01;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=200;
	ch3->MaxErr=1e+06;
	ch3->MaxOutput=200;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=3;
	ch3->InputChan1=0;
	ch3->OutputChan0=11;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x110;
	ch3->LimitSwitchNegBit=1029;
	ch3->LimitSwitchPosBit=1030;
	ch3->SoftLimitPos=1e+09;
	ch3->SoftLimitNeg=-1e+09;
	ch3->InputGain0=1;
	ch3->InputGain1=1;
	ch3->InputOffset0=0;
	ch3->InputOffset1=0;
	ch3->OutputGain=1;
	ch3->OutputOffset=0;
	ch3->SlaveGain=1;
	ch3->BacklashMode=BACKLASH_OFF;
	ch3->BacklashAmount=0;
	ch3->BacklashRate=0;
	ch3->invDistPerCycle=1;
	ch3->Lead=0;
	ch3->MaxFollowingError=1000000000;
	ch3->StepperAmplitude=20;

	ch3->iir[0].B0=1;
	ch3->iir[0].B1=0;
	ch3->iir[0].B2=0;
	ch3->iir[0].A1=0;
	ch3->iir[0].A2=0;

	ch3->iir[1].B0=1;
	ch3->iir[1].B1=0;
	ch3->iir[1].B2=0;
	ch3->iir[1].A1=0;
	ch3->iir[1].A2=0;

	ch3->iir[2].B0=0.000769;
	ch3->iir[2].B1=0.001538;
	ch3->iir[2].B2=0.000769;
	ch3->iir[2].A1=1.92081;
	ch3->iir[2].A2=-0.923885;

	EnableAxisDest(3,0);
	//End Z Axis

	// Define A Axis
	ch4->InputMode=NO_INPUT_MODE;
	ch4->OutputMode=STEP_DIR_MODE;
	ch4->Vel=40000;
	ch4->Accel=400000;
	ch4->Jerk=4e+06;
	ch4->P=0.2;
	ch4->I=0;
	ch4->D=0;
	ch4->FFAccel=0;
	ch4->FFVel=0;
	ch4->MaxI=200;
	ch4->MaxErr=200;
	ch4->MaxOutput=200;
	ch4->DeadBandGain=1;
	ch4->DeadBandRange=0;
	ch4->InputChan0=4;
	ch4->InputChan1=1;
	ch4->OutputChan0=12;
	ch4->OutputChan1=1;
	ch4->MasterAxis=-1;
	ch4->LimitSwitchOptions=0x100;
	ch4->LimitSwitchNegBit=0;
	ch4->LimitSwitchPosBit=0;
	ch4->SoftLimitPos=1e+09;
	ch4->SoftLimitNeg=-1e+09;
	ch4->InputGain0=1;
	ch4->InputGain1=1;
	ch4->InputOffset0=0;
	ch4->InputOffset1=0;
	ch4->OutputGain=1;
	ch4->OutputOffset=0;
	ch4->SlaveGain=1;
	ch4->BacklashMode=BACKLASH_OFF;
	ch4->BacklashAmount=0;
	ch4->BacklashRate=0;
	ch4->invDistPerCycle=1;
	ch4->Lead=0;
	ch4->MaxFollowingError=10000000;
	ch4->StepperAmplitude=250;

	ch4->iir[0].B0=1;
	ch4->iir[0].B1=0;
	ch4->iir[0].B2=0;
	ch4->iir[0].A1=0;
	ch4->iir[0].A2=0;

	ch4->iir[1].B0=1;
	ch4->iir[1].B1=0;
	ch4->iir[1].B2=0;
	ch4->iir[1].A1=0;
	ch4->iir[1].A2=0;

	ch4->iir[2].B0=1;
	ch4->iir[2].B1=0;
	ch4->iir[2].B2=0;
	ch4->iir[2].A1=0;
	ch4->iir[2].A2=0;

	EnableAxisDest(4,0);
	// End A Axis

	SetBitDirection(DRIVEENABLE,1);  // set Enable Signal as Output
	SetBit(DRIVEENABLE);				// Enable the amplifiers
	
	DefineCoordSystem(0,2,3,-1); // Channel numbers for X,Y,Z,A

	// Configure KFLOP to service Konnect 32 Input 16 output IO board. Address is 0,
	// 16 Outputs are mapped to Virtual IO 48-63 (VirtualBits)
	// 32 Inputs are mapped to Virtual IO 1024-1055 (VirtualBits[0])

	// Attach Service to Aux0 Port (KFLOP JP4) instead of standard Aux1 Port (KFLOP JP6)
	InitAux();
	AddKonnect(0,&VirtualBits,VirtualBitsEx);

	// Added for MPG
	int BitA,Change1=0,Change2=0, DiffX2;
	int PosNoWrap, NewPos, Pos=0, wraps;
	int InMotion=FALSE,Axis,LastAxis=-1;
	double LastChangeTime=0,Target,Factor=0,Factor1=0;
	float NEWTAU;

	int result;
	
	
	//Aded for FRO Test *******
	double FRO = 1.0;
	double SSO = 1.0;
	double T; 
    int FROActive;
    int SSOActive;

	for(;;) // Start of FOREVER loop
	{
        
		if(ReadBit(FROSELECT))
		{
			// Manual Pulse Generator (MPG encoder) for Jog
			// convert quadrature to 2 bit binary
			BitA = ReadBit(QA);
			PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);
			// Diff between expected position based on average of two prev deltas
			// and position with no wraps.  (Keep as X2 to avoid division by 2)

			// DiffX2 = 2*(Pos-PosNoWrap) + (Change2+Change1); --------origonal line--------
			DiffX2 = 2*(Pos-PosNoWrap); // ----------modified to prevent runaway--------------

			// Calc quadrature wraparounds to bring Diff nearest zero
			// offset by 128 wraps to avoid requiring floor()
			wraps = ((DiffX2+1028)>>3)-128;
			// factor in the quadrature wraparounds
			NewPos = PosNoWrap + (wraps<<2);
			Change2 = Change1;
			Change1 = NewPos - Pos;
			Pos = NewPos;
			
		}
		
		if(!ReadBit(FROSELECT))
		{
			// Manual Pulse Generator (MPG encoder) for Jog
			// convert quadrature to 2 bit binary
			BitA = ReadBit(QA);
			PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);
			// Diff between expected position based on average of two prev deltas
			// and position with no wraps.  (Keep as X2 to avoid division by 2)

			// DiffX2 = 2*(Pos-PosNoWrap) + (Change2+Change1); --------origonal line--------
			DiffX2 = 2*(Pos-PosNoWrap); // ----------modified to prevent runaway--------------

			// Calc quadrature wraparounds to bring Diff nearest zero
			// offset by 128 wraps to avoid requiring floor()
			wraps = ((DiffX2+1028)>>3)-128;
			// factor in the quadrature wraparounds
			NewPos = PosNoWrap + (wraps<<2);
			Change2 = Change1;
			Change1 = NewPos - Pos;
			Pos = NewPos;

			//Read selected jog scale for MPG move calculations
			if (ReadBit(SELECTX1))  	// is X1 selected?
				Factor = 0.01;
			else if (ReadBit(SELECTX10))  // is X10 selected?
				Factor = 0.1;
			else if (ReadBit(SELECTX100))  // is X100 selected?
				Factor = 1.0;

			//Read Axis selection to apply scale factor and jog correct axis
			if (ReadBit(SELECTX))  // is x selected?
			{
				Axis=0;
				Factor = Factor * CPI_X; // multiply factor by counts per inch divided by 100
			}
			else if (ReadBit(SELECTY) )  // is y selected?
			{
				Axis=2;
				Factor = Factor * CPI_Y; // multiply factor by counts per inch divided by 100
			}
			else if (ReadBit(SELECTZ))  // is z selected?
			{
				Axis=3;
				Factor = Factor * CPI_Z; // multiply factor by counts per inch divided by 100
			}
			else if (ReadBit(SELECTA))  // is a selected?
			{
				Axis=4;
				Factor = Factor * CPI_A; // multiply factor by counts per inch divided by 100
			}
			else					// ignore encoder
				Change1 = 0;

			//Start Jog movement
			// check if the Axis just changed or we have been
			// converging to the target for a long time
			if (Axis != LastAxis || (InMotion && Time_sec() > LastChangeTime+FINAL_TIME))
			{
				// Optional Acceleration change during mpg movement
				// These next three lines set accelleration back to normal and finishes mpg movement
				//ch0->Accel=4000000;
				//ch1->Accel=4000000;
				//ch2->Accel=6000000;

				if (InMotion)
					Move(LastAxis,Target);  //finalize any motion
				LastAxis = Axis;
				InMotion = FALSE;
			}

			if (Change1) // did we move?
			{
				// Optional Acceleration change during mpg movement
				// These next three lines set acceleration to a lower value for mpg movement
				//ch0->Accel=2000000;
				//ch1->Accel=2000000;
				//ch2->Accel=2500000;

				if (!InMotion) Target = chan[Axis].Dest;
				Target += Change1 * Factor;
				MoveExp(Axis,Target,TAU);  // note: contains a WaitNextTimeSlice
				LastChangeTime = Time_sec();
				InMotion=TRUE;
			}

			if (InMotion) 							// If still moving
			{
				NEWTAU = TAU - (RATE * (Time_sec() - LastChangeTime));	 	// Adjust NEWTAU
				if (NEWTAU < MINTAU) NEWTAU = MINTAU; 				// Limit NEWTAU Mininum
				MoveExp(Axis,Target,NEWTAU);  					// Move with updated smoothing factor
			}
			else
			{
				WaitNextTimeSlice();
			}
		}

		// Beginning of Hard Wird Buttons
		WaitNextTimeSlice();

		// Handle Cycle Start Button
		result = Debounce(ReadBit(CYCLESTARTBIT),&cyclestartcount,&cyclestartlast,&cyclestartlastsolid);
		if  (result == 1)
		{
			DoPC(PC_COMM_EXECUTE);
		}

		// Handle FeedHold/Resume Button
		result = Debounce(ReadBit(FEEDHOLDBIT),&feedholdcount,&feedholdlast,&feedholdlastsolid);
		if  (result == 1)
		{
			if (CS0_StoppingState == 0)
				StopCoordinatedMotion();
			else
				ResumeCoordinatedMotion();
		}

		// Handle Stop Button
		result = Debounce(ReadBit(HALTBIT),&haltcount,&haltlast,&haltlastsolid);
		if  (result == 1)
		{
			DoPC(PC_COMM_HALT);
		}

		// Handle ESTOP Button
		result = Debounce(ReadBit(ESTOP),&estopcount,&estoplast,&estoplastsolid);
		if  (result == 0)
		{
			
			SetBit(DRIVEENABLE);
			WaitNextTimeSlice();
			if (ch0->Enable) DisableAxis(0);  // axis still enabled?  - Disable it
			if (ch1->Enable) DisableAxis(1);  // axis still enabled?  - Disable it
			if (ch2->Enable) DisableAxis(2);  // axis still enabled?  - Disable it
			if (ch3->Enable) DisableAxis(3);  // axis still enabled?  - Disable it
			if (ch4->Enable) DisableAxis(4);  // axis still enabled?  - Disable it
			
			DoPC(PC_COMM_ESTOP);
		}
		if  (result == 1)
		{
			ClearBit(DRIVEENABLE);
			WaitNextTimeSlice();
		}

		// FRO TEST ********
		
		// Handle FRO Button
		
		FROTemp = (ReadBit(FROSELECT));
		//result = Debounce(ReadBit(FROSELECT),&frocount,&frolast,&frolastsolid);
		if(FROTemp == 1)
		//if (result == 1)
		{
			FROActive = 1;
		}
		else
		{
			FROActive = 0;
		}
	
		// Handle SSO Button
		
		SSOTemp = (ReadBit(SSOSELECT));
		if(SSOTemp == 1)
		{
			SSOActive = 1;
		}
		else
		{
			SSOActive = 0;
		}
				
		// Handle Reset Button FRO TEST   *****
		result = Debounce(ReadBit(RESET),&resetcount,&resetlast,&resetlastsolid);
		if  (result == 1)
		{
			if (FROActive==1)
			{
				//T = WaitNextTimeSlice();
				//FRO = FRO + .1;
				// send message to KMotionCNC if the pot changed significantly
				// and it has been a while since the last message
				//if ((FRO > LastFRO+CHANGE_TOL || FRO < LastFRO-CHANGE_TOL) && 
				//T > LastFROTime+CHANGE_TIME)
				if (FRO < 2.0)
				{
					FRO = FRO + .10;
					DoPCFloat(PC_COMM_SET_FRO,FRO);
					LastFRO=FRO;
					//LastFROTime=T;
				}
			}
			else if (SSOActive == 1)
			{
				//T = WaitNextTimeSlice();
				//SSO = SSO + .1;
				// send message to KMotionCNC if the pot changed significantly
				// and it has been a while since the last message
				//if ((SSO > LastSSO+CHANGE_TOL || SSO < LastSSO-CHANGE_TOL) && 
				//T > LastSSOTime+CHANGE_TIME)
				if (SSO < 2.0)
				{
					SSO = SSO + .10;
					DoPCFloat(PC_COMM_SET_SSO,SSO);
					LastSSO=SSO;
					//LastSSOTime=T;
				}
			}
			else
			DoPCInt(PC_COMM_USER_BUTTON,0);
		}

		// Handle Home Button     FRO TEST   *****
		result = Debounce(ReadBit(HOME),&homecount,&homelast,&homelastsolid);
		if  (result == 1)
		{
			if (FROActive==1)
			{
				//T = WaitNextTimeSlice();
				//FRO = FRO - .1;
				// send message to KMotionCNC if the pot changed significantly
				// and it has been a while since the last message
				//if ((FRO > LastFRO+CHANGE_TOL || FRO < LastFRO-CHANGE_TOL) && 
				//T > LastFROTime+CHANGE_TIME)
				if (FRO > .20)
				{
					FRO = FRO - .10;
					DoPCFloat(PC_COMM_SET_FRO,FRO);
					LastFRO=FRO;
					//LastFROTime=T;
				}
			}
			else if (SSOActive==1)
			{
				//T = WaitNextTimeSlice();
				//SSO = SSO - .1;
				// send message to KMotionCNC if the pot changed significantly
				// and it has been a while since the last message
				//if ((SSO > LastSSO+CHANGE_TOL || SSO < LastSSO-CHANGE_TOL) && 
				//T > LastSSOTime+CHANGE_TIME)
				if (SSO > .20)
				{
					SSO = SSO - .10;
					DoPCFloat(PC_COMM_SET_SSO,SSO);
					LastSSO=SSO;
					//LastSSOTime=T;
				}
			}
			else
			DoPCInt(PC_COMM_USER_BUTTON,1);
		}

		// Handle Set Z Offset Button
		result = Debounce(ReadBit(SETZBIT),&setzcount,&setzlast,&setzlastsolid);
		if  (result == 1)
		{
			DoPCInt(PC_COMM_USER_BUTTON,2);
		}

		// Handle Safe Z Button
		result = Debounce(ReadBit(SAFEZBIT),&safezcount,&safezlast,&safezlastsolid);
		if  (result == 1)
		{
			DoPCInt(PC_COMM_USER_BUTTON,3);
		}

		// Handle Laser On/Off Button
		result = Debounce(ReadBit(LASERBUTTON),&lasercount,&laserlast,&laserlastsolid);
		if  (result == 1)
		{
			DoPCInt(PC_COMM_USER_BUTTON,7);
		}

		// Handle ZERO DRO Button
		result = Debounce(ReadBit(ZERODROBIT),&zerodrocount,&zerodrolast,&zerodrolastsolid);
		if  (result == 1)
		{
			if (ReadBit(SELECTX)&& !ReadBit(LASERON))  	//If X is selected and laser is not on, zero X DRO
			{
				DoPCFloat(PC_COMM_SET_X,0.0);
				//GetFixtureIndex(&FixtureIndex); // read active fixture index number
					//printf("FixtureIndex = %d\n", FixtureIndex);
				//GetMachine(&Machinex,&Machiney,&Machinez,&Machinea,&Machineb,&Machinec);
				//NewOriginOffsetX = Machinex;
				//OriginZeroX=0;
				//SetUserDataDouble(TMP,NewOriginOffsetX);
				//SetVars(5201+FixtureIndex*20+Xaxis, 1, TMP);
				//DoPC(PC_COMM_UPDATE_FIXTURE);
			}
			else if (ReadBit(SELECTX)&& ReadBit(LASERON))  	//If X is selected and laser is on, set X DRO to laser offset
			{
				DoPCFloat(PC_COMM_SET_X,LASER_OFFSET);
			}
			else if (ReadBit(SELECTY) )  	// If Y is selected zero Y DRO
			{
				DoPCFloat(PC_COMM_SET_Y,0.0);
				//GetFixtureIndex(&FixtureIndex); // read active fixture index number
					//printf("FixtureIndex = %d\n", FixtureIndex);
				//GetMachine(&Machinex,&Machiney,&Machinez,&Machinea,&Machineb,&Machinec);
				//NewOriginOffsetY = Machiney;
				//OriginZeroY=0;
				//SetUserDataDouble(TMP,NewOriginOffsetY);
				//SetVars(5201+FixtureIndex*20+Yaxis, 1, TMP);
				//DoPC(PC_COMM_UPDATE_FIXTURE);
			}
			else if (ReadBit(SELECTZ))  	// If Z is selected zero Z DRO
			{
				DoPCFloat(PC_COMM_SET_Z,0.0);
				//GetFixtureIndex(&FixtureIndex); // read active fixture index number
					//printf("FixtureIndex = %d\n", FixtureIndex);
				//GetMachine(&Machinex,&Machiney,&Machinez,&Machinea,&Machineb,&Machinec);
				//NewOriginOffsetZ = Machinez;
				//OriginZeroZ=0;
				//SetUserDataDouble(TMP,NewOriginOffsetZ);
				//SetVars(5201+FixtureIndex*20+Zaxis, 1, TMP);
				//DoPC(PC_COMM_UPDATE_FIXTURE);
			}
			else if (ReadBit(SELECTA))  	// If A is selected zero A DRO
			{
				DoPCFloat(PC_COMM_SET_A,0.0);
				//GetFixtureIndex(&FixtureIndex); // read active fixture index number
					//printf("FixtureIndex = %d\n", FixtureIndex);
				//GetMachine(&Machinex,&Machiney,&Machinez,&Machinea,&Machineb,&Machinec);
				//NewOriginOffsetA = Machinea;
				//OriginZeroA=0;
				//SetUserDataDouble(TMP,NewOriginOffsetA);
				//SetVars(5201+FixtureIndex*20+Aaxis, 1, TMP);
				//DoPC(PC_COMM_UPDATE_FIXTURE);
			}
		}
		//Screen dispaly LEDs for current pendant controlled axis
		if (ReadBit(SELECTX))  		// is X selected?
		{
			SetBit(AXISLEDBIT);
		}
		else if (ReadBit(SELECTY) )  	// is Y selected?
		{
			SetBit(AXISLEDBIT);
		}
		else if (ReadBit(SELECTZ))  	// is Z selected?
		{
			SetBit(AXISLEDBIT);
		}
		else if (ReadBit(SELECTA))  	// is A selected?
		{
			SetBit(AXISLEDBIT);
		}
		else
		{
			ClearBit(AXISLEDBIT);
		}

	}
}


// Put a Float as a parameter and pass the command to the App
int DoPCFloat(int cmd, float f)
{
	int result;
	persist.UserData[PC_COMM_PERSIST+1] = *(int*)&f;
	return DoPC(cmd);
}

// Pass a command to the PC and wait for it to handshake
// that it was received by either clearing the command
// or changing it to a negative error code
int DoPC(int cmd)
{
	int result;

	persist.UserData[PC_COMM_PERSIST]=cmd;

	do
	{
		WaitNextTimeSlice();
	}while (result=persist.UserData[PC_COMM_PERSIST]>0);
	return result;
}


// Debounce a bit
//
// return 1 one time when first debounced high
// return 0 one time when first debounced low
// return -1 otherwise
#define DBTIME 300

int Debounce(int n, int *cnt, int *last, int *lastsolid)
{
	int v = -1;

	if (n == *last)  // same as last time?
	{
		if (*cnt == DBTIME-1)
		{
			if (n != *lastsolid)
			{
				v = *lastsolid = n;  // return debounced value
			}
		}
		if (*cnt < DBTIME)	(*cnt)++;
	}
	else
	{
		*cnt = 0;  // reset count
	}
	*last = n;
	return v;
}