//*************************************************
//Made by JB CNC & Linear Components and CNC-Drive
//*************************************************
//Simple Probe for setting tool offset
//This routine probes in negative Z at the current XY location
//Z offsets for G54 to G59 are then re-written to the new Z0

double ZRetractHeight = -1;  //Height above Z0 to which probe will retract
double CoarseRate = 300; //Feedrate for initial probing
double FineRate = 50;  //Feedrate for fine probing
double Zmin = -200; //maximum probing distance
 
double PlateThickness = 0;  //thickness of the probing plate
double MaterialOffset = 0; //compensation for part thickness

double CombinedOffset = 0;


double Zmachposvariable = 0;
double XToolSetPos = 1300;
double YToolSetPos = 52;
double ZSafePos = -1;
double ZDownPos = -1;
double ZSpeedDown = 200;


if(!exec.GetLED(56)||!exec.GetLED(57)||!exec.GetLED(58)) // If machine was not homed then it is not smart to do this, stop here...
{
  MessageBox.Show("The machine was not yet homed, home the machine before setting Z-zero");
  exec.Stop();
  return;
}

/*
if(Currenttoolvariable == 0) // Tool cannot be zero
{
  MessageBox.Show("Current tool cannot be zero");
  exec.Stop();
  return;
}
  */
  

double val = exec.Question("Vilket verktyg?");

exec.AddStatusmessage("Valt verktyg  # "+ val);

int valint = Convert.ToInt32(val);
exec.Setcurrenttool(valint);
exec.Wait (100);

int Currenttoolvariable = valint;



exec.Callbutton(519); //softlimits off

exec.Code("G53 G0 Z" + ZSafePos); // Move to Z0 position
while(exec.IsMoving()){}
exec.Wait(200);

exec.Code("G53 G0  Y200"); // Move to y position
while(exec.IsMoving()){}

exec.Code("G53 G0  X1300"); // Move to X position
while(exec.IsMoving()){}

exec.Code("G53 G0  X" + XToolSetPos + " Y" +YToolSetPos); // Move to X position
while(exec.IsMoving()){}


exec.Code("G53 G1 Z" + ZDownPos + " F" + ZSpeedDown); // Move to Z0 position
while(exec.IsMoving()){}

exec.Wait(200);

exec.Code("G91 G31 Z" + Zmin + "F" + CoarseRate); // Probe Z quickly to get rough height
while(exec.IsMoving()){}
exec.Wait(200);
if(!exec.GetLED(244)) //Kolla så fick kontakt
{
	MessageBox.Show("Verktyget fick ej kontakt med verktysmätaren");
	return;
}

exec.Code ("G00 Z" + 1.3); // Retract .05" above the plate
while(exec.IsMoving()){}
exec.Wait(100);

exec.Code("G31 Z" + Zmin + "F" + FineRate); // Probe Z slowly for better resolution
while(exec.IsMoving()){}
exec.Wait(200);
if(!exec.GetLED(244)) //Kolla så fick kontakt
{
	MessageBox.Show("Verktyget fick ej kontakt med verktysmätaren");
	return;
}

Zmachposvariable = exec.GetZmachpos();
// Zmachposvariable = Zmachposvariable - Zmachposvariable - Zmachposvariable;

int toolHeightBox = 195 + Currenttoolvariable; //Första rutan för tool lenght minus ett
AS3.Setfield(Zmachposvariable, toolHeightBox);
AS3.Validatefield(toolHeightBox);

exec.Wait(100);
exec.Code("G43 H" + Currenttoolvariable); // Hämta ny tool offset

exec.Wait(200);

exec.Code ("G53 G00 Z" + ZRetractHeight); //Retract Z axis
while(exec.IsMoving()){}


exec.Wait(200);
exec.Code ("G90");

exec.Callbutton(518); //softlimits on

int CurrentTool = exec.Getcurrenttool(); //Hämta nuvarande verktyg 
string CurrentToolString = CurrentTool.ToString(); //Konvertera till string 
exec.Writekey("frasbord", "CurrentTool", CurrentToolString); //Skriv till profil

exec.Wait(100);
exec.Callbutton(167); //Save