component cinturnchanger                        """This component controls the Cincinnati Cinturn  12u Lathe 6 position 12 tool turret. M6 calls this""";



// this code is released into the public commons by me 1-2-2021, I give no warranty or fuck of any kind, if you die, you die. 
// use at your own risk. Machine tools are big and dangerous, a lathe will rip your limbs of like a angry orangatang being 
// mauled by a tiger, that is being eaten alive by a polar bear. These things are fucking dangerous!!!! This code might not be safe !!!!!!
// take care, use that rock between your eyes and for fucks sakes get a helemt, Ron White will tell you when to put it on
// Have a nice day !!!!



// this pin out OneShotStarts are a of flags set to TRUE to allow logic to canceled and remove the dependency chain from the tool logic, 
// instead of a never ending loop this canels and allows subsequent functions to be cancelled we set these true and move them false to cahncel functions. 

pin out bit OneShotStart = true				" single shot start is reset every pass at the end of the tool change logic";
pin out bit toolchanger	= true					"used as single shot and tracking variable";
pin out bit ErrorTest = true					"used once at startup to check BCD switch rationality and decoding rationality";
pin out bit toolchanging = false			"used to inhibit the tool change shortcut for shared tools on same pocket  or when the pockets agree before the turret is locked etc set true when tool change starts"; 
                                                  
// the meat and potatoes, get a toolchange request and a toolnumber and get to work
// sent from IOCONTROL.v2.c if your curious about it, comes from HAL pipeline in EMC subsystems 

pin in bit toolchange = false                          "Receives signal that tool change required";
pin in s32 toolnumber                           """Receives Tx data (tool number requested)  Only allows 1-12""";
 
 
 
// IO from EMC hal subsystem fairly self explanatory 
                                                        
pin in s32 currenttoolnumber                    "Receives old tool number";
pin out bit toolchanged = false                 "Sends signal when tool change finished";




// these are the input position switches that have a binary coding from the inside of the turret
// there are 3 switchs on a cam with 3 lobes, each lobe has a uniquie shape and number of on offs relative
// to each turret station the decoding is done below in the bcd table

pin in bit bcd1 = false      			"State of bcd switch 1";
pin in bit bcd2 = false 			    "State of bcd switch 2";
pin in bit bcd3 = false 			    "State of bcd switch 3";
pin in bit bcd4 = false 				"State of bcd switch 4";   // not currently used



// there is a rod attached to the turret clamp and when the turret is unclamped & pushed away, the rod actuates these switches
// they are normally open  , if you are here you should grasp how this works

pin in bit turretunlocked  = false    			"State of turret unlocked switch";
pin in bit turretlocked    = false    		    "State of turret locked switch";



// the cincinatti has a somewhat interesting design, there is a strobe piston and a strobe swtich, the piston get's pressureized hydrualic
// oil when the motor is off, it has a shuttle valve, when the motor is on the shuttle valve direct hydrualic pressure to the motor and away 
// from the piston. this allows the motor to rotate, when the motor is OFF the strobe piston ALIGNS the turret with the lock mechanism by pushing
// on a 6 lobe cam to position is accurately over the lock adn alignment pins ensuring repeatable accurate tool position
													
pin in bit strobe = false            			"State of strobe switch";



// lathe turret motor solenoid

pin out bit motor = false               		"turret motor solenoid";
				
pin in float MinMotorOn = 0.300					"settable hal pin to control motor on period";
pin in float MinMotorOff = 0.750					"settable hal pin to set motor off period";

param rw bit MotorFlag = false 					"method to control the turret motor on time";
param rw bit MotorState = false   				"flag to control the motor timer";
param rw float MotorTimeOff = 0					"timer to control motor time off";
param rw float MotorTimeOn = 0					"timer to control motor on time";						


// unclamp turret solenoid

pin out bit unlock = false              		"turret unlock solenoid";



// IO for multiple uses to control EMC IO stack

pin in bit reset = false					    " used to reset error states of set_warn_flag the entire purpiose of which is the shut the error messages up we need them but not at servo thread speed";
pin out bit estop = false                       " in the event any number of errors is true, estop will be set  to disable machine";



// watch and control program flow path good debugging tool 

param rw s32 counterT = 0			                "initialize counter for flow control and logic stepping"; 			   	
  
  

// watching tool change decoding logic for BCD table and Tool translation
  
