component toolchanger "Control lathe tool turret";


pin in bit clamped		" signal indicates if turret is clamped";
pin in bit ref			"signal true when tool 1 in position";
pin in bit betw_stations	"signal true when turret is between stations";
pin in bit indx			" used for counter - true for every second tool";
pin in bit toolchange		"initiates toolchange";
pin in signed toolnumber	"too lumber requested from M6";
pin in bit ishomedX = false    	"Status of X axis homing";
pin in bit ishomedZ = false   	"Status of Z axis homing";
pin in bit tool_prepare = false " signal to prepare tool ";
pin in signed tool_prep_num	" tool number to prepare ";
pin in bit button =false	"button to rotate turret manually";
pin in bit machine_is_manual=false  " signal true when machine is in manual mode";


pin out bit toolchanged	= false	" signal to confirm tool changed";
pin out bit declamp=false	" signal to declamp turret" ;
pin out bit fwd=false		" signal to rotate turet fwd";
pin out bit rev=false		" signal to torate turret rev";
pin out bit tool_prepared=false		" signal to confirm tool prepared";
pin out signed current_tool		" indicates current tool";
pin out signed last-tool;
// Internal 
pin out signed progress_level = 0;         // progress of toolchange
pin out bit error;			// true if current_tool != toolnumber


// internal variables
// variable int current_tool=0;	//number of current tool
variable int next_tool=0;	//number of next tool
variable bool warn = false;            // first toolnumber reminder
variable int station_difference=0;	// difference between requested tool and current_tool
variable bool last_state=false;		// state change -  to count tools
variable int last_req_tool_num=0;	//last req tool number

option userspace yes ;
option singleton yes ;               // makes no sense to have more than one of these components running - only one ATC


author "henk du preez - 2016 hdpreez2@gmail.com";

license "GPL";
;;
#include <unistd.h> 

/* *********************************************************** */
void user_mainloop(void) 
{
 while (1) 
 {
  FOR_ALL_INSTS()
  { 



last_tool=last_req_tool_num;

if(!ishomedX && !ishomedZ)
	{ progress_level=0;}


if (tool_prep_num != 0)

	{	
	last_req_tool_num=tool_prep_num;
	
	}
	

	switch (progress_level)
	
	{     
     	
	case 0: //declamp turret and rotate to find ref signal - set current_tool to 1.
		if(ishomedX && ishomedZ)
			{
        	  
        	   	declamp=true;
			
			if (clamped==false)
				{
				fwd=true;
				if (ref==true)	
					{	
					fwd=false;
					declamp=false;	
					current_tool=1;
					
					progress_level=5;
					
					}
				}


			
		
        	   	}



		break;

	case 1: // button
		if (button==true)
		{
			
			declamp=true;
		
			if (clamped == false)
				{
				fwd=true;
				
				
				}
		}
		else
		{
			if (betw_stations==false)
				{		

				fwd=false;
				declamp=false;
				progress_level=5;

				}
			else progress_level=1;
		}
		
		if (betw_stations != last_state)
			{
			if (betw_stations==false)
				{
				current_tool++;
				if (current_tool==9)
					{					
					current_tool=1;
					}

			if (toolnumber != current_tool)
					error = true;
					else error = false;

					

					
				}
			}
		last_state=betw_stations;
		
		
		
		
		
		break;
					
	case 2: // decide which direction to rotate wnen tool-change is requested	
		station_difference=last_req_tool_num - current_tool;
		//last_req_tool_num=tool_prep_num;
		tool_prepared=true;
		if (toolchange==true)
		{
		if (station_difference == 0 && current_tool !=0)
			{
			//tool_prepared=true;
			toolchanged=true;
			}
		else if (station_difference < 0 && station_difference >= -3 ) //rotate rev to next tool
			{ progress_level=3; }
		
		else if (station_difference > 0 && station_difference <= 4) //rotate fwd to next tool
			{ progress_level=4; }

		else if (station_difference > 4 ) //rotate rev to next tool
			{ progress_level=3; }

		else if (station_difference <= -4 ) //rotate fwd to next tool
			{ progress_level=4; }
		}

		break;

	case 3: // rotate rev to next tool

		

		declamp=true;
		if (clamped == false)
			{
			rev=true;			
			}
		
		if (betw_stations != last_state)
			{
			if (betw_stations==false)
				{
				current_tool--;
				if (current_tool==0)
					{					
					current_tool=8;
					}
				}
			}

		last_state=betw_stations;

		if (current_tool==last_req_tool_num)
			{ 
			rev=false;
			declamp=false;
			if (clamped==true)
				{
				
				toolchanged==true;
				progress_level=5;
				}
			
			}
		
		break;


	case 4: // rotate fwd to next tool

		
		
		declamp=true;
		if (clamped == false)
			{
			fwd=true;			
			}
		
		if (betw_stations != last_state)
			{
			if (betw_stations==false)
				{
				current_tool++;
				if (current_tool==9)
					{					
					current_tool=1;
					}
				}
			}

		last_state=betw_stations;

		if (current_tool==last_req_tool_num)
			{ 
			fwd=false;
			declamp=false;
			if (clamped==true)
				{
				
				toolchanged==true;
				progress_level=5;
				}
			
			}
		
		
		break;

	case 5: //do nothing - wait for tool change request
			

		if (machine_is_manual && ishomedX && ishomedZ && button)

			{progress_level=1;}


		if (!machine_is_manual && ishomedX && ishomedZ && tool_prepare)

			{progress_level=2;}
		
		break;


	default :
	
	progress_level =5;
	}
  }
 }
}
/* ******************************************************************* */