﻿//Example linear toolchanger code


bool debug = false;

bool DammskyddActivated = true;

int timeoutInputs = 0;
int timeOutTime = 100;
int timeOutTimeLong = 100;

int DammskyddUppPort = 1;
int DammskyddUppPin = 1;

int BlowChuckport = 1;
int BlowChuckpin = 17;

int Chuckopenport = 2; 
int Chuckopenpin = 16; 

int Chuckcloseport = 3; 
int Chuckclosepin = 3; 


double PosXfranVerktyg = 3233.60; 

double xSoftlimit = 2700;
double xSoftlimitAtc = 2700;

double[] ToolX = new double[11];
double[] ToolY = new double[11];

int DammSkyddLED = 83; //Port2 pin 15

double SafeZ = -1;
double ZoverTool = -170;
double Ztoolrelease = -264; 
double Ztoolpickup = -264; 
double ZspeedUp = 8000; 
double ZspeedDwn = 3000; 

int ToolReleaseLED = 6;
int ToolOpenLED = 7;

double Yspeed = 0;  //Från plugin

int toolsInMag = 8;//Antal verktyg i magasin
int numberOfTools = 96; //Antal verktyg som går att välja, inkl utanför verktygsmagasinet.

double spindleFeedRate = 0; //Ändra ej

double ZspeedDwn2 = 500; //Ej från plugin / Saktare fart sista biten ned till verktyget
double ZposBeforeDwn2 = 3; //Ej Från plugin / Position över Ztoolpickup för att byta till sakta
int SpindleOffLED = 10; //Ingår för spindle är av

try
{
	
		
	
		
}
catch (Exception ex)
{
	MessageBox.Show("Fel värde från profil" + ex);
	exec.Callbutton(130); //Stop
	return;
}

ToolX[0] = 0; // Tool0 X position
ToolY[0] = 0; // Tool0 Y position
ToolX[1] = 3309.0; // Tool 1 X position
ToolY[1] = 1452.32; // Tool 1 Y position
ToolX[2] = 3306.5; // Tool 2 X position
ToolY[2] = 1275.09; // Tool 2 Y position
ToolX[3] = 3306.5; // Tool 3 X position
ToolY[3] = 1096.35; // Tool 3 Y position
ToolX[4] = 3305.5; // Tool 4 X position
ToolY[4] = 916.65; // Tool 4 Y position
ToolX[5] = 3305.0; // Tool 5 X position
ToolY[5] = 739.35; // Tool 5 Y position
ToolX[6] = 3305.0; // Tool 6 X position
ToolY[6] = 559.61; // Tool 6 Y position
ToolX[7] = 3303.5; // Tool 7 X position
ToolY[7] = 380.91; // Tool 7 Y position
ToolX[8] = 3303.0; // Tool 8 X position
ToolY[8] = 205.20; // Tool 8 Y position

int Newtool = exec.Getnewtool();
	
int Currenttool = exec.Getcurrenttool();



if(Newtool == -1) // If new tool number is -1 means a missing T code, so we need to stop here...
{MessageBox.Show("Verktyg kan ej vara minus");
return; }

if(Newtool == 0) // Tool number is out of range, so we need to stop here...
{MessageBox.Show("Verktyg kan ej vara 0");
return;}

if(debug) exec.AddStatusmessage("newtool = " + Newtool + " currenttool = " + Currenttool);

if(Newtool >0 && Newtool <=numberOfTools ) // kollar så att verkygsnumret motsvarar antal hållare.
{


	if(Newtool == Currenttool) // Same tool was selected, get height and return.
	{exec.Code("G43 H" + Newtool); // Load new tool offset
	return; }

	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, do homing before executing a tool change!");
	  exec.Stop();
	  return;
	}

	if(Currenttool == 0) 
	{
	  MessageBox.Show("Nuvarande verktyg kan ej vara 0");
	  exec.Stop();
	  return;
	}

	while(exec.IsMoving()){}

	if (exec.Ismacrostopped())
	{
	  exec.AddStatusmessage("Tool change aborted!");
	  return;
	}

	exec.Callbutton(519); //softlimits off

	while(exec.IsMoving()){}

	double Xoriginalpos = exec.GetXmachpos();
	double Yoriginalpos = exec.GetYmachpos();


	// Stop spindle if running and Move Z up

		while(exec.IsMoving()){}
		exec.Code("M5"); // Stoppa för att pausa spånsug
		while(exec.IsMoving()){}
		exec.Stopspin(); 

	//Hämta nuvarande spindelhastighet
	
	try
	{
		spindleFeedRate = Convert.ToDouble(AS3.Getfielddouble(869));
		Console.WriteLine("spindleFeedRate = " + spindleFeedRate);
	}
	catch (Exception ex)
	{Console.WriteLine(ex.Message);}

	//exec.Code("G00 G53 Z"+ Ztoolrelease); // Move Z up
	while(exec.IsMoving()){}

	exec.Code("G00 G53 Z"+ SafeZ); // Move Z up

	while(exec.IsMoving()){}

	
	if(DammskyddActivated)
	{
		exec.Clroutpin(DammskyddUppPort, DammskyddUppPin); // Kör upp dammskydd
	}
