﻿//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 = 1000; //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;

int Currenttoolvariable = exec.Getcurrenttool();
double Zmachposvariable = 0;
double XToolSetPos = 3170.9;
double YToolSetPos = 1561;
double ZSafePos = -1;
double ZFastPos = -130;
double XsafePos = 3050;

int DammskyddUppPort = 1;
int DammskyddUppPin = 1;

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

while (true)
		{
		
			DialogResult dialogResult = MessageBox.Show("Är position rätt?", "",MessageBoxButtons.YesNoCancel);
						
			if(dialogResult == DialogResult.Yes)
			{
				
				break;//do something
			}
			
			if(dialogResult == DialogResult.No)
			{
				while(exec.IsMoving()){}
				exec.Code("M5"); // Stoppa för att pausa spånsug
				while(exec.IsMoving()){}
				exec.Stopspin(); 
				exec.Callbutton(518); //softlimits on
				return;
			}
			
			else if (dialogResult == DialogResult.Cancel)
			{
				while(exec.IsMoving()){}
				exec.Code("M5"); // Stoppa för att pausa spånsug
				while(exec.IsMoving()){}
				exec.Stopspin(); 
				exec.Callbutton(518); //softlimits on
				return;
			}
					
		}
		
exec.Callbutton(519); //softlimits off
while(exec.IsMoving()){}

exec.Clroutpin(DammskyddUppPort, DammskyddUppPin); // Kör upp dammskydd
while(exec.IsMoving()){}



exec.Code("G91 G31 Z" + Zmin + "F" + CoarseRate); // Probe Z quickly to get rough height
while(exec.IsMoving()){}
exec.Wait(200);

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

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

if(Currenttoolvariable == 1) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 196);
  AS3.Validatefield(196);
}
if(Currenttoolvariable == 2) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 197);
  AS3.Validatefield(197);
}
if(Currenttoolvariable == 3) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 198);
  AS3.Validatefield(198);
}
if(Currenttoolvariable == 4) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 199);
  AS3.Validatefield(199);
}
if(Currenttoolvariable == 5) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 200);
  AS3.Validatefield(200);
}
if(Currenttoolvariable == 6) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 201);
  AS3.Validatefield(201);
}
if(Currenttoolvariable == 7) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 202);
  AS3.Validatefield(202);
}
if(Currenttoolvariable == 8) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 203);
  AS3.Validatefield(203);
}
if(Currenttoolvariable == 9) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 204);
  AS3.Validatefield(204);
}
if(Currenttoolvariable == 10) // Write tool offset depending on current tool
{
  AS3.Setfield(Zmachposvariable, 205);
  AS3.Validatefield(205);
}
exec.Wait(100);
exec.Code("G43 H" + Currenttoolvariable); // Hämta ny tool offset

exec.Wait(200);

exec.Code ("G53 G0 Z0"); //Retract Z axis
while(exec.IsMoving()){}
exec.Wait(200);
exec.Code ("G90");

exec.Callbutton(518); //softlimits on

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