#include "KMotionDef.h"
#include "Driver\Defines.c"
#define TMP 10 // which spare persist to use to transfer data
#include "C:\KMotion433\C Programs\KflopToKMotionCNCFunctions.c"

#define Z_UP_AFTER_TOUCH 1.0 //Move up this amount in Inches after tool touch 
#define Offset_FromTouch 1.85 //if you don't want the dro set to zero
#define TOUCH_PROBE_PIN 17
double CountsPerInchZ;



float saveaccel,savejerk;
double touchPos;

double offsetFromFirstTool;
double offsetFromPartZero;
float newDRO;

//-------------------------------------------------------
//-------------------------------------------------------
main()
{
	int FixtureIndex,Units, TWORD, HWORD, DWORD;
	CountsPerInchZ = persist.UserData[ZSPI]; //get count

	// Save current Accel/Jerk
	saveaccel=chan[Z].Accel;
	savejerk=chan[Z].Jerk;

	// Set Accel/Jerk to Very fast
	chan[Z].Accel = 1000000000;
	chan[Z].Jerk = 1000000000;

	// Move Down
	Jog(Z,-5000);

	// Waite for Touch Probe hit
	while(ReadBit(TOUCH_PROBE_PIN))
	{
		if (CheckDone(Z))
			break;
	}
	touchPos = chan[Z].Position;	// Record position

	if (!ReadBit(ESTOP_SW)) return;

	Jog(Z, 0);
	
	// Restore saved Accel/Jerk
	chan[Z].Accel = saveaccel;
	chan[Z].Jerk = savejerk;
	
	Move(Z, touchPos);
	while(!CheckDone(Z));

    GetMiscSettings(&Units, &TWORD, &HWORD, &DWORD);
    if (Units==CANON_UNITS_INCHES)            // for inches
        newDRO = Offset_FromTouch;
    else  
        newDRO = Offset_FromTouch * 25.4;
	
	//printf("%f, %f, %f\n", newDRO, offsetFromPartZero, offsetFromFirstTool);
	
	// Set New DRO Z Height
	DoPCFloat(PC_COMM_SET_Z, newDRO);

	Delay_sec(1.0);
	MoveRel(Z, Z_UP_AFTER_TOUCH * CountsPerInchZ);
}