//M31 probing macro

double probeX = 200;
double probeY = 300;
double Zmin = -50;
double FeedrateFast = 1000;
double FeedrateSlow = 1000;
double SafeZ = 100;
double retractheight = 3.8;
double retractforsecondmeasurement = 1;
double offsetGivareZ = -1;
bool enableProbe = true; //Aktiverar tex ohmic sensor
int enableProbePort = 3;
int enableProbePin = 8;

int probe2LED = 99; //Pin 14 port 3 (Varje utgång och ingång motsvaras av en "LED" se dokumentation)
bool enableProbe2 = true;
double offsetGivare2Z = -5;

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

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

if(enableProbe) {exec.Setoutpin(enableProbePort, enableProbePin);}



while(exec.IsMoving()){}

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.Code("G91 G0 Z" + retractforsecondmeasurement);
  exec.Code("G90"); 
}

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

if(!exec.GetLED(244))
{
	MessageBox.Show("Probe ej aktiverad, probningen misslyckades");
	exec.Stop();
	return;
}



if(!exec.Ismacrostopped()) // If tool change was not interrupted with a stop only then validate new tool number
{
	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;
	}

	if(!exec.GetLED(probe2LED) || !enableProbe2) // Om ej probe2 är påverkad eller funktion ej aktiverad
	{
	exec.mainform.sumoffsetcontrol1.G54.newCzinput(offsetGivareZ);  // Set G54 to new Zzero
	exec.mainform.sumoffsetcontrol1.G55.newCzinput(offsetGivareZ);  // Set G55 to new Zzero	
	exec.mainform.sumoffsetcontrol1.G56.newCzinput(offsetGivareZ);  // Set G56 to new Zzero
	exec.mainform.sumoffsetcontrol1.G57.newCzinput(offsetGivareZ);  // Set G57 to new Zzero
	exec.mainform.sumoffsetcontrol1.G58.newCzinput(offsetGivareZ);  // Set G58 to new Zzero
	exec.mainform.sumoffsetcontrol1.G59.newCzinput(offsetGivareZ);  // Set G59 to new Zzero
	exec.Wait(400);
	}
	else
	{
	exec.mainform.sumoffsetcontrol1.G54.newCzinput(offsetGivare2Z);  // Set G54 to new Zzero
	exec.mainform.sumoffsetcontrol1.G55.newCzinput(offsetGivare2Z);  // Set G55 to new Zzero	
	exec.mainform.sumoffsetcontrol1.G56.newCzinput(offsetGivare2Z);  // Set G56 to new Zzero
	exec.mainform.sumoffsetcontrol1.G57.newCzinput(offsetGivare2Z);  // Set G57 to new Zzero
	exec.mainform.sumoffsetcontrol1.G58.newCzinput(offsetGivare2Z);  // Set G58 to new Zzero
	exec.mainform.sumoffsetcontrol1.G59.newCzinput(offsetGivare2Z);  // Set G59 to new Zzero
	exec.Wait(400);
	}


	exec.Code("G00 G53 Z" + Zup); // Move 10mm above probe plate

	if(enableProbe) {exec.Clroutpin(enableProbePort, enableProbePin);}

	while(exec.IsMoving()){}

	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()){}
	}

}