/*	
	if(!exec.GetLED(10)) // //Spindel av från VFD port 1 pin 10
{
			while(exec.IsMoving()){}
			exec.Code("M5"); // Stoppa för att pausa spånsug
			while(exec.IsMoving()){}
			exec.Stopspin(); 
			exec.Code("S" + spindleFeedRate);	
			exec.Callbutton(518); //softlimits on

			exec.Callbutton(512); //Estoppa 
			MessageBox.Show("Spindel ej stoppad, avbryter verktygsbyte, kontrollera verktygsnummer");
  return;
}
*/

	if(Currenttool >toolsInMag)
	{
		exec.Code("G00 G53 X0 " + "y1000"); // Kör X så att man kan nå spindeln.
		MessageBox.Show("Monterat verktyg nr."+ Currenttool +" finns ej i dom fasta verktygsplatserna, avlägsna verktyg och tryck OK");
		while(exec.IsMoving()){}
		exec.Code("M5"); // Stoppa för att pausa spånsug
		while(exec.IsMoving()){}
		exec.Stopspin(); 

		timeoutInputs = 0;

		/* while(!exec.GetLED(ToolOpenLED)) //Verktyg släppt, givare spindel
		{
		  exec.Wait(100); // Wait one 100msec
		  timeoutInputs = timeoutInputs + 1;
		  if(timeoutInputs > timeOutTime)
		  {
						while(exec.IsMoving()){}
				exec.Code("M5"); // Stoppa för att pausa spånsug
				while(exec.IsMoving()){}
				exec.Stopspin(); 
				exec.Code("S" + spindleFeedRate);	
				//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
				exec.Callbutton(518); //softlimits on

				exec.Callbutton(512); //Estoppa 
				MessageBox.Show("Verktyg ej släppt");
				
				return;
		  }
		  
		} */
		exec.Wait(100); // Wait one 1000msec
		exec.Clroutpin(Chuckopenport, Chuckopenpin); // Close the chuck with pneumatic valve

		
	}
	else
	{
		
		if(DammskyddActivated)
		{
			while(!exec.GetLED(DammSkyddLED)) //Vänta på dammskydd uppe
			{
			  exec.Wait(300); // Wait one 100msec
			  timeoutInputs = timeoutInputs + 1;
			  if(timeoutInputs > timeOutTime)
			  {
					while(exec.IsMoving()){}
					exec.Code("M5"); // Stoppa för att pausa spånsug
					while(exec.IsMoving()){}
					exec.Stopspin(); 
					exec.Code("S" + spindleFeedRate);	
					//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
					exec.Callbutton(518); //softlimits on

					exec.Callbutton(512); //Estoppa 
					MessageBox.Show("Dammskydd ej uppe");
					
					return;
			  }
			  
			}
		}

		exec.Callbutton(109); //HomeZ
		while(exec.IsMoving()){}
		
		exec.Code("G00 G53 Y" + ToolY[Currenttool] + " X" + PosXfranVerktyg);

		while(exec.IsMoving()){}

			exec.Code("G00 G53 Z"+ Ztoolrelease + " F" + ZspeedDwn); // Move Z axis down to tool holder position
			
		while(exec.IsMoving()){}
			
			exec.Code("G01 G53 X"+ ToolX[Currenttool] + " F" + "2000"); // Move X axis to tool holder position
		

		while(exec.IsMoving()){}
		if (exec.Ismacrostopped())
		{
			while(exec.IsMoving()){}
			exec.Code("M5"); // Stoppa för att pausa spånsug
			while(exec.IsMoving()){}
			exec.Stopspin();  
			exec.Code("S" + spindleFeedRate);	
			//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
			exec.Callbutton(518); //softlimits on
			exec.Callbutton(512); //Estoppa 
		  exec.AddStatusmessage("Tool change aborted!");
		  return;
		}
		
		exec.Clroutpin(Chuckcloseport, Chuckclosepin); // Open the chuck with pneumatic valve	
		while(exec.IsMoving()){}
		exec.Wait(300); // Wait one 1000msec
		exec.Setoutpin(Chuckopenport, Chuckopenpin); // Open the chuck with pneumatic valve
		while(exec.IsMoving()){}
		exec.Wait(300); // Wait one 1000msec
		exec.Setoutpin(BlowChuckport, BlowChuckpin); // Blow the chuck with pneumatic valve
		while(exec.IsMoving()){}
		exec.Wait(100); // Wait one 1000msec
		while(exec.IsMoving()){}
		exec.Code("M5"); // Stoppa för att pausa spånsug
		while(exec.IsMoving()){}
		exec.Stopspin();  

		timeoutInputs = 0;

		while(!exec.GetLED(ToolOpenLED) && exec.GetLED(ToolReleaseLED)) //Verktyg släppt, givare spindel
		{
		  exec.Wait(100); // Wait one 100msec
		  timeoutInputs = timeoutInputs + 1;
		  if(timeoutInputs > timeOutTime)
		  {
				while(exec.IsMoving()){}
				exec.Code("M5"); // Stoppa för att pausa spånsug
				while(exec.IsMoving()){}
				exec.Stopspin(); 
				exec.Code("S" + spindleFeedRate);	
				//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
				exec.Callbutton(518); //softlimits on

				exec.Callbutton(512); //Estoppa 
				MessageBox.Show("Verktyg ej släppt");
				
				return;
		  }
		  
		}

		exec.Code("G00 G53 Z"+ ZoverTool + " F" + ZspeedUp); // Move Z up


//		exec.Wait(100); // Wait one 100msec
		while(exec.IsMoving()){}
		exec.Clroutpin(Chuckopenport, Chuckopenpin); // Close the chuck with pneumatic valve		
		exec.Clroutpin(BlowChuckport, BlowChuckpin); // Stop blow the chuck with pneumatic valve
		exec.Setoutpin(Chuckcloseport, Chuckclosepin); // Open the chuck with pneumatic valve
		

		while(exec.IsMoving()){}

	}
	
	if(Newtool <= toolsInMag)
	{
	
		
		while(exec.IsMoving()){}
		
		
		exec.Code("G00 G53 X" + ToolX[Newtool] + " Y" + ToolY[Newtool]);


		while(exec.IsMoving()){}

	    //--------------------------------------------------------------------

		// Pick new tool

		
		exec.Clroutpin(Chuckcloseport, Chuckclosepin);
		exec.Setoutpin(BlowChuckport, BlowChuckpin); // Blow the chuck with pneumatic valve
		exec.Setoutpin(Chuckopenport, Chuckopenpin); // Open the chuck with pneumatic valve
		exec.Wait(50); // Wait one 100msec
		while(exec.IsMoving()){}
		exec.Code("G00 G53 Z-250" + " F" + ZspeedDwn); // Move Z axis down to tool holder position fast			
		//exec.Setoutpin(Chuckopenport, Chuckopenpin); // Open the chuck with pneumatic valve	
		
	//	while(exec.IsMoving()){}		
	//	exec.Clroutpin(Chuckcloseport, Chuckclosepin);
	//	while(exec.IsMoving()){}
	//	exec.Setoutpin(Chuckopenport, Chuckopenpin); // Open the chuck with pneumatic valve
		
		exec.Code("G01 G53 Z"+ Ztoolpickup + " F" + ZspeedDwn2); // Move Z axis down to tool holder position slow
		while(exec.IsMoving()){}
		exec.Clroutpin(Chuckopenport, Chuckopenpin); // Close the chuck with pneumatic valve
		exec.Clroutpin(BlowChuckport, BlowChuckpin); // Stop blow the chuck with pneumatic valve
		exec.Setoutpin(Chuckcloseport, Chuckclosepin);
		while(exec.IsMoving()){}
		timeoutInputs = 0;
		exec.Wait(100); // Wait one 1000msec

		while(!exec.GetLED(ToolReleaseLED)) //Chuckgivare
		{
		  exec.Wait(200); // Wait one 100msec
		  timeoutInputs = timeoutInputs + 1;
		  if(timeoutInputs > timeOutTime)
		  {
			  
				DialogResult dialogResult = MessageBox.Show("Verktyg ej gript, vill du fortsätta ändå?", "Fel, givare grip spindel", MessageBoxButtons.YesNo);
				if(dialogResult == DialogResult.Yes)
				{
					break;//do something
				}
				else if (dialogResult == DialogResult.No)
				{
					while(exec.IsMoving()){}
					exec.Code("M5"); // Stoppa för att pausa spånsug
					while(exec.IsMoving()){}
					exec.Stopspin(); 
					exec.Code("S" + spindleFeedRate);
					//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
					exec.Callbutton(518); //softlimits on
					exec.Callbutton(512); //Estoppa 
					return;
				}
			}
		}


		while(exec.IsMoving()){}
		exec.Code("G00 G53 X" + PosXfranVerktyg + " F" + "1000"); 
		while(exec.IsMoving()){}

		exec.Code("G00 G53 Z"+ SafeZ); // Move Z up
		while(exec.IsMoving()){}

		exec.Code("G00 G53 X" + 3050); //Kör in innanför softlimits
		while(exec.IsMoving()){}
	}
	else
		
	{
		
		exec.Clroutpin(Chuckopenport, Chuckopenpin); // Close the chuck with pneumatic valve
		exec.Setoutpin(Chuckcloseport, Chuckclosepin);
		exec.Code("G00 G53 X0 " + "Y1000"); // Kör X så att man kan nå spindeln.
		
		
		while (true)
		{
			MessageBox.Show("Montera verktyg nr." + Newtool + " och tryck OK");
			
				
			DialogResult dialogResult = MessageBox.Show("Vill du mäta verktyget?", "",MessageBoxButtons.YesNoCancel);
						
			if(dialogResult == DialogResult.Yes)
			{
				exec.Code("M20301");
				while(exec.IsMoving()){}
				break;//do something
			}
			
			if(dialogResult == DialogResult.No)
			{
				break;//do something
			}
			
			else if (dialogResult == DialogResult.Cancel)
			{
				while(exec.IsMoving()){}
				exec.Code("M5"); // Stoppa för att pausa spånsug
				while(exec.IsMoving()){}
				exec.Stopspin(); 
				exec.Code("S" + spindleFeedRate);
				//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
				exec.Callbutton(518); //softlimits on
				exec.Callbutton(512); //Estoppa 
				return;
			}
					
		}
		while(!exec.GetLED(ToolReleaseLED)) //Chuckgivare
		{
		  exec.Wait(200); // Wait one 100msec
		  timeoutInputs = timeoutInputs + 1;
		  if(timeoutInputs > timeOutTime)
		  {
			  
				DialogResult dialogResult = MessageBox.Show("Verktyg ej gript, 	 fortsätta ändå?", "Fel, givare grip spindel", MessageBoxButtons.YesNo);
				if(dialogResult == DialogResult.Yes)
				{
					break;//do something
				}
				else if (dialogResult == DialogResult.No)
				{
					while(exec.IsMoving()){}
					exec.Code("M5"); // Stoppa för att pausa spånsug
					while(exec.IsMoving()){}
					exec.Stopspin(); 
					exec.Code("S" + spindleFeedRate);
					//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
					exec.Callbutton(518); //softlimits on
					exec.Callbutton(512); //Estoppa 
					return;
				}
			}
		}

	}

	while(exec.IsMoving()){}

	exec.Code("G00 G53 X" + Xoriginalpos + " Y" + Yoriginalpos); //Kör till position innan verktygsbyte
	while(exec.IsMoving()){}


	}