param rw s32  pocket   = 0						"create pocket and use for debugging of truth table";
param rw s32  pocket_number = 0                 "create requested pocket number to track for errors"; 
param rw s32  tool_number = 0                   "create initial tool number for tool_translation table";
param rw s32  pocket_number_out = 0             "create requested pocket number to track for errors";  


// filtered  pocket output from bcd switch table


param rw s32 pocketfiltered = 0					 " filtered pocket number";

// these are the timer variable store, visible in hal

param rw float	timer0 = 0.0					" variable for timer control";
param rw float 	timer1 = 0.0					" variable for timer control";
param rw float 	timer2 = 0.0					" variable for timer control";
param rw float 	timer3 = 0.0					" variable for timer control";
param rw float 	timer4 = 0.0					" variable for timer control";
param rw float  timer5 = 0.0					" debounce bcd inputs prior to error checking";
param rw float  timer6 = 0.0					" debounce toolnumber from iocontrol";
param rw float  timer7 = 0.0					" check if turret is locked delay";
param rw float  timer8 = 0.0					" debounce pocket number to stop noisy output";
param rw float  timer9 = 0.0					" pause to check for error at machine start up";

// filter bcd switches

param rw float BCD1T = 0						" bcd switched debounce";
param rw float BCD2T = 0						" bcd switched debounce";
param rw float BCD3T = 0						" bcd switched debounce";

param rw bit bcd1state = false					"bcd switch state";
param rw bit bcd2state = false					"bcd switch state";
param rw bit bcd3state = false					"bcd switch state";

param rw bit bcd1F = false;						// filtered bcd switch output
param rw bit bcd2F = false;
param rw bit bcd3F = false;


// filter strobe input

param rw float strobeT = 0;						 // same filtering structure as the BCD switches
param rw bit strobestate = false;
param rw bit strobeF = false;

// delay time variables adjustable in hal 

pin in float unlockpause = 1.000						"these are time variables for adjusting operational delays";
pin in float unlocktimeout = 10.000                  "timeout setting for turret  unlock watchdog in seconds";
pin in float motortimeout = 30.000					"timeout setting for turret in position from motor run time in seconds";
pin in float turretsettletime = 2.000				"pause timer after motor off to let turret settle";
pin in float debounceswitches = 0.100				"debounce time after turret lock to allow switches to settle";
pin in float debounceBCD = 0.001					"debounce the BCD swtiches";
pin in float debouncetoolnumber = 0.001				"debounce the pocket number ";
pin in float IsTurretLockedDelay = 2.000				"delay value before checking if turret is locked turret can be slow to lock sometimes";
pin in float PocketFilterTime = 0.001					"debounce and delay pocket value to prevent false turn offs";
pin in float DiagWaitTime  = 0.100					"superflous error messages are annoying at startup so we wait before checking the bcd states to give adequate debounce time";
pin in float debouncestrobe = 0.001					" filtering for strober input";

// Diagnostic error flags also used to supress annoying messages, nd turn the estop etc etc. 

pin out bit set_warn_flag1  = false				"sets warning to tell operator to home the turret with a call for tool 1";
pin out bit set_warn_flag2  = false				"sets warning check condition of BCD inputs to ensure they work properly and decode to a valid number";
pin out bit set_warn_flag3  = false				"sets warning tells operator to pick the proper pocket number";
pin out bit set_warn_flag4  = false				"sets warning tell operator pocket number is invalid";
pin out bit set_warn_flag5  = false				"bcd switch open";
pin out bit set_warn_flag6  = false				"sets warning turret motor ran to long ";
pin out bit set_warn_flag7  = false				"sets warning for turret failed to aling correctly";
pin out bit set_warn_flag8  = false				"disable turret, fualts present";
pin out bit set_warn_flag9  = false				"warn user to home turret with tool change";
pin out bit set_warn_flag10 = false				"turret failed to unlock";
pin out bit set_warn_flag11 = false				"bcd switch short";
pin out bit set_warn_flag12 = false				"turret took to long to unlock";
pin out bit set_warn_flag13 = false 			" turret failed to align properly !!!! FATAL error";

// we use this for flow control and execution monitoring. I had a simpler scheme, but 
// that shit didn't work out

