#include "KMotionDef.h"


//  R I G I D   T A P P I N G   


//  Mcode Format = M119 P.05 Q-.5 (P is pitch, Q is depth from start position)


// DONT FEEDHOLD WHILE TAPPING! 

#define ZAXIS 2			// Axis to slave
#define SPINDLE_AXIS 4		// Spindle Encoder Feedback Axis 
#define SPINDLECW 154		// SPINDLE CW Bit number
#define SPINDLECCW 155		// SPINDLE CCW Bit number
#define Z_CPI 152400		// Counts per inch of slave axis
#define CPR 7760		// Counts per rev of spindle axis
#define TAU 0.001		// Smoothing time
#define MAXTAPSPEED 850 	// Maximum Tapping speed in rpm
#define DELAY 2.0		// Delay to keep slaving while spindle stops (this is a factor, not seconds)
#define OSFACTOR 0.008	 	// Overshoot Factor (zero cancels overshoot compensation)

double SlaveGain,ToCut,Z0,S0,OS;

void Slave(void);


main()
{
		
// Set variables
	
        float Speed = *(float *)&persist.UserData[9];     	//Current spindle speed
	float DPR = *(float *)&persist.UserData[0];      	//(MCode Parameter P) Distance per rev (Thread Pitch)
	float Depth = *(float *)&persist.UserData[1];         	//(MCode Parameter Q) Depth in incremental coords from current position

	printf("Pitch=%f\n",DPR);				// Print Pitch
	printf("Depth=%f\n",Depth);				// Print Depth
	

// Check if Max Speed exceeded

	if (Speed > MAXTAPSPEED) 
	{
		ClearBit(144); 					// e-stop
		printf("MaX Tap Speed Exceeded\n");		// Print error
		return 0;  					// End
	}


// Prep Tapping Parameters
	
	float i = Speed * DELAY; 				// Set variable for stopping delay
	OS = (Speed * Speed) * OSFACTOR * DPR * Z_CPI / 1000;	// Overshoot calculation	
	SlaveGain = Z_CPI * DPR * -1 / CPR;			// Set Slave factor		
	Z0 = chan[ZAXIS].Dest;					// Record Z start position
	float ToCut = Depth * Z_CPI * -1;                   	// Calculted incremental distance to cut
	float StopPoint = Z0 - ToCut + OS;			// Position where spindle is command to stop (Absolute Position)
	Zero(SPINDLE_AXIS);					// Zero Spindle Axis
	float Zmin = 0;						// Set Zmin zero
	float Zmininches = 0;					// Set Zmininches zero
	int Done = 0;						// Tap cycle not complete
	
// Start Spindle
	
 	SetBit(SPINDLECW); 					// spindle on cw
		

// Record spindle position

	//while (!ReadBit(1045)) WaitNextTimeSlice(); 		// Option to start off spindle index pulse	
	S0 = chan[SPINDLE_AXIS].Position;			// Record Spindle encoder position


// Begin Tapping Operation
	
        while(Done == 0)       					// Tap while Done = 0
	{
		Slave();					// Slave Z axis
        	if (chan[ZAXIS].Dest <= StopPoint)		// If stoppoint Reached
		{					
			ClearBit(SPINDLECW);			// turn off spindle clockwise	
              		SetBit(SPINDLECCW); 			// turn on spindle counterclockwise   			
		}
		if ((chan[ZAXIS].Dest > Z0) && !ReadBit(SPINDLECW)) 	// If Tapping done
		{				
			ClearBit(SPINDLECCW); 			// turn off spindle counterclockwise
			i--;				 	// Delay while spindle slows down
			if (i<=1) Done = 1;			// Done, exit loop
		}
		if (chan[ZAXIS].Dest < Zmin)			// If Z is lower the Zmin
		{
			Zmin = chan[ZAXIS].Dest;		// Record new Zmin
		}
	}	
	
	MoveRel(ZAXIS, 5);					// Add tiny relative move to finish MoveExp
	while (!CheckDone(ZAXIS));				// wait for Z to finish
	Zmininches = (Z0 - Zmin) / Z_CPI * -1;			// Calculate Z min in inches

	printf("Tap Complete. Z Minimun=%f\n",Zmininches);	// Print Z minimium

}	    


void Slave(void)
{
	float Destination = (chan[SPINDLE_AXIS].Position-S0) * SlaveGain + Z0;
	MoveExp(ZAXIS, Destination, TAU);
	WaitNextTimeSlice();
}