// Touch Off_v3.c
//
// Edited version of touch off routine for Jon's & Fred's routers.
// Modified from Randy's version, with sign changes and pin numbers.
// Mic'ed block was 0.514" thick, desired back off total of 1".
//
// v1 edit based off of Randy's original version 040718  jpm
// v2 commented out the chan[Z].Position which was resetting
//    machine z, and therefore messing up soft limits. 051018 jpm
// v3 general cleanup, delete old calls, ref to KStep & changed
//    ticks to be 5080 counts in z per inch 051118 jpm
// 

#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"


#define PIN 19              // I/O pin 19 for the touch off block
#define Z 3                 // Channel 3 is Z axis
#define COUNTS 5080          // 5080 counts per inch on Z axis
#define PUCK_THICKNESS .514 // Thickness of touch off block in inches
#define BACKOFF .486        // Difference between touch off block thickness and desired back off in inches
#define SLOW 2000
#define FAST 5000

main()
{
    
    FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80;  // set polarity and pulse length to 4us
 
    EnableAxis(Z); 
  
    Jog(Z,-SLOW);              // jog slowly negative
    while (ReadBit(PIN));      // loop until IO bit goes low
    Jog(Z,0);                  // stop the axis
    while (!CheckDone(Z));     // loop until motion completes 
 
    MoveRelAtVel(Z,BACKOFF*COUNTS,FAST);    // move up the backoff distance
    while (!CheckDone(Z));

	// set the DRO and current fixture offset Z to 1" by adding thickness & backoff
    DoPCFloat(PC_COMM_SET_Z,PUCK_THICKNESS+BACKOFF);
	
	printf("Puck thickness is = %f\n",PUCK_THICKNESS);
	printf("Back off is = %f\n",BACKOFF);
	printf("Total back off is = %f\n",PUCK_THICKNESS+BACKOFF);

}