pin out bit count1 = false						"we use these counts to flow control manage the code due to the forced execution nature of RTAPI design";
pin out bit count2 = false						"flow control I suppose dyou could pipe these to the GUI and use them to track toolchanger progress ";
pin out bit count2_5 = false					"meh, needed a half step";
pin out bit count3 = false						"or you could use them to drive a pin on hardware and put cool little blinket lights on a display ";												
pin out bit count3_5 = false					"meh, needed a half step";
pin out bit count4 = false						"you could also maybe convert this set of bits into a bit array amd use them to drive a led strip";		
pin out bit count4_5 = false;					// pause timer for turret settling
pin out bit count4_55 = false;					// second step for turret settling
pin out bit count5 = false						"and have it change colors";								
pin out bit count6 = false						"conversely you could also use these to out a message on screen to show the progress";
pin out bit count7 = false						"there are a lot of things you could use these bits for, be creative";
pin out bit count8 = false						"and here we are finished";
pin out bit count9 = false						"here we cleanup toolchanger program state for next tool change";
pin out bit count10 = false						"";
pin out bit count11 = false						"";
pin out bit count12 = false						" enable for turret locked in position check";
pin out bit count13 = false						"";
pin out bit count14 = false						"";
pin out bit count15 = false						"";
pin out bit count100 = false				    "error state not good things went wrong !!!!";


//control flags for variable filtering 			

pin out bit PocketFilterFlag = false			" control flag for pocket value filtering";

//set instance maximums

option singleton yes;  							// change to no for multi turret machine, configured for single turret machine originally



function _;
author "Sean Collins smc.collins1977@gmail.net";
license "MIT";
;;



// some required headers and maybe just some useless headers, who knows

#include <rtapi_math.h>
#include <rtapi.h>
#include <stdio.h>
#include <unistd.h>
#include <time.h>  
#include <rtapi_app.h>
      

// lets start by annoying the operator of the machine with a message
// operators love that
												


// send message to notify user to set tool 1
		
if (set_warn_flag1  == false) {							// this is a flood control measure only need to tell the operator 1 time !
	rtapi_print_msg(RTAPI_MSG_ERR, "Set Turret tool 1  with M06 T01 command in MDI & HOME THE TURRET !!!");
	set_warn_flag1 = true;								// turn the flood control measure on I use the same strategy across the engine program in many places
}												


//strobe filtering for switch debounce


if (strobe == true) {
    if (strobestate == false) {
        strobeT = 0;
    }
    strobestate = true;
} 

 if (strobe == false) {
    if (strobestate == true) {
        strobeT = 0;
    }
    strobestate = false;
}

if (strobestate != strobe) {
    strobeT += fperiod;
    } else if (strobestate == strobe) {
        strobeT += fperiod;
    }
	
if (strobeT > debouncestrobe && strobe == true) {
    strobeF = true;
} else if (strobeT > debouncestrobe && strobe == false) {
    strobeF = false;
}
//BCD filtering for switch debounce



if (bcd1 == true) {
    if (bcd1state == false) {
        BCD1T = 0;
    }
    bcd1state = true;
} 

 if (bcd1 == false) {
    if (bcd1state == true) {
        BCD1T = 0;
    }
    bcd1state = false;
}

if (bcd1state != bcd1) {
    BCD1T += fperiod;
    } else if (bcd1state == bcd1) {
        BCD1T += fperiod;
    }
	

if (BCD1T > debounceBCD && bcd1 == true) {
    bcd1F = true;
} else if (BCD1T > debounceBCD && bcd1 == false) {
    bcd1F = false;
}



if (bcd2 == true) {
    if (bcd2state == false) {
        BCD2T = 0;
    }
    bcd2state = true;
} 

if (bcd2 == false) {
    if (bcd2state == true) {
        BCD2T = 0;
    }
    bcd2state = false;
}

if (bcd2state != bcd2) {
    BCD2T += fperiod;
} else if (bcd2state == bcd2) {
    BCD2T += fperiod;
}

if (BCD2T > debounceBCD && bcd2 == true) {
    bcd2F = true;
} else if (BCD2T > debounceBCD && bcd2 == false) {
    bcd2F = false;
}


if (bcd3 == true) {
    if (bcd3state == false) {
        BCD3T = 0;
    }
    bcd3state = true;
} 

 if (bcd3 == false) {
    if (bcd3state == true) {
        BCD3T = 0;
    }
    bcd3state = false;
}

