#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 0
#define RESTARTBIT 1042
#define ALARMBIT 1024
#define LowHydPressure 1028
#define LowPneumPressure 1029

//MPG Direction
int DirMPG;

//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;


int main() 
{


//--------------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=NO_INPUT_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;
	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=NO_INPUT_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=400000;
	ch1->Accel=1000000;
	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;
	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=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=120000;
	ch2->Accel=5000000;
	ch2->Jerk=6e+6;
	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=3;
	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;
	
	LastF=1;
	LastRr=1;
	LastS=1;

	

	for (;;) // loop forever for CSS - Alarm - Estop - Buttons - MPG
	{
		WaitNextTimeSlice();
		
		ServiceCSS();
		
		// 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),&hcount,&hlast,&hlastsolid);
		//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;
			//printf("SelectX \n");
		}
		else if (ReadBit(SELECTZ))  // is Y selected?
		{
			Axis=1;
			DirMPG=-1;
			//printf("SelectZ \n");
			
		}
		else if (ReadBit(SELECTY))  // is Z selected?
		{
			Axis=3;
			DirMPG=1;
			//printf("SelectY \n");
		}
		else 
			Change1=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)
			{
				LastF=0;
			}
			if (LastF>2)
			{
				LastF=2;
			}
			DoPCFloat(PC_COMM_SET_FRO,LastF);
		}
		//printf("Before RRo \n");


		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;
			}
			if (LastRr>1.01)
			{
				LastRr=1.01;
			}		
			DoPCFloat(PC_COMM_SET_RRO,LastRr);

		}	
		
		//printf("After RRo \n");
		
		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;
			}
			//DoPCFloat(PC_COMM_SET_SSO,LastS);

		}
		
		
		//printf("dernier for \n");
	}

	printf("dernier \n");

    //return 0;
    
}


// 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);
	}
}