#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"

//MPG Definition
#define QA 44	// define to which IO bits the AB signals are connected 
#define QB 45	

#define TAU 0.08 // smoothness factor (Low Pass Time constant seconds)
#define FINAL_TIME 1.0 // Set final dest after this amount of time with no change

#define MPG_LOW_PASS_K 0.001f  // smaller gives more smoothing
#define MPG_ACCEL 0.01f        // bigger gives more effect

#include "MySpindleDefs.h"
#include "CSSJog.c"


//Potmeters definition
#define SELECTX 16
#define SELECTZ 17
#define SELECTY 18


#define Potx1 19
#define Potx10 20
#define Potx100 21

#define PotXX 22
#define PotYY 22

#define Fro 23
#define Rro 24
#define Sso 25




// Buttons definition
#define ESTOPPOWER 48

#define FEEDHOLDBIT 1041
#define CYCLESTARTBIT 1040
#define ESTOP 1025
#define HALTBIT 1042
#define RESTARTBIT 1042
#define ALARMBIT 1024
#define LowHydPressure 1028
#define LowPneumPressure 1029

//MPG Direction
int DirMPG;

//MPG counter for sliders refresh
int CounterFresh=0;

//Screenset slider status
int Slider;

// function prototypes for compiler
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *ON);

// state variables for switch debouncing
int fON=1,fcnt=0;
int cON=1,ccnt=0;
int eON=0,ecnt=0;
int hON=1,hcnt=0;
int rON=1,rcnt=0;
int zON=1,zcnt=0;
int aON=1,acnt=0;
int pON=1,pcnt=0;
int yON=1,ycnt=0;

// Hydraulic pump
int PumpRelay = 52;


long value0;
long value1;
long value2;
long value3;
long value4;

float Vout = 0.0; // desired voltage