if (bcd3state != bcd3) {
    BCD3T += fperiod;
    } else if (bcd3state == bcd3) {
        BCD3T += fperiod;
    }
	

if (BCD3T > debounceBCD && bcd3 == true) {
    bcd3F = true;
} else if (BCD3T > debounceBCD && bcd3 == false) {
    bcd3F = false;
}


// BCD or Binary Coded Switch Decoding to determine turret position. 
// tried a bunch of methods, this just, it was the simplest most robust least
// obfuscated path to the victory. Every other method became more difficult to control for custom mappings
	    
if (motor == false){
	if (bcd1F && bcd2F && bcd3F) pocketfiltered = 0;
	else if (!bcd1F && bcd2F && bcd3F) pocketfiltered = 1;
	else if (bcd1F && !bcd2F && bcd3F) pocketfiltered = 2;
	else if (!bcd1F && !bcd2F && bcd3F) pocketfiltered = 3;
	else if (bcd1F && bcd2F && !bcd3F) pocketfiltered = 4;
	else if (!bcd1F && bcd2F && !bcd3F) pocketfiltered = 5;
	else if (bcd1F && !bcd2F && !bcd3F) pocketfiltered = 6;
	else if (!bcd1F && !bcd2F && !bcd3F) pocketfiltered = 7;
}









// this is a error check to make sure we don't have a bad switch or improper value in the bcd table 

pocket_number_out = pocketfiltered; // various transformations of the BCD table decoding used in multiple places
pocket = pocketfiltered; 

if (toolchange == true && count8 == true){
    ErrorTest = true;
    timer5 = 0;
}


if (ErrorTest == true){
	timer9 += fperiod;
}

if (ErrorTest == false){
	timer9 = 0;
}

if (ErrorTest == true && timer9 >= DiagWaitTime){							// initialized as true, cleared with no errors found is reset after each sucessful tool change 
	timer5 += fperiod;	
if (pocket < 1 || pocket > 6 && timer5 > debounceBCD && toolchange == false) {						// check to make sure pocket value is within range of 1 to 6 modify to suit your needs 
	if (set_warn_flag2 == false) {				// you will see these repeatdely as hal loops through this file if we don't put this code here the error message will flood
	rtapi_print_msg(RTAPI_MSG_ERR, "Fualt BCD switches not decoded properly terminating tool changer ESTOP ON");
    set_warn_flag2 = true;						// set flag true to prevent error message flooding
    count100 = true;
    count15 = true;
    ErrorTest = false;                          // shut off timer, operator has been notified
	timer5 = 100;								// reset timer
    toolchanger = false;
		}
	}
else {
if (pocket >= 1 && pocket <= 6 && timer5 > debounceBCD && toolchange == false){				// if pocket is between 1-6 set toolchanger to true
    toolchanger = true;							// once the error above clears the toolchange will be allowed again
    count15 = false; 
    ErrorTest = false;                          // test ok kill timer
    timer5 = 0;									// reset timer
    set_warn_flag2 = false;
    
		}
	}
if (pocket == 0 && timer5 > debounceBCD && toolchanger == false) {						// check to make sure pocket value is within range of 1 to 6 modify to suit your needs 
	if (set_warn_flag5 == false) {							// you will see these repeatdely as hal loops through this file if we don't put this code here the error message will flood
	rtapi_print_msg(RTAPI_MSG_ERR, "LESS THAN ONE BCD SWITCHES ON OPEN CIRCUIT");
    set_warn_flag5 = true;							// set flag true to prevent error message flooding
    count100 = true;
    ErrorTest = false;                             // kill timer operator was notified 
    timer5 = 200;									  // leave a clue for the error condition
    toolchanger = false;
    estop = true;
		}
	}
else{
if (pocket == 7 && timer5 > debounceBCD && toolchanger == false ) {						// check to make sure pocket value is within range of 1 to 6 modify to suit your needs 
	if (set_warn_flag11 == false) {							// you will see these repeatdely as hal loops through this file if we don't put this code here the error message will flood
	rtapi_print_msg(RTAPI_MSG_ERR, "3 OR MORE BCD SWITCHES ON CHECK FOR SHORT CIRCUIT");
    set_warn_flag11 = true;							// set flag true to prevent error message flooding
    count100 = true;
    toolchanger = false;
    ErrorTest = false;                             // kill timer operator was notified 
    timer5 = 300;								   // leave a clue for the error type
    estop = true;
			}
		}
	}
}

