//M31 probing macro

double probeX = 200;
double probeY = 300;
double Zmin = -100;
double FeedrateFast = 300;
double FeedrateSlow = 100;
double SafeZ = -5;
double retractheight = 10;
double retractforsecondmeasurement = 1;
bool MPG = false;

bool domoveXY = false; //Enable XY movement
bool dodualcycle = true; //Do probing from 2 cycles, first with Fast and second with Slow feedrates


double ProbeHeight = 32.47;


if(!exec.GetLED(56)||!exec.GetLED(57)||!exec.GetLED(58)) // If machine was not homed then it is unsafe to move in machine coordinates, stop here...
{
  MessageBox.Show("The machine was not yet homed, home the machine before run to parking position!");
  exec.Stop();
  return;
}


while(exec.IsMoving()){}
exec.Wait(200);


//wenn MPG aktiv auf Continous umschalten, Rückschaltung am Ende
if(!exec.GetLED(145))
{
	exec.Callbutton(161);
	MPG = true;
}



double Xoriginalpos = exec.GetXmachpos(); // Get the current machine coordinates
double Yoriginalpos = exec.GetYmachpos(); // Get the current machine coordinates

if(domoveXY) // Make XY movement only if enabled
{
  exec.Code("G00 G53 Z" + SafeZ); // Move Z up first
  while(exec.IsMoving()){}

  exec.Code("G00 G53 X" + probeX +" Y" + probeY); // Move to the probe sensor position in XY
  while(exec.IsMoving()){}
}


if(dodualcycle)
{
  exec.Code("G31 Z" + Zmin + "F" + FeedrateFast); // Do the Z probing with Fast feedrate first
  while(exec.IsMoving()){}
  exec.Wait(200);
  exec.Code("G91 G0 Z" + retractforsecondmeasurement);
  exec.Code("G90"); 
}

exec.Code("G31 Z" + Zmin + "F" + FeedrateSlow); // Do the Z probing again with Slow Feedrate to get a more accurate reading
while(exec.IsMoving()){}
exec.Wait(200);




if(!exec.Ismacrostopped()) // If tool change was not interrupted with a stop only then validate new tool number
{

//*****raus, referenziert zur Oberkante Portal*****
// exec.Code("G44 H1"); // Load tool offset one, note the tool lenght is defined in the tools menu
// while(exec.IsMoving()){}
// exec.Wait(200);

 double Zup = exec.GetZmachpos() + retractheight;

 if(Zup > SafeZ)
 {
   Zup = SafeZ;
 }
 
 //***neu***
 exec.ChangeaxisDROvalue(2, ProbeHeight.ToString()); // Change the Z DRO value to the probe height
 exec.Wait(200);                                     // Wait for syncronisation
 //*****


 exec.Code("G00 G53 Z" + Zup); // Move 10mm above probe plate
 while(exec.IsMoving()){}
 exec.Wait(200);

if(domoveXY) // Make XY movement back to start position only if XY movement is enabled
{
  exec.Code("G00 G53 X" + Xoriginalpos +" Y" + Yoriginalpos); // Move back to the original XY position
  while(exec.IsMoving()){}
}

}

//falls MPG aktiv war wieder aktivieren
if(MPG)
{
	exec.Callbutton(226);
}