exec.Wait(100);
while(exec.IsMoving()){}
if(!exec.Ismacrostopped()) // If tool change was not interrupted with a stop only then validate new tool number
{
		//exec.Stopspin(); 
		//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
		exec.Setcurrenttool(Newtool); //Set the current tool -> the new tool
		exec.Code("S" + spindleFeedRate);
		
		int CurrentTool = exec.Getcurrenttool(); //Hämta nuvarande verktyg
		exec.Code("G43 H" + Newtool); // Load new tool offset
		string CurrentToolString = CurrentTool.ToString(); //Konvertera till string
		exec.Writekey("JB CNC", "CurrentTool", CurrentToolString); //Skriv till profil
		exec.Callbutton(518); //softlimits on
		while(exec.IsMoving());
		if(DammskyddActivated) //Kör ned dammskydd om aktiverat
		{	
			exec.Setoutpin(DammskyddUppPort, DammskyddUppPin); // Kör ned dammskydd
		}
		
		//MessageBox.Show("Tool change done.");
		
	
}
else
{
	while(exec.IsMoving()){}
	exec.Code("M5"); // Stoppa för att pausa spånsug
	while(exec.IsMoving()){}
	exec.Stopspin(); 
	//exec.Setoutpin(3, 6); //port pin} OutputPT3PN6 återgå till vanlig spindelrörelse
	exec.StopWithDeccel();
	exec.Callbutton(518); //softlimits on
	exec.Code("S" + spindleFeedRate);
	MessageBox.Show("Verktygsbytet avbröts, kontrollera att rätt verktygsnummer sitter i spindeln!");

  
  return;
}