// expanding the tool options with a lookup table to translate pocket to tool
// turret lathes can often multi tool a station for example ID and OD tools can share a station
// turret lathes can also have multple ID and multiple OD tools on each station unlike a mill where 
// the spindle holds the tool a lathe holds the tool and the spinlde move the work piece. 
// this expanded capacity is a must have for a multi tool per station turret the cincinatti cinturn has 
// tooling options that allow for upto 6 tools per station on some models 4 per station here seemed like a 
// sane defualt
																

		
//  perform tool to pocket translation 

tool_number = toolnumber; 								// assign the incoming toolnumber to tool_number hal only sends toolnumber, but we can't change it so we need to reassign it 

if (toolchange == true && toolnumber != currenttoolnumber){   // only execute when tool is actually different									
if (tool_number == 1 || tool_number == 7 || tool_number == 13 || tool_number == 19) {
	pocket_number = 1;
	} 
	if (tool_number == 2 || tool_number == 8 || tool_number == 14 || tool_number == 20) {
	pocket_number = 2;
	} 
	 if (tool_number == 3 || tool_number == 9 || tool_number == 15 || tool_number == 21) {
	pocket_number = 3;
	} 
	if (tool_number == 4 || tool_number == 10 || tool_number == 16 || tool_number == 22) {
	pocket_number = 4;
	} 
	if (tool_number == 5 || tool_number == 11 || tool_number == 17 || tool_number == 23) {
	pocket_number = 5;
	} 
	 if (tool_number == 6 || tool_number == 12 || tool_number == 18 || tool_number == 24) {
	pocket_number = 6;
}
}

 

// toolnumber sanity check is it within bounds 

if (toolchange == true && ErrorTest == true) {  // we disable this during the toolchange to prevent false errors being generated !!!
	{
	timer6 += fperiod;
	}
if (ErrorTest == true && timer6 > debouncetoolnumber && toolchanger == true){		 
if (tool_number < 1 || tool_number > 24) { 		// check for valid tool number less than 1 greater than 24
	if (set_warn_flag9 == false) {						// flood control cancel 1 message is enough per cycle 
	rtapi_print_msg(RTAPI_MSG_ERR, "SELECT TOOL NUMBER BETWEEN 1 && 24 TOOL NUMBER INVALID");
	set_warn_flag9 = true;						// tool is invalid set flag
	toolchanger = false;						// kill this variable to prevent tool change until a valid pocket is set
	ErrorTest = false;							// supress error test after fault it's bad enough we screwed up
	timer6 = 100;								// leave a clue as to why
		}	
	}
}
else {
if (ErrorTest == false && timer5 > debouncetoolnumber && tool_number >= 1 && tool_number <= 24) {	// if pocket at least 1-24 set toolchanger to true
    toolchanger = true;							// once the error above clears the toolchange will be allowed again
	ErrorTest = false;							// supress error checking becaase it is annoyingly fickle
	timer6 = 0; 								// reset timer	
		}	
	}		
}
		
// here we hopefully perform a toolchange, this toolchanger path has some protections to prevent operating is errors are present
// there is a clear error hal output than can be tired to a hal virtual input to clear these errors
// this is needed code to prevent the machine from operating the toolchanger if it has faults should 	
// has estop output for fualt conditions to prevent bad juju from happening to the machine
								


// begining of toolchange logic 
																


	
//toolchange request from hal starts the logic is basically 
//stair step logic to run turret. Roost and simple and highly deterministic
// there are fancier ways to write this code, feel free to do so 


//   check if the pockets are the same, do nothing, if they are different, perform toolchange. 

if (pocketfiltered == pocket_number && toolchange == true && toolchanging == false){			//	if the pockets are the same, send back toolchanged flag and disable the toolchange process, reduce uneeded cycling save time
    toolchanger = false;
    toolchanged = true;
	OneShotStart = false;
}else if (pocketfiltered != pocket_number && toolchange == true){			//	if the tools are in different pockets, change the tools and move the program flow forward. 
    toolchanger = true;
 	OneShotStart = true;
	toolchanging = true;			// we use this to inhibit the tool change shortcut for shared tools or when the pockets agree before the turret is locked etc. 
     toolchanged = false;
    }


	      	