int main() 
{

//--------------PWM Spindle----------------//

	void ServiceKonnectPWM(void);
	double spindlespeed;
	double T,T0 = 0;
	


//--------------CSS Parameters--------------//

	int *css_mode = &persist.UserData[PC_COMM_CSS_MODE];			// Mode 1=Normal RPM mode. 2=CSS
	float *css_xoff = &persist.UserData[PC_COMM_CSS_X_OFFSET];		// X axis counts for Radius zero
	float *css_xfactor = &persist.UserData[PC_COMM_CSS_X_FACTOR];	// X axis factor to convert counts to inches
	float *css_s = &persist.UserData[PC_COMM_CSS_S];				// S speed setting in inches/sec
	float *css_max_rpm = &persist.UserData[PC_COMM_CSS_MAX_RPM];	// Limit max RPM to this value as Radius approaches zero
	
	double css_T=0; 	// update only every so often
	#define CSS_UPDATE_DT 0.05
	
	
//------------Other Parameters-------------//

	printf("Init START\n");
	InitAux();
	AddKonnect(0,&VirtualBits,VirtualBitsEx);

	double Tau = 0.003; // seconds for Low Pass Filter Time Constant
	KLP = exp(-TIMEBASE/Tau);

	SetBitDirection(PumpRelay,1);
	SetBit(PumpRelay);
	
	SetBitDirection(ESTOPPOWER,1);
	SetBit(ESTOPPOWER);
	
	
	Delay_sec(2);
	
//------------MPG Parameters--------------//
	
	
	int BitA,Change1=0,Change2=0, Change3=0, DiffX2;
	int PosNoWrap, NewPos, Pos=0, wraps;
	int InMotion=FALSE,Axis,LastAxis=-1;
	double LastChangeTime=0,Target,Factor=0, LastF, LastRr, LastS;
	float MPG_Vel_Filtered=0.0;
	
	
//----------Axis Parameters--------------//


	//X
	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=STEP_DIR_MODE;
	ch0->Vel=400000;
	ch0->Accel=1000000;
	ch0->Jerk=3e+09;
	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=20;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x100;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=10e999;
	ch0->SoftLimitNeg=-10e999;
	ch0->InputGain0=1.2;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_LINEAR;
	ch0->BacklashAmount=5;
	ch0->BacklashRate=5000;
	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.000768788;
	ch0->iir[2].B1=0.00153758;
	ch0->iir[2].B2=0.000768788;
	ch0->iir[2].A1=1.92076;
	ch0->iir[2].A2=-0.923833;
	ch0->Position = ch0->Dest;
	EnableAxis(0);

	//Z
	ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=300000;
	ch1->Accel=500000;
	ch1->Jerk=3e+09;
	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=1;
	ch1->InputChan1=0;
	ch1->OutputChan0=21;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x100;
	ch1->LimitSwitchNegBit=0;
	ch1->LimitSwitchPosBit=0;
	ch1->SoftLimitPos=10e999;
	ch1->SoftLimitNeg=-10e999;
	ch1->InputGain0=1.15;
	ch1->InputGain1=1;
	ch1->InputOffset0=0;
	ch1->InputOffset1=0;
	ch1->OutputGain=1;
	ch1->OutputOffset=0;
	ch1->SlaveGain=1;
	ch1->BacklashMode=BACKLASH_LINEAR;
	ch1->BacklashAmount=6;
	ch1->BacklashRate=5000;
	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.92081;
	ch1->iir[2].A2=-0.923885;
	ch1->Position = ch1->Dest;
    EnableAxis(1);

	//Spindle1
	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=NO_OUTPUT_MODE;
	ch2->Vel=50000000;
	ch2->Accel=500000;
	ch2->Jerk=6e+9;
	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=23;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x100;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=10e999;
	ch2->SoftLimitNeg=-10e999;
	ch2->InputGain0=1;
	ch2->InputGain1=1;
	ch2->InputOffset0=0;
	ch2->InputOffset1=0;
	ch2->OutputGain=6;
	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;
	ch2->Position = ch2->Dest;
    EnableAxis(2);


	//Y
	ch3->InputMode=NO_INPUT_MODE;
	ch3->OutputMode=STEP_DIR_MODE;
	ch3->Vel=120000;
	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=2;
	ch3->InputChan1=0;
	ch3->OutputChan0=22;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x100;
	ch3->LimitSwitchNegBit=0;
	ch3->LimitSwitchPosBit=0;
	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_LINEAR;
	ch3->BacklashAmount=110;
	ch3->BacklashRate=1000;
	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;
	ch3->Position = ch3->Dest;
    EnableAxis(3);


	//Spindle 2
	ch4->InputMode=NO_INPUT_MODE;
	ch4->OutputMode=STEP_DIR_MODE;
	ch4->Vel=4000000;
	ch4->Accel=500000; //100000
	ch4->Jerk=4e+10;
	ch4->P=0.0; //1.5
	ch4->I=0.0000;  //0.001
	ch4->D=0;
	ch4->FFAccel=0;
	ch4->FFVel=0;
	ch4->MaxI=200000;
	ch4->MaxErr=4000;
	ch4->MaxOutput=00000;
	ch4->DeadBandGain=0;
	ch4->DeadBandRange=0;
	ch4->InputChan0=1;
	ch4->InputChan1=1;
	ch4->OutputChan0=19;
	ch4->OutputChan1=1;
	ch4->MasterAxis=-1;
	ch4->LimitSwitchOptions=0x100;
	ch4->LimitSwitchNegBit=0;
	ch4->LimitSwitchPosBit=0;
	ch4->SoftLimitPos=1e+999;
	ch4->SoftLimitNeg=-1e+999;
	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=4e+100;
	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;
	ch4->Position = ch4->Dest;
    	EnableAxis(4);
	
	
	//Encoder Turret
	ch5->InputMode=ENCODER_MODE;
	ch5->OutputMode=NO_OUTPUT_MODE;
	ch5->Vel=40000;
	ch5->Accel=400000;
	ch5->Jerk=4e+06;
	ch5->P=0.2;
	ch5->I=0;
	ch5->D=0;
	ch5->FFAccel=0;
	ch5->FFVel=0;
	ch5->MaxI=200;
	ch5->MaxErr=200;
	ch5->MaxOutput=200;
	ch5->DeadBandGain=1;
	ch5->DeadBandRange=0;
	ch5->InputChan0=3;
	ch5->InputChan1=0;
	ch5->OutputChan0=0;
	ch5->OutputChan1=0;
	ch5->MasterAxis=-1;
	ch5->LimitSwitchOptions=0x100;
	ch5->LimitSwitchNegBit=0;
	ch5->LimitSwitchPosBit=0;
	ch5->SoftLimitPos=1e+09;
	ch5->SoftLimitNeg=-1e+09;
	ch5->InputGain0=-1;
	ch5->InputGain1=1;
	ch5->InputOffset0=0;
	ch5->InputOffset1=0;
	ch5->OutputGain=1;
	ch5->OutputOffset=0;
	ch5->SlaveGain=1;
	ch5->BacklashMode=BACKLASH_OFF;
	ch5->BacklashAmount=0;
	ch5->BacklashRate=0;
	ch5->invDistPerCycle=1;
	ch5->Lead=0;
	ch5->MaxFollowingError=10000000;
	ch5->StepperAmplitude=250;

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

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

	ch5->iir[2].B0=1;
	ch5->iir[2].B1=0;
	ch5->iir[2].B2=0;
	ch5->iir[2].A1=0;
	ch5->iir[2].A2=0;
	EnableAxisDest(5,0);



	//ConfigSpindle(1,4,0.1,0.1,10000);
	DefineCoordSystem(0,-1,1,-1);
	
	//Setting of encoder filter time
	FPGAW(ENC_NOISE_FILTER_ADD)=255;
	
	//Setting length of the step/dir
	FPGA(STEP_PULSE_LENGTH_ADD)=32;
		
    //printf("Init performed");
	int result;
	
	//Resume Jog
	Jog(0,0);
	Jog(1,0);
	Jog(2,0);
	Jog(3,0);
	Jog(4,0);
	Jog(5,0);
	Jog(6,0);
	Jog(7,0);
	Jog(8,0);
	
	LastF=1;
	LastRr=1;
	LastS=1;

	

	for (;;) // loop forever for CSS - Alarm - Estop - Buttons - MPG
	{
		//WaitNextTimeSlice();
		
		Spindle.pEncoderPos = &chan[2].Dest;
		
		ServiceCSS();
		
		ServiceKonnectPWM();
		spindlespeed = chan[2].last_vel / 40960 ;
		Vout = ( - 0.0004*pow(spindlespeed,5) + 0.0115*pow(spindlespeed,4) - 0.1149*pow(spindlespeed,3) + 0.5385*pow(spindlespeed,2) + 0.0097*spindlespeed  - 0.0043 );

		
		
		// Handle FeedHold/Resume
		result = Debounce(ReadBit(FEEDHOLDBIT),&fcnt,&fON);
		if  (result == 1)
		{
			if (CS0_StoppingState == 0)
				StopCoordinatedMotion();
			else
				ResumeCoordinatedMotion();
		}

		// Handle Cycle Start
		result = Debounce(ReadBit(CYCLESTARTBIT),&ccnt,&cON);
		if  (result == 1)
		{
			DoPC(PC_COMM_EXECUTE);
		}

		// Handle ESTOP
		result = Debounce(ReadBit(ESTOP),&ecnt,&eON);
		if  (result == 1)
		{
			printf("Estop \n");
			DoPC(PC_COMM_ESTOP);
		}
		
		// Handle LowHydPressure
		result = Debounce(ReadBit(LowHydPressure),&ycnt,yON);
		if  (result == 1)
		{
			printf("Low Hydraulic Pressure \n");
			DoPC(PC_COMM_ESTOP);
		}
		
		// Handle LowPneumPressure
		result = Debounce(ReadBit(LowPneumPressure),&pcnt,pON);
		if  (result == 1)
		{
			printf("Low Pneumatic Pressure \n");
			DoPC(PC_COMM_ESTOP);
		}
		
		 //Handle HALT
		result = Debounce(ReadBit(HALTBIT),&hcnt,&hON);
		if  (result == 1)
		{
			DoPC(PC_COMM_HALT);
		}
		
		// Handle RESTART
		//result = Debounce(ReadBit(RESTARTBIT),&rcount,&rlast,&rlastsolid);
		//if  (result == 1)
		//{
		//	DoPC(PC_COMM_RESTART);
		//}
		
		
		// Handle ALARM
		result = Debounce(ReadBit(ALARMBIT),&acnt,&aON);
		if  (result == 1)
		{
			printf("Estop - ALARM \n");
			DoPC(PC_COMM_ESTOP);
		}
		
		
		//------------------MPG--------------------
		
		
		// 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);
		
		// 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;
		Change3 = NewPos - Pos;
		Pos = NewPos;
		
		MPG_Vel_Filtered = Change1 * MPG_LOW_PASS_K + MPG_Vel_Filtered * (1.0f-MPG_LOW_PASS_K);


		if (ReadBit(Potx1))  // is X1 selected?		
			Factor = 1;
		else if (ReadBit(Potx10))  // is X10 selected?
			Factor = 5;
		else if (ReadBit(Potx100))  // is X100 selected?
			Factor = 25.0;
		else                   // must be X0.00001 then
			Factor = 0;


		if (ReadBit(SELECTX))  // is X selected?
		{
			Axis=0;
			DirMPG=1;
			Slider=0;

		}
		else if (ReadBit(SELECTZ))  // is Y selected?
		{
			Axis=1;
			DirMPG=-1;
			Slider=0;

			
		}
		else if (ReadBit(SELECTY))  // is Z selected?
		{
			Axis=3;
			DirMPG=1;
			Slider=0;

		}
		else if (ReadBit(Fro))  // is Fro selected?
		{
			Axis=6;
			DirMPG=1;
			Factor = 1;

		}
		else if (ReadBit(Rro))  // is Rro selected?
		{
			Axis=6;
			DirMPG=0;
			Factor = 1;

		}
		else if (ReadBit(Sso))  // is Rro selected?
		{
			Axis=6;
			DirMPG=0;
			Factor = 1;

		}
		else 
		{
			Axis=6;
			Slider=0;
		}
			
			
		// 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))
		{
			if (InMotion)
				Move(LastAxis,Target);  //finalize any motion
	
			LastAxis = Axis;
			InMotion = FALSE;
		}
		
		if (Change1) // did we move?
		{
			if (!InMotion) Target = chan[Axis].Dest;
			Target += Change1 * Factor * (1.0+MPG_ACCEL*fast_fabs(MPG_Vel_Filtered)) * DirMPG;
			MoveExp(Axis,Target,TAU);  // note: contains a WaitNextTimeSlice
			LastChangeTime = Time_sec();
			InMotion=TRUE;
		}
		else
		{
			//WaitNextTimeSlice();
		}		
	
	
	
		if (ReadBit(Fro))  // is FRO selected? RRO SSO
		{
			if (Slider!=1)
			{
				ScreenScript("WinMsg:DlgName;IDC_Feed;BM_CLICK;");
				Slider=1;
			}
				
			LastF=LastF-0.0025*Change3;
		
			if (LastF<0.1)
			{
				LastF=0.1;
			}
			if (LastF>2)
			{
				LastF=2;
			}
			
			if (CounterFresh > 2000)
			{
			
				//WaitNextTimeSlice;			
				DoPCFloat(PC_COMM_SET_FRO,LastF);
				CounterFresh = 0;
				
			}
			
			CounterFresh ++;
		}


		if (ReadBit(Rro))  // is RRO selected? RRO SSO
		{
			if (Slider!=2)
			{
				ScreenScript("WinMsg:DlgName;IDC_Rapid;BM_CLICK;");
				Slider=2;
			}
			
			LastRr=LastRr-0.0025*Change3;
			if (LastRr<0)
			{
				LastRr=0.1;
			}
			if (LastRr>1)
			{
				LastRr=1;
			}
			
			if (CounterFresh > 2000)
			{
			
				//WaitNextTimeSlice;			
				DoPCFloat(PC_COMM_SET_RRO,LastRr);
				CounterFresh = 0;
				
			}
			
			CounterFresh ++;
			

		}	
		
;
		
		if (ReadBit(Sso))  // is SSO selected? RRO SSO
		{
			LastS=LastS-0.0025*Change3;		
			if (LastS<0.7)
			{
				LastS=0.7;
			}
			if (LastS>1.3)
			{
				LastS=1.3;
			}
	
			if (CounterFresh > 2000)
			{
			
				//WaitNextTimeSlice;			
				DoPCFloat(PC_COMM_SET_SSO,LastS);
				CounterFresh = 0;
				
			}
			
			CounterFresh ++;

		}
		
		

	}




    
}