if (toolchange == true  && OneShotStart == true){							 
	unlock = true;								// unlock turret mechanism 
	count1 = true;								// hal output for tracking  flow control
	count9 = false;								// reset cleanup flag !
   
}
		

// time delay to give settling time to turret after unlock  is enabled
// if the motor starts to fast it will beat up the turret locking pins not good for turret 
// becuase linuxcnc iterates this file every servo peroid, we have to CANCEL EVERYTHING MAUALLY becuase
// the code is not called it is FORCE FED THROUGH



if (count1 == true){							// unlock and count1 start this timer 
	timer0  +=  fperiod;						// timer is incremented by fperoid 
	count2_5 = true;							// start timer for turret watchdog
	OneShotStart = false;						// we need a way to cancel the start chain, so this is set to true by defualt and then cleared, this blocks futher execution of this
												// block and allows the logic to be free to execute in order
}

if (timer0 > unlockpause  && toolchanger == true){	// 1 second unlock pause
    count1 = false;								// flow control rest at start of routine
	count2 = true;								// move flow control forward
	ErrorTest = false;							// don't want errors setting while the turret is rolling thats a stupid idea
	timer0 = 0;									// clear timer to clean up 
}


				
		
	
   

// turret unlock time watchdog system  make sure turret unlocks in reasonable amount of time

if (count2_5 == true && unlock == true){		// unlock and count2 start this timer 
	timer1  +=  fperiod;							// timer is incremented by fperoid 
}
 if (count2_5 == true && timer1 >= unlocktimeout  && turretlocked == true) {	// just changing the timer name, becuase memory leaks
	unlock = false;								// disable motor
	toolchanger = false;						// top toolchange code flow 
	count100 = true;							// set this error flag
    estop = true; 								// disable machine
    if (set_warn_flag12 == false){				// block repetitive error messages they are annoying and flood the system
		rtapi_print_msg(RTAPI_MSG_ERR, "Fault 010 TURRET TOOK TO LONG TO UNLOCK EXCEEDED ESTOP ON TOOLCHANGER DISABLED");
        set_warn_flag12 = true;						// set flag to block prinint excesive error messages
		counterT = 105;								// diagnostic counter if the program hang use this in halscope to capture the value when it does, at least you got a shot 
		}											// diagnosing the problem
	}			



// turn turret motor on if the following conditions are met and move turret
// motor control system using flags, because nothing else seems to work 

if (count2 == true && unlock == true && turretunlocked == true ) {    
	MotorState = true; 								// turn motor on
	timer1 = 0;									// clear the timer
	count2_5 = false;							// clear turrret unlock timeout flag and turn off timer
	count3 = true;								// step forward     
}  
        // the turret did not unlock, turn off the motor and set error flags

	else if (count2 == true && unlock == true && turretlocked == true) {    			// block motor starting if progress count3 is reached and turretlocked, should have timed out earilier, but you can nevr be sure
	motor = false; 								// turn off motor dumbass					
	count100 = true;							// this flag is used to track error status
	count2_5 = false;
	counterT = 102;
		if (set_warn_flag10 == false) {  					// if the error fault is true do not set error again it will flood rtapi for messages otherwise
		rtapi_print_msg(RTAPI_MSG_ERR, "Fault 002 TURRET FAILED TO UNLOCK ESTOP ON");												
		set_warn_flag10 = true;		
		}												
	}

// the code inside the main loop




if (MotorState == true && count3 == true) {
    if (MotorTimeOn < MinMotorOn) {
        MotorTimeOn += fperiod;
        motor = true; // turn on the motor
    } else {
        if (MotorTimeOff < MinMotorOff) {
            MotorTimeOff += fperiod;
            motor = false; // turn off the motor
        } else {
            MotorTimeOn = 0; // reset the timer
            MotorTimeOff = 0;
        }
    }
} else {
    MotorTimeOn = 0; // reset the timer
    MotorTimeOff = 0;
    motor = false; // turn off the motor
}


// motor time watchdog system uses tooclahnger bit input to to activiate, then sets maximum time to 10 seconds


if (count3 == true ){				//  flow control variable for execution 
	timer2  +=  fperiod;							// timer is incremented by fperoid 
	}

if (timer2 > motortimeout && motor == true && toolchange == true){		// compare timer to motortimeout value and is toolchange active
	count3_5 = true;								// if expression evaluates true set count3_5 and error out below		
	}else if (timer2 < motortimeout && motor == true && toolchange == true    ){	// compare timer to motortimeout value and is toolchange active
   count3_5 = false;								// continue with execution of code until timer exceeds value
	}else if (count3_5 == true ) {					// if motor count3_5 and unlock all true after  motortimeout variable is exceeded, shut things down
        motor = false;								// disable motor
        toolchanger = false;						// clear timer companion variable to disable timer and stop timer runon
	    count4 = true;								// start lock delay to allow turret to position, don't slam it on the pins
	    count100 = true;							// set this error flag
        estop = true;                          		// estop on if malfunction
        if (!set_warn_flag7){							// block repetitive error messages they are annoying and flood the system
            rtapi_print_msg(RTAPI_MSG_ERR, "Fault 020 TURRET TOOK TO LONG REACH POSITION ESTOP ON TOOLCHANGER DISABLED");
            set_warn_flag7 = true;						// set flag to block prinint excesive error messages
            counterT = 115;								// diagnostic counter if the program hang use this in halscope to capture the value when it does, at least you got a shot 
			}											// diagnosing the problem
} 

								

// comparing pocket from bcd table to pocket_number from tool translation table and the motor_flag
// to determine if the motor is in position will iterate at 1000hz, if your turret is to fast you could implrment a 
// pulse count system to determine optimal run time  this is not that 


// Turn off motor if conditions are met turret in position

if (pocketfiltered == pocket_number && count3 == true && strobe == true) {
	MotorState = false;							// turn off motor we are in position !!
	count1 = false;
	count2 = false;
	count3 = false;
	count4 = true;
	count3_5 = false;
	timer0 = 0;
	timer1 = 0;
	timer2 = 0;
	timer3 = 0;
	}	


	



// we delay the turret lock function to give the unit time to settle mechanically
// the change over between the motor hydrualic circuit and the strobe ram takes a few seconds. 

if (count4 == true){		// start the 2 second delay timer before locking the turret	
	count2 = false;							// clean up timer start and timer logic. 
	count3 = false;							// clean up motor shut off logic
	count4_5 = true;
	timer2 = 0;								// clear motor runtime watchdog counter 
	toolchanger = false;						// clear timer companion variable to disable timer and stop timer runon
}else if (count4 == true && pocketfiltered != pocket_number){
    count4_5 = false;
	count4_55 = false;
	estop = true;
    rtapi_print_msg(RTAPI_MSG_ERR, "FATAL ERROR TURRET STOPPED OUT OF POSITION STOP WORK NOW !!!!!!!!"); // THIS IS INTENTIONALLY NOT FLOOD CONTROLED 
    set_warn_flag13 = true;	
    }
    	

if ( count4_5 == true) {
   count4_55 = true;
   timer3 += fperiod; 
   } 
       
 



if (count4_55 == true && timer3 >= turretsettletime ){			// compare timer3 to turretsettleime, when true execute instructions below and advance operation
	count5 = true;							// advance to next step in logic
}

    
																		
// lock turret function after delay is finished
			
if (count5 == true  ) { 	// comparison to check if strobe is on and turret can lock 
	unlock = false;						// lock the turret by setting lock output to false
	count4 = false;							// turn off timer	
	count4_5 = false;
	count4_55 = false;
	count6 = true;						// move forward to debounce cycle
	timer3 = 0;
}



// quick little debounce pause to allow all of the switches to settle

if (count6 == true){		// unlock and count1 start this timer 
	timer4  +=  fperiod;							// timer is incremented by fperoid 
	}

if (count6 == true && timer4 > debounceswitches ){		// compare timer4 value to debounceswitches value, once exceed execute cammands below
   count5 = false;									// clear flags to prevent race condition
   count7 = true;									// step forward into the program another step
  }	
     


																
// check condition of turret to ensure toolchange was sucessful 
// and  check turret for alignmnet and cleanup program state	
	
if (count7 == true ) { 						//  we need to keep the flow control going forward
		count6 = false;						// cancel time count to prevent run-on	
		count8 = true;						// set this flag to indicate toolchange finished on a hal output can be monitored 
		timer4 = 0;										// clear timer value						
}


if (count7 == true){		
	timer7  +=  fperiod;					// timer is incremented by fperoid 
	}