// 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);
	
	//printf("Result = %d\n",result);

	return result;
}

// Debounce a bit
//
// return 1 if debounced
// return 0 otherwise 

#define DBTIME 500

int Debounce(int n, int *cnt, int *ON)
{
	int v = 0;
	
	if (n == *ON)  // same as define ON
	{
		if (*cnt == DBTIME-1)
		{
			v = 1;  // return 1
		}
		if (*cnt < DBTIME)	(*cnt)++;
		
	}
	
	else
	{
		*cnt = 0;  // reset count
	}
	return v;
}

void ServiceCSS(void)
{
	float rpm;
	double T=Time_sec();
	
	if (*css_mode == 2 && T > css_T) 	// check if we are in CSS mode and it is time to update
	{
		css_T=T+CSS_UPDATE_DT;	// determine next time to update
		
		// convert axis position to distance from center in inches
		float radius = fast_fabs((chan[CS0_axis_x].Dest - *css_xoff) * *css_xfactor);
		
		if (radius > 0.0f)
			rpm = *css_s / (radius * (TWO_PI_F/60.0f));
		else
			rpm = *css_max_rpm;
			
		if (rpm > *css_max_rpm) rpm = *css_max_rpm;
		
		if (persist.UserData[STATEVAR]!=0) 	// if spindle is already on, ramp to new speed
		{
			if (USE_POS_NEG_VOLTAGE)
				Jog(SPINDLEAXIS,rpm * FACTOR * persist.UserData[STATEVAR]);
			else
				Jog(SPINDLEAXIS,rpm * FACTOR);
		}
		// printf("xoff=%f radius= %f xfactor=%f s=%f(ips) maxrpm=%f rpm=%f\n",*css_xoff,radius,*css_xfactor,*css_s,*css_max_rpm,rpm);
	}
}

#define C 0.00029f // 1000uF
#define R 100.0f // 100 ohms
#define Vcc 12f // supply voltage
#define HIGH_BIT 61 // This output drives Cap high
#define LOW_BIT 60 // This output drives Cap low

void ServiceKonnectPWM(void)
{

	static int FirstTime=TRUE;
	static float Vc=0.0f;
	static double T0;
	static int State;
	double T=Time_sec();
	if (FirstTime)
	{
		FirstTime=FALSE;
		T0=T;
		State=0;
	}
	else
	{
		float V,I;
		// Compute Voltage applied to Cap
		V=Vcc*State;
		// Compute current
		I=(V-Vc)/R;
		// Compute new Cap Voltage
		Vc += I/C*(T-T0);
		// determine next state
		
		if (Vc > Vout)
		{
			ClearBit(HIGH_BIT);
			SetBit(LOW_BIT);
			State=0;
		}
		else
		{
			ClearBit(LOW_BIT);
			SetBit(HIGH_BIT);
			State=1;
		}
		
	T0=T; // save time when applied
	}
}