if (count8 == true && timer7 > IsTurretLockedDelay && turretlocked == true && pocketfiltered  == pocket_number){			// if all are good exectue to cleanup block 
	timer7 = 0;								// clear timer value for next tool change
	count7 = false;							// stops the timer from executing by canceling the previous stepping control
	count9 = true;							// move forward to end of blocks of logic
	count12 = true;							// move forward to last error check
    toolchanged = true;						// toolchanged to true so the controlled mvoes forward
	toolchanging = false;					// gaurd function to prevent unwanted toolchanged flag before tool change is 100% finished
} 
 if (count12 == true && timer7 > IsTurretLockedDelay && turretlocked == false && pocketfiltered  == pocket_number){	// final check to make sure turret is locked and in position properly
		// if this happened, fualt out , turret must be locked !!!!			    							 
	if (set_warn_flag7 ==false) {
	rtapi_print_msg(RTAPI_MSG_ERR,"Fault 003 TURRET FAILED TO LOCK WHILE IN POSITION"); 
	toolchanger = false;				// disable tool change now 
	toolchanging = true;				// inhibit reporting tool change shortcut 
	set_warn_flag7 = true;				// set flag to prevent flood of messages 1 is enough 
	count1 = false;						// prevent code execution past count1 
	count100 = true;					// blocking flag
	counterT = 103;						// set counter to fault number to prevent execution of code force execution to stop with fualt value 
	estop = true;
	}	
}
		
			
// reset timers and flow control status if is hasn't happened yet 

if (count9 == true && toolchanged == true) {	
	counterT = 0;						// count 8 is reset by toolchange request from hal this prevents unwanted execution			
	timer0 = 0;
	timer1 = 0;							// reset flow counter for error states 	
	timer2 = 0;
	timer3 = 0;							// clear all of the timer values  just for good measure it is possible some may not clear on first try !!
	timer4 = 0;
	timer5 = 0;
	timer6 = 0;
	timer7 = 0;
	timer8 = 0;
	timer9 = 0;
	count1 = false;
	count2 = false;
	count2_5 = false;
	count3 = false;
	count3_5 = false;
	count4 = false;						// reset flow control flags just to be safe
	count5 = false;
	count6 = false;
	count7 = false;
	count8 = false;
//	count9 = false;						// will cause recursive overflow or failure to execute if you cancel the block you are in while you are in it, rtapi forces the execution in the same cycle
	count10 = true;
	count12 = false;
	
	}
	
			
// end of tool change functions
// 2 handy features for clearing errors and turning on the estop 
 

// simple way	to reset error flags  set reset to true  flags are all reset   caveman logic
// allows for easy trouble shooting and repair 

if (reset == true) {								// reset tool changer, input available in hal, 
	counterT = 0;             
//	set_warn_flag1 = false;   				// resetting turret control causes flood of home turret messages, error could always be used for some other message later commented out
//	set_warn_flag2 = false;
//	set_warn_flag3 = false;
	set_warn_flag4 = false;
	set_warn_flag5 = false;
	set_warn_flag6 = false;
	set_warn_flag7 = false;
	set_warn_flag8 = false;
//	set_warn_flag9 = false;
	set_warn_flag10 = false;
	timer1 = 0;	
	timer2 = 0;
	timer3 = 0;							// clear all of the timer values  just for good measure 
	timer4 = 0;
	timer5 = 0;	
	timer6 = 0;
	timer7 = 0;							// clear all of the timer values  just for good measure 
	timer8 = 0;
	timer9 = 0;
	count1 = false;
	count2 = false;
	count2_5 = false;
	count3 = false;
	count3_5 = false;
	count4 = false;						// reset flow control flags just to be safe
	count5 = false;
	count6 = false;
	count7 = false;
	count8 = false;
	count9 = false;
	count10 = false;
    count12 = false;
    count100 = false;					// this flag blocks the tool change functions at the top 
	counterT = 0;
	estop = false;						// clear estop
	
}

											
											
// function to turn on estop output to prevent machine from running in case of toolchanger fault
											
//if ((())) {		
//		estop = true;                      		// turn Estop on to inhibit machine 
//}	
										
										// congratulations the tool change code will now rinse and repeat this is the song that never ends it goes on and on my friends 
										// recomened with a nice Red and a fine cheese