C noob having a rough time tweaking the linear tool changer code

Moderators: TomKerekes, dynomotion

Garage14
Posts: 27
Joined: Thu Jan 02, 2020 10:57 pm

C noob having a rough time tweaking the linear tool changer code

Post by Garage14 » Tue Jun 07, 2022 9:18 pm

So I have 3 parking spots that are an almost perfect transplant for the provided linear tool change code.
20220607_134654.jpg
I did my best to change the code to suit but I have 2 wrinkles.
1 : my machine homes to the back right to 0,0 but everything on the table is negative in terms of absolute coordinates.
2 : my tool parking is linear along the Y axis instead of X which I think is what the code was originally written for.

SO here's "my" code

#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "..\..\KflopToKMotionCNCFunctions.c"

//-----------------------------------------
// LINEAR TOOL CHANGING
//-----------------------------------------
#define AXISX 0
#define AXISY 1
#define AXISZ 2

//---------Absolute position of tool holders
#define HOLDER_Y_1 -253
#define HOLDER_Y_2 -414
#define HOLDER_Y_3 -581
#define HOLDER_X -717
#define HOLDER_Z -154

// absolute position of the tool height setting plate
#define TOOL_HEIGHT_PLATE_X -300
#define TOOL_HEIGHT_PLATE_Y -50

// absolute position to move to that is permanently unobstructed, and safe to move down in Z
#define TOOL_CHANGE_SAFE_POS_X -633
#define TOOL_CHANGE_SAFE_POS_Y -50

#define AXIS_SAFE_DISTANCE_X -600 // distance in mm to approach tool holder
//---------

//--------- Spindle IO bits
#define CLAW_EJECT 152 // IO bit to eject tool from spindle (Kanalog OUTPUT 152)
#define CLAW_LOOSE 138 // IO bit to sense whether the claw has ejected (Kanalog INPUT 138)
#define TOOL_SENSE 137 // IO bit to sense whether the a tool is in the spindle (Kanalog INPUT 137)
//---------

#define TOOL_VAR 9 // Tool changer desired new tool Var

// Tool changer Last tool loaded is saved globally in this Var
#define LAST_TOOL_VAR 8 // -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle
#define TOOL_DISK_FILE "c:\\Temp\\ToolChangerData.txt"


#define CLAMP_TIME 10 // seconds to wait for the clamp/unclamp
#define TOOL_HEIGHT_BIT 143 //bit to read tool height plate (Kanalog INPUT 143)

#define SAFE_HEIGHT_Z 100 // relative distance in mm to move to clear the top of the tool taper
#define TOOL_RETRACT_SPEED_Z 5.0 //speed in mm/second to move spindle up after tool has been ejected

#define SlowSpeed 2.0 //mm/sec

#define CNT_PER_MM_X 800.0
#define CNT_PER_MM_Y 800.0
#define CNT_PER_MM_Z 800.0

// function prototypes
int DoToolChange(int ToolSlot);
int GetCurrentTool(int *tool);
int SaveCurrentTool(int tool);
BOOL ToolNumberValid(int tool);
float ToolPositionX(int tool);
int MoveXY(float x, float y, float Speed);
int MoveZ(float z, float Speed);
int UnloadTool(int CurrentTool);
int LoadNewTool(int Tool);
int EjectTool(void);



main()
{
int ToolSlot = persist.UserData[TOOL_VAR]; // Requested tool to load (value stored an integer)

if (DoToolChange(ToolSlot)) // perform Tool Change
{
// error, Halt Job
DoPC(PC_COMM_HALT);
}
}

// Perform Tool Change. Return 0=Success, 1=Failure
int DoToolChange(int ToolSlot)
{
int CurrentTool;

if (GetCurrentTool(&CurrentTool)) return 1; // -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle

printf("Load Tool Slot %d requested, Current Tool %d\n",ToolSlot, CurrentTool);

if (!ToolNumberValid(ToolSlot)) // check if invalid
{
char s[80];
sprintf(s,"Invalid Tool Change Number %d\n",ToolSlot);
printf(s);
MsgBox(s, MB_ICONHAND | MB_OK);
return 1;
}

if (CurrentTool!=-1) // is there a tool in the Spindle??
if (UnloadTool(CurrentTool)) return 1; // yes, unload it

// Now Spindle is empty, load requested tool
if (LoadNewTool(ToolSlot)) return 1;

SaveCurrentTool(ToolSlot); // save the one that has been loaded
return 0; // success
}


// - Load new Tool (Spindle must be empty)
int LoadNewTool(int Tool)
{
// - Move to position of requested tool
// - Rapid move to absolute position of new tool only in X and Y
if (MoveXY(ToolPositionY(Tool),HOLDER_X,SlowSpeed)) return 1;

// - Move to tool Z position at TOOL_RETRACT_SPEED_Z
if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

// - Engage new tool
// - CLAW_EJECT bit is currently high from tool removal operation
// - Turn off CLAW_EJECT bit to engage tool
ClearBit(CLAW_EJECT);

// - Wait for time in seconds defined by CLAMP_TIME
Delay_sec(CLAMP_TIME);

// - Check to see if CLAW_LOOSE and TOOL_SENSE are high; if either are not,
// something has gone wrong; halt everything and display message indicating failure
// - Tool has been engaged
if (!ReadBit(CLAW_LOOSE))
{
printf("Claw Still Loose Error\n");
MsgBox("Claw Still Loose Error\n", MB_ICONHAND | MB_OK);
return 1;
}
if (!ReadBit(TOOL_SENSE))
{
printf("Tool Sense Error\n");
MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
return 1;
}

// - Leave tool holder by moving X axis by the positive value of X_AXIS_SAFE_DISTANCE
// - Move to position of requested tool
// - Rapid move to absolute position of new tool only in X and Y
if (MoveXY(HOLDER_X+AXIS_SAFE_DISTANCE_X,ToolPositionY(Tool),SlowSpeed)) return 1;

// - Rapid to Z home
if (MoveZ(0.0,SlowSpeed)) return 1;

return 0; //success
}


// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{
// - Rapid to Z Home to clear any work that may be on the table
if (MoveZ(0.0,SlowSpeed)) return 1;

// - Rapid to TOOL_CHANGE_SAFE_POS to execute a safe negative Z move
if (MoveXY(TOOL_CHANGE_SAFE_POS_Y,TOOL_CHANGE_SAFE_POS_X,SlowSpeed)) return 1;

// - Approach tool holder by matching Z height of tool flange currently in spindle with tool holder claw
if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

// - After matching height above, approach tool holder by moving to holder Y position
if (MoveXY(ToolPositionY(CurrentTool),TOOL_CHANGE_SAFE_POS_X,SlowSpeed)) return 1;

// - After matching Y position, match tool X position
if (MoveXY(ToolPositionY(CurrentTool),HOLDER_X,SlowSpeed)) return 1;

// - Move only in X position until current position matches tool holder position (maybe disable Y) axis?)
// ???

// - Eject tool
if (EjectTool()) return 1;


return 0; //success
}


// - Eject tool
int EjectTool(void)
{
// - Turn on CLAW_EJECT bit to remove tool from spindle
SetBit(CLAW_EJECT);

// - Wait for time in seconds defined by CLAMP_TIME
Delay_sec(CLAMP_TIME);

// - Read CLAW_LOOSE bit to see whether the tool is loose, to make a safe Z move without
// destroying tool holder
// - If CLAW_LOOSE bit is high, something has gone wrong;
// halt everything and display message indicating failure
if (ReadBit(CLAW_LOOSE))
{
printf("Claw Loose Error\n");
MsgBox("Claw Loose Error\n", MB_ICONHAND | MB_OK);
return 1;
}

// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z_SAFE_HEIGHT
if (MoveZ(HOLDER_Z+SAFE_HEIGHT_Z,TOOL_RETRACT_SPEED_Z)) return 1;

// - Read TOOL_SENSE bit to see whether the tool has been successfully ejected from the spindle
// - If TOOL_SENSE bit is high, something has gone wrong;
// halt everything and display message indicating failure
if (ReadBit(TOOL_SENSE))
{
printf("Tool Sense Release Error\n");
MsgBox("Tool Sense Release Error\n", MB_ICONHAND | MB_OK);
return 1;
}
return 0; // success
}



//return y position of tool holder as a function of the tool
float ToolPositionY(int tool)
{
return (HOLDER_Y_2-HOLDER_Y_1)*(tool-1) + HOLDER_Y_1;
}



// Get the last loaded tool. Parameter points to where to return tool
// First try to get from KFLOP memory
// if memory is invalid, try to read from disk
// if can't read disk then ask Operator
// returns 0 on success, 1 on fail or Operator asked to abort

int GetCurrentTool(int *ptool)
{
int success,Answer,result,tool;
float value;

tool = persist.UserData[LAST_TOOL_VAR];
success = ToolNumberValid(tool); // check if valid

if (!success) // invalid after power up, try to read from PC Disk File
{
// Try to open file
FILE *f=fopen(TOOL_DISK_FILE,"rt");
if (f) // did file open?
{
// read a line and convert it
result=fscanf(f,"%d",&tool);
fclose(f);

if (result==1 && ToolNumberValid(tool))
{
printf("Read Disk File Value of %d\n",tool);
success=TRUE; // success if one value converted
}
}

if (!success) printf("Unable to open/read file:%s\n",TOOL_DISK_FILE);
}

if (!success) // if still no success ask Operator
{
Answer = InputBox("Tool in Spindle or -1",&value);
if (Answer)
{
printf("Operator Canceled\n");
return 1;
}
else
{
tool=value;
printf("Operator Entered Value of %d\n",tool);
}
}

if (!ToolNumberValid(tool)) // check if invalid
{
char s[80];
sprintf(s,"Invalid Current Tool Number %d\n",tool);
printf(s);
MsgBox(s, MB_ICONHAND | MB_OK);
return 1;
}

printf("Current tool = %d\n",tool);
*ptool = tool; // return result to caller
return 0; //success
}

// save the tool number to KFLOP global Variable and to PC Disk file in case we loose power
int SaveCurrentTool(int tool)
{
persist.UserData[LAST_TOOL_VAR]=tool;
FILE *f=fopen(TOOL_DISK_FILE,"wt");
fprintf(f,"%d\n",tool);
fclose(f);
return 0;
}

// check if Current Tool number Valid
// -1 = no tool loaded
// 1-3 = valid tool
BOOL ToolNumberValid(int tool)
{
return tool == -1 || (tool>=1 && tool<=3);
}


// Move Axis XY at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveXY(float x, float y, float Speed)
{
MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
MoveAtVel(AXISY, y * CNT_PER_MM_Y, Speed * CNT_PER_MM_Y);

while (!CheckDone(AXISX) || !CheckDone(AXISY))
{
if (!chan[AXISX].Enable)
{
printf("Error X Axis Disabled\n");
MsgBox("Error X Axis Disabled\n", MB_ICONHAND | MB_OK);
return 1;
}
if (!chan[AXISY].Enable)
{
printf("Error Y Axis Disabled\n");
MsgBox("Error Y Axis Disabled\n", MB_ICONHAND | MB_OK);
return 1;
}
}
return 0; //success
}

// Move Axis Z at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveZ(float z, float Speed)
{
MoveAtVel(AXISZ, z * CNT_PER_MM_Z, Speed * CNT_PER_MM_Z);

while (!CheckDone(AXISZ))
{
if (!chan[AXISZ].Enable)
{
printf("Error Z Axis Disabled\n");
MsgBox("Error Z Axis Disabled\n", MB_ICONHAND | MB_OK);
return 1;
}
}
return 0; //success
}


My issue is that when I try to call tool one (id1001) per the interface, the machine moves simultaneously on x and y in the correct direction but it runs into the limit switch instead of stopping when it hits Y-253mm like I think it should. The max for Y is more than my measured coordinates for the parking spot so I dunno why it overshoots. My DRO in Kmotioncnc reads more than the value of the expected stopping point as it forks off into the reeds.

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: C noob having a rough time tweaking the linear tool changer code

Post by TomKerekes » Wed Jun 08, 2022 2:21 am

I think in a few places calling MoveXY have the X Y values swapped. Should be X then Y.

You might also tell us what is printed to the Console.

Please post Code within the Code Tags so it is more readable.
Regards,

Tom Kerekes
Dynomotion, Inc.

Garage14
Posts: 27
Joined: Thu Jan 02, 2020 10:57 pm

Re: C noob having a rough time tweaking the linear tool changer code

Post by Garage14 » Wed Jun 22, 2022 8:20 pm

So I think I fixed everywhere that I reversed the x and y. Unfortunately I get the same result. If I home the machine and then zero all axis, I then click id 1 under the tool dropdown I get a box that prompts if there is a tool in the spindle. I enter -1 for no and hit ok and the box goes away as the machine moves on x and y. When it hits a limit switch I get an error that pops up saying that the axis is disabled. If I stop the machine mid tool change I get a claw still loose error. Maybe I am calling the tool change incorrectly? Or I need to change something to make the machine stop as it reaches it's destination because it is moving in a negative direction towards a negative destination?

Here's the tool setup screen
tool setup m6.png
and my tool file
tool file.png

Code: Select all

#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "..\..\KflopToKMotionCNCFunctions.c"

//-----------------------------------------
//		LINEAR TOOL CHANGING
//-----------------------------------------
#define AXISX 0
#define AXISY 1
#define AXISZ 2

//---------Absolute position of tool holders
#define HOLDER_Y_1 -253
#define HOLDER_Y_2 -414
#define HOLDER_Y_3 -581
#define HOLDER_X -717
#define HOLDER_Z -154

// absolute position of the tool height setting plate
#define TOOL_HEIGHT_PLATE_X -300 
#define TOOL_HEIGHT_PLATE_Y -50 

// absolute position to move to that is permanently unobstructed, and safe to move down in Z
#define TOOL_CHANGE_SAFE_POS_X -633  
#define TOOL_CHANGE_SAFE_POS_Y -50

#define AXIS_SAFE_DISTANCE_X -600  // distance in mm to approach tool holder
//---------

//--------- Spindle IO bits
#define CLAW_EJECT 152		// IO bit to eject tool from spindle (Kanalog OUTPUT 152)
#define CLAW_LOOSE 138		// IO bit to sense whether the claw has ejected (Kanalog INPUT 138)
#define TOOL_SENSE 137		// IO bit to sense whether the a tool is in the spindle (Kanalog INPUT 137)
//---------

#define TOOL_VAR 9        	// Tool changer desired new tool Var

// Tool changer Last tool loaded is saved globally in this Var
#define LAST_TOOL_VAR 8   	//  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle
#define TOOL_DISK_FILE "c:\\Temp\\ToolChangerData.txt"


#define CLAMP_TIME 10    	// seconds to wait for the clamp/unclamp
#define TOOL_HEIGHT_BIT	143	//bit to read tool height plate (Kanalog INPUT 143)

#define SAFE_HEIGHT_Z 100   // relative distance in mm to move to clear the top of the tool taper
#define TOOL_RETRACT_SPEED_Z 5.0	//speed in mm/second to move spindle up after tool has been ejected

#define SlowSpeed 2.0 //mm/sec

#define CNT_PER_MM_X 800.0 
#define CNT_PER_MM_Y 800.0 
#define CNT_PER_MM_Z 800.0 

// function prototypes
int DoToolChange(int ToolSlot);
int GetCurrentTool(int *tool);
int SaveCurrentTool(int tool);
BOOL ToolNumberValid(int tool);
float ToolPositionX(int tool);
int MoveXY(float x, float y, float Speed);
int MoveZ(float z, float Speed);
int UnloadTool(int CurrentTool);
int LoadNewTool(int Tool);
int EjectTool(void);



main()
{
	int ToolSlot = persist.UserData[TOOL_VAR];  // Requested tool to load (value stored an integer) 

	if (DoToolChange(ToolSlot))  // perform Tool Change
	{
		// error, Halt Job
		DoPC(PC_COMM_HALT);
	}
}

// Perform Tool Change.  Return 0=Success, 1=Failure
int DoToolChange(int ToolSlot)
{
	int CurrentTool;

	if (GetCurrentTool(&CurrentTool)) return 1;  //  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle

	printf("Load Tool Slot %d requested, Current Tool %d\n",ToolSlot, CurrentTool);
	
	if (!ToolNumberValid(ToolSlot))  // check if invalid
	{
		char s[80];
		sprintf(s,"Invalid Tool Change Number %d\n",ToolSlot);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}
	
	if (CurrentTool!=-1) // is there a tool in the Spindle??
		if (UnloadTool(CurrentTool)) return 1;  // yes, unload it
		
	// Now Spindle is empty, load requested tool
	if (LoadNewTool(ToolSlot)) return 1;
	
	SaveCurrentTool(ToolSlot);  // save the one that has been loaded
	return 0;  // success
}


// - Load new Tool (Spindle must be empty)
int LoadNewTool(int Tool)
{
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(HOLDER_X,ToolPositionY(Tool),SlowSpeed)) return 1;

	// - Move to tool Z position at TOOL_RETRACT_SPEED_Z
	if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

	// - Engage new tool
	// - CLAW_EJECT bit is currently high from tool removal operation
	// - Turn off CLAW_EJECT bit to engage tool
	ClearBit(CLAW_EJECT);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);

	// - Check to see if CLAW_LOOSE and TOOL_SENSE are high; if either are not, 
	//		something has gone wrong; halt everything and display message indicating failure
	// - Tool has been engaged
	if (!ReadBit(CLAW_LOOSE))
	{
		printf("Claw Still Loose Error\n");
		MsgBox("Claw Still Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	if (!ReadBit(TOOL_SENSE))
	{
		printf("Tool Sense Error\n");
		MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}

	// - Leave tool holder by moving X axis by the positive value of X_AXIS_SAFE_DISTANCE
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(HOLDER_X+AXIS_SAFE_DISTANCE_X,ToolPositionY(Tool),SlowSpeed)) return 1;

	// - Rapid to Z home
	if (MoveZ(0.0,SlowSpeed)) return 1;

	return 0; //success
}


// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{
	// - Rapid to Z Home to clear any work that may be on the table
	if (MoveZ(0.0,SlowSpeed)) return 1;

	// - Rapid to TOOL_CHANGE_SAFE_POS to execute a safe negative Z move
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,TOOL_CHANGE_SAFE_POS_Y,SlowSpeed)) return 1;
	
	// - Approach tool holder by matching Z height of tool flange currently in spindle with tool holder                            claw
	if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

	// - After matching height above, approach tool holder by moving to holder Y position
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,ToolPositionY(CurrentTool),SlowSpeed)) return 1;

	// - After matching X position, match tool Y position
	if (MoveXY(HOLDER_X,ToolPositionY(CurrentTool),SlowSpeed)) return 1;

	// - Move only in X position until current position matches tool holder position (maybe disable Y)                          axis?)
	// ???
	
	// - Eject tool
	if (EjectTool()) return 1;


	return 0; //success
}


// - Eject tool
int EjectTool(void)
{ 
	// - Turn on CLAW_EJECT bit to remove tool from spindle
	SetBit(CLAW_EJECT);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);
	
	// - Read CLAW_LOOSE bit to see whether the tool is loose, to make a safe Z move without  
    //      destroying tool holder
	// - If CLAW_LOOSE bit is high, something has gone wrong;
	//		halt everything and display message indicating failure
	if (ReadBit(CLAW_LOOSE))
	{
		printf("Claw Loose Error\n");
		MsgBox("Claw Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}

	// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z_SAFE_HEIGHT
	if (MoveZ(HOLDER_Z+SAFE_HEIGHT_Z,TOOL_RETRACT_SPEED_Z)) return 1;

	// - Read TOOL_SENSE bit to see whether the tool has been successfully ejected from the spindle
	// - If TOOL_SENSE bit is high, something has gone wrong; 
	//		halt everything and display message indicating failure
	if (ReadBit(TOOL_SENSE))
	{
		printf("Tool Sense Release Error\n");
		MsgBox("Tool Sense Release Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	return 0; // success
}



//return y position of tool holder as a function of the tool
float ToolPositionY(int tool)
{
	return (HOLDER_Y_2-HOLDER_Y_1)*(tool-1) + HOLDER_Y_1;
}



// Get the last loaded tool.  Parameter points to where to return tool
// First try to get from KFLOP memory
// if memory is invalid, try to read from disk
// if can't read disk then ask Operator
// returns 0 on success, 1 on fail or Operator asked to abort

int GetCurrentTool(int *ptool)
{
	int success,Answer,result,tool;
	float value;

	tool = persist.UserData[LAST_TOOL_VAR];
	success = ToolNumberValid(tool);  // check if valid

	if (!success)   // invalid after power up, try to read from PC Disk File
	{
		// Try to open file
		FILE *f=fopen(TOOL_DISK_FILE,"rt");
		if (f)  // did file open?
		{
			// read a line and convert it
			result=fscanf(f,"%d",&tool);
			fclose(f);
			
			if (result==1 && ToolNumberValid(tool))
			{
				printf("Read Disk File Value of %d\n",tool);
				success=TRUE; // success if one value converted
			}
		}
		
		if (!success) printf("Unable to open/read file:%s\n",TOOL_DISK_FILE);  
	}

	if (!success)   // if still no success ask Operator
	{
		Answer = InputBox("Tool in Spindle or -1",&value);
		if (Answer)
		{
			printf("Operator Canceled\n");
			return 1;
		}
		else
		{
			tool=value;
			printf("Operator Entered Value of %d\n",tool);
		}
	}

	if (!ToolNumberValid(tool))  // check if invalid
	{
		char s[80];
		sprintf(s,"Invalid Current Tool Number %d\n",tool);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}
	
	printf("Current tool = %d\n",tool);
	*ptool = tool;  // return result to caller
	return 0;  //success
}

// save the tool number to KFLOP global Variable and to PC Disk file in case we loose power
int SaveCurrentTool(int tool)
{
	persist.UserData[LAST_TOOL_VAR]=tool;
	FILE *f=fopen(TOOL_DISK_FILE,"wt");
	fprintf(f,"%d\n",tool);
	fclose(f);
	return 0;
}

// check if Current Tool number Valid
// -1 = no tool loaded
// 1-3 = valid tool
BOOL ToolNumberValid(int tool)
{
	return tool == -1 || (tool>=1 && tool<=3);
}


// Move Axis XY at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveXY(float x, float y, float Speed)
{
	MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
	MoveAtVel(AXISY, y * CNT_PER_MM_Y, Speed * CNT_PER_MM_Y);
	
	while (!CheckDone(AXISX) || !CheckDone(AXISY))
	{
		if (!chan[AXISX].Enable)
		{
			printf("Error X Axis Disabled\n");
			MsgBox("Error X Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
		if (!chan[AXISY].Enable)
		{
			printf("Error Y Axis Disabled\n");
			MsgBox("Error Y Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;  //success
}

// Move Axis Z at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveZ(float z, float Speed)
{
	MoveAtVel(AXISZ, z * CNT_PER_MM_Z, Speed * CNT_PER_MM_Z);
	
	while (!CheckDone(AXISZ))
	{
		if (!chan[AXISZ].Enable)
		{
			printf("Error Z Axis Disabled\n");
			MsgBox("Error Z Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;  //success
}


Thanks for any advice. I'm pretty stuck on what else to try.

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: C noob having a rough time tweaking the linear tool changer code

Post by TomKerekes » Thu Jun 23, 2022 12:37 am

The code looks reasonable to me.

is 800 counts/mm correct for your machine?

You didn't tell us what is printed on the KMotion.exe Console Screen.

Its very simple to debug these types of issues by adding printf statements.

Add this printf to the beginning of your MoveXY function:

Code: Select all

	printf("MoveXY X=%fmm Y=%fmm X=%fcnts Y=%fcnts\n",x,y,x * CNT_PER_MM_X,y * CNT_PER_MM_Y);
Then make a tool change to slot 1 again and tell us what is printed on the Console Screen.
Regards,

Tom Kerekes
Dynomotion, Inc.

Garage14
Posts: 27
Joined: Thu Jan 02, 2020 10:57 pm

Re: C noob having a rough time tweaking the linear tool changer code

Post by Garage14 » Mon Jul 11, 2022 1:01 am

800 counts per mm is correct afaik. The machine says 20320 for scale factor and I did the math, converting to mm=800. Also, I didn't know what print to console meant but I've got it now. Initially I got the same error as this post =>
https://www.cnczone.com/forums/dynomoti ... -code.html
So per that thread I made a temp folder and named a text file toolchangerdata with -1 as the only text. Having tried another another tool change after adding that print command I get this...
console print error.png
as the machine hits the Y limit switch.

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: C noob having a rough time tweaking the linear tool changer code

Post by TomKerekes » Mon Jul 11, 2022 1:22 am

Desired new tool is 1 and Current as -1 looks correct

I don't see a Move XY printout. Did you add it? Please re-post the file with it added.
Regards,

Tom Kerekes
Dynomotion, Inc.

Garage14
Posts: 27
Joined: Thu Jan 02, 2020 10:57 pm

Re: C noob having a rough time tweaking the linear tool changer code

Post by Garage14 » Wed Jul 13, 2022 6:57 pm

Code: Select all

#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "..\..\KflopToKMotionCNCFunctions.c"

//-----------------------------------------
//		LINEAR TOOL CHANGING
//-----------------------------------------
#define AXISX 0
#define AXISY 1
#define AXISZ 2

//---------Absolute position of tool holders
#define HOLDER_Y_1 -100
#define HOLDER_Y_2 -200
#define HOLDER_Y_3 -300
#define HOLDER_X -718
#define HOLDER_Z -154

// absolute position of the tool height setting plate
#define TOOL_HEIGHT_PLATE_X -300 
#define TOOL_HEIGHT_PLATE_Y -50 

// absolute position to move to that is permanently unobstructed, and safe to move down in Z
#define TOOL_CHANGE_SAFE_POS_X -650  
#define TOOL_CHANGE_SAFE_POS_Y -50

#define AXIS_SAFE_DISTANCE_X -600  // distance in mm to approach tool holder
//---------

//--------- Spindle IO bits
#define CLAW_EJECT 152		// IO bit to eject tool from spindle (Kanalog OUTPUT 152)
#define CLAW_LOOSE 138		// IO bit to sense whether the claw has ejected (Kanalog INPUT 138)
#define TOOL_SENSE 137		// IO bit to sense whether the a tool is in the spindle (Kanalog INPUT 137)
//---------

#define TOOL_VAR 9        	// Tool changer desired new tool Var

// Tool changer Last tool loaded is saved globally in this Var
#define LAST_TOOL_VAR 8   	//  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle
#define TOOL_DISK_FILE "c:\\Temp\\ToolChangerData.txt"


#define CLAMP_TIME 10    	// seconds to wait for the clamp/unclamp
#define TOOL_HEIGHT_BIT	143	//bit to read tool height plate (Kanalog INPUT 143)

#define SAFE_HEIGHT_Z 100   // relative distance in mm to move to clear the top of the tool taper
#define TOOL_RETRACT_SPEED_Z 5.0	//speed in mm/second to move spindle up after tool has been ejected

#define SlowSpeed 2.0 //mm/sec

#define CNT_PER_MM_X 800.0 
#define CNT_PER_MM_Y 800.0 
#define CNT_PER_MM_Z 800.0 

// function prototypes
int DoToolChange(int ToolSlot);
int GetCurrentTool(int *tool);
int SaveCurrentTool(int tool);
BOOL ToolNumberValid(int tool);
float ToolPositionX(int tool);
int MoveXY(float x, float y, float Speed);
int MoveZ(float z, float Speed);
int UnloadTool(int CurrentTool);
int LoadNewTool(int Tool);
int EjectTool(void);



main()
{
	int ToolSlot = persist.UserData[TOOL_VAR];  // Requested tool to load (value stored an integer) 

	if (DoToolChange(ToolSlot))  // perform Tool Change
	{
		// error, Halt Job
		DoPC(PC_COMM_HALT);
	}
}

// Perform Tool Change.  Return 0=Success, 1=Failure
int DoToolChange(int ToolSlot)
{
	int CurrentTool;

	if (GetCurrentTool(&CurrentTool)) return 1;  //  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle

	printf("Load Tool Slot %d requested, Current Tool %d\n",ToolSlot, CurrentTool);
	
	if (!ToolNumberValid(ToolSlot))  // check if invalid
	{
		char s[80];
		sprintf(s,"Invalid Tool Change Number %d\n",ToolSlot);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}
	
	if (CurrentTool!=-1) // is there a tool in the Spindle??
		if (UnloadTool(CurrentTool)) return 1;  // yes, unload it
		
	// Now Spindle is empty, load requested tool
	if (LoadNewTool(ToolSlot)) return 1;
	
	SaveCurrentTool(ToolSlot);  // save the one that has been loaded
	return 0;  // success
}


// - Load new Tool (Spindle must be empty)
int LoadNewTool(int Tool)
{
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(HOLDER_X,ToolPositionY(Tool),SlowSpeed)) return 1;

	// - Move to tool Z position at TOOL_RETRACT_SPEED_Z
	if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

	// - Engage new tool
	// - CLAW_EJECT bit is currently high from tool removal operation
	// - Turn off CLAW_EJECT bit to engage tool
	ClearBit(CLAW_EJECT);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);

	// - Check to see if CLAW_LOOSE and TOOL_SENSE are high; if either are not, 
	//		something has gone wrong; halt everything and display message indicating failure
	// - Tool has been engaged
	if (!ReadBit(CLAW_LOOSE))
	{
		printf("Claw Still Loose Error\n");
		MsgBox("Claw Still Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	if (!ReadBit(TOOL_SENSE))
	{
		printf("Tool Sense Error\n");
		MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}

	// - Leave tool holder by moving X axis by the positive value of X_AXIS_SAFE_DISTANCE
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(HOLDER_X+AXIS_SAFE_DISTANCE_X,ToolPositionY(Tool),SlowSpeed)) return 1;

	// - Rapid to Z home
	if (MoveZ(0.0,SlowSpeed)) return 1;

	return 0; //success
}


// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{
	// - Rapid to Z Home to clear any work that may be on the table
	if (MoveZ(0.0,SlowSpeed)) return 1;

	// - Rapid to TOOL_CHANGE_SAFE_POS to execute a safe negative Z move
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,TOOL_CHANGE_SAFE_POS_Y,SlowSpeed)) return 1;
	
	// - Approach tool holder by matching Z height of tool flange currently in spindle with tool holder                            claw
	if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

	// - After matching height above, approach tool holder by moving to holder Y position
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,ToolPositionY(CurrentTool),SlowSpeed)) return 1;

	// - After matching X position, match tool Y position
	if (MoveXY(HOLDER_X,ToolPositionY(CurrentTool),SlowSpeed)) return 1;

	// - Move only in X position until current position matches tool holder position (maybe disable Y)                          axis?)
	// ???
	
	// - Eject tool
	if (EjectTool()) return 1;


	return 0; //success
}


// - Eject tool
int EjectTool(void)
{ 
	// - Turn on CLAW_EJECT bit to remove tool from spindle
	SetBit(CLAW_EJECT);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);
	
	// - Read CLAW_LOOSE bit to see whether the tool is loose, to make a safe Z move without  
    //      destroying tool holder
	// - If CLAW_LOOSE bit is high, something has gone wrong;
	//		halt everything and display message indicating failure
	if (ReadBit(CLAW_LOOSE))
	{
		printf("Claw Loose Error\n");
		MsgBox("Claw Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}

	// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z_SAFE_HEIGHT
	if (MoveZ(HOLDER_Z+SAFE_HEIGHT_Z,TOOL_RETRACT_SPEED_Z)) return 1;

	// - Read TOOL_SENSE bit to see whether the tool has been successfully ejected from the spindle
	// - If TOOL_SENSE bit is high, something has gone wrong; 
	//		halt everything and display message indicating failure
	if (ReadBit(TOOL_SENSE))
	{
		printf("Tool Sense Release Error\n");
		MsgBox("Tool Sense Release Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	return 0; // success
}



//return y position of tool holder as a function of the tool
float ToolPositionY(int tool)
{
	return (HOLDER_Y_2+HOLDER_Y_1)*(tool-1) + HOLDER_Y_1;
}



// Get the last loaded tool.  Parameter points to where to return tool
// First try to get from KFLOP memory
// if memory is invalid, try to read from disk
// if can't read disk then ask Operator
// returns 0 on success, 1 on fail or Operator asked to abort

int GetCurrentTool(int *ptool)
{
	int success,Answer,result,tool;
	float value;

	tool = persist.UserData[LAST_TOOL_VAR];
	success = ToolNumberValid(tool);  // check if valid

	if (!success)   // invalid after power up, try to read from PC Disk File
	{
		// Try to open file
		FILE *f=fopen(TOOL_DISK_FILE,"rt");
		if (f)  // did file open?
		{
			// read a line and convert it
			result=fscanf(f,"%d",&tool);
			fclose(f);
			
			if (result==1 && ToolNumberValid(tool))
			{
				printf("Read Disk File Value of %d\n",tool);
				success=TRUE; // success if one value converted
			}
		}
		
		if (!success) printf("Unable to open/read file:%s\n",TOOL_DISK_FILE);  
	}

	if (!success)   // if still no success ask Operator
	{
		Answer = InputBox("Tool in Spindle or -1",&value);
		if (Answer)
		{
			printf("Operator Canceled\n");
			return 1;
		}
		else
		{
			tool=value;
			printf("Operator Entered Value of %d\n",tool);
		}
	}

	if (!ToolNumberValid(tool))  // check if invalid
	{
		char s[80];
		sprintf(s,"Invalid Current Tool Number %d\n",tool);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}
	
	printf("Current tool = %d\n",tool);
	*ptool = tool;  // return result to caller
	return 0;  //success
}

// save the tool number to KFLOP global Variable and to PC Disk file in case we loose power
int SaveCurrentTool(int tool)
{
	persist.UserData[LAST_TOOL_VAR]=tool;
	FILE *f=fopen(TOOL_DISK_FILE,"wt");
	fprintf(f,"%d\n",tool);
	fclose(f);
	return 0;
}

// check if Current Tool number Valid
// -1 = no tool loaded
// 1-3 = valid tool
BOOL ToolNumberValid(int tool)
{
	return tool == -1 || (tool>=1 && tool<=3);
}

printf("MoveXY X=%fmm Y=%fmm X=%fcnts Y=%fcnts\n",x,y,x * CNT_PER_MM_X,y * CNT_PER_MM_Y);
// Move Axis XY at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveXY(float x, float y, float Speed)
{
printf("MoveXY X=%fmm Y=%fmm X=%fcnts Y=%fcnts\n",x,y,x * CNT_PER_MM_X,y * CNT_PER_MM_Y);
	MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
	MoveAtVel(AXISY, y * CNT_PER_MM_Y, Speed * CNT_PER_MM_Y);
	
	while (!CheckDone(AXISX) || !CheckDone(AXISY))
	{
		if (!chan[AXISX].Enable)
		{
			printf("Error X Axis Disabled\n");
			MsgBox("Error X Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
		if (!chan[AXISY].Enable)
		{
			printf("Error Y Axis Disabled\n");
			MsgBox("Error Y Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;  //success
}

// Move Axis Z at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveZ(float z, float Speed)
{
	MoveAtVel(AXISZ, z * CNT_PER_MM_Z, Speed * CNT_PER_MM_Z);
	
	while (!CheckDone(AXISZ))
	{
		if (!chan[AXISZ].Enable)
		{
			printf("Error Z Axis Disabled\n");
			MsgBox("Error Z Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;  //success
}


So I hade the print command outside the brackets derp.
Now that It's telling me where it's going I can see that it is trying to send Y to outer space.
I read back through to try and find why it would ignore my Y holder location, that I put at the top, and I found a formula a ways down:

Code: Select all

//return y position of tool holder as a function of the tool
float ToolPositionY(int tool)
{
	return (HOLDER_Y_2-HOLDER_Y_1)*(tool-1) + HOLDER_Y_1;
}
I dunno why the need for a formula instead of just grabbing the absolute position of the tool holder but I also dunno how to recode it so I moved my holders to y-100 y-200 and y-300 so the formula works after changing the sign between (HOLDER_Y_2-HOLDER_Y_1) to be (HOLDER_Y_2+HOLDER_Y_1) since my values are negative.
I get a slightly different value from the print command on Y when I call tool1 but it is still comically incorrect.
console readout.png
the first 2 coordinates are my original code just with the print command inside the brackets for move xy and the last is after I changed my holder Y locations and inverted the sign in the formula.

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: C noob having a rough time tweaking the linear tool changer code

Post by TomKerekes » Wed Jul 13, 2022 8:39 pm

I see we forgot to define a function prototype for:

float ToolPositionY(int tool);

When the Compiler first encounters that function it basically makes a guess on what type of return value and parameters it has. Later when the function is actually defined if it is different then the result is basically garbage. 'float' and 'int' memory formats are different. We should always declare function prototypes near the beginning of the file (before any function calls) to avoid this issue. The TI Compiler can be invoked by right-clicking the C Program editor. It is more thorough at warning of this.

Regarding the formula: the idea is if there are a bunch of tool holders all equally spaced, then the nth position can be determined by that formula which is simpler than a bunch of checks for what tool and then fetching the corresponding positions.

btw the code you posted doesn't compile. There is an extra printf statement left in before the MoveXY function.

HTH
Regards,

Tom Kerekes
Dynomotion, Inc.

Garage14
Posts: 27
Joined: Thu Jan 02, 2020 10:57 pm

Re: C noob having a rough time tweaking the linear tool changer code

Post by Garage14 » Fri Jul 15, 2022 7:03 pm

Ok getting pretty close now, first issue is that now that I have the commanded position correct I can see an offset of some kind at the holder when I get there.
tool holder offset.png
the spindle starts moving down with a 2.5mm difference from what the tool changer code is sending. I checked the tool setup and I have a .25 diameter for tool one. Assuming that page is in inches, .25" diameter doesn't equal 2.5mm radius so I think I have an offset somewhere else I am not aware of.
The next issue is that the code needs more tweaking for my personal machine because my holders are C shaped instead of U shaped so I can't just leave straight out the side. I have to lift up a little before making my X move. I tried adding a move X and Move Y command to help facilitate these moves but my program won't compile. I am obviously not understanding something about how the code works because just slapping new commands in doesn't jive 8-)
I assume it has something to do with moveX not being defined in the KMotionDef.h or KflopToKMotionCNCFunctions.c
Can I add easily add it? Or is there a sneaky workaround to move on a single axis at a time? MoveZ works fine so I know it can be done.
Here's the freshly mangled code

Code: Select all

#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "..\..\KflopToKMotionCNCFunctions.c"

//-----------------------------------------
//		LINEAR TOOL CHANGING
//-----------------------------------------
#define AXISX 0
#define AXISY 1
#define AXISZ 2

//---------Absolute position of tool holders
#define HOLDER_Y_1 -100
#define HOLDER_Y_2 -200
#define HOLDER_Y_3 -300
#define HOLDER_X -718
#define HOLDER_Z -162

// absolute position of the tool height setting plate
#define TOOL_HEIGHT_PLATE_X -300 
#define TOOL_HEIGHT_PLATE_Y -50 

// absolute position to move to that is permanently unobstructed, and safe to move down in Z
#define TOOL_CHANGE_SAFE_POS_X -650  
#define TOOL_CHANGE_SAFE_POS_Y -50
#define TOOL_CHANGE_SAFE_POS_Z -154

#define AXIS_SAFE_DISTANCE_X -600  // distance in mm to approach tool holder
//---------

//--------- Spindle IO bits
#define CLAW_EJECT 152		// IO bit to eject tool from spindle (Kanalog OUTPUT 152)
#define CLAW_LOOSE 138		// IO bit to sense whether the claw has ejected (Kanalog INPUT 138)
#define TOOL_SENSE 137		// IO bit to sense whether the a tool is in the spindle (Kanalog INPUT 137)
//---------

#define TOOL_VAR 9        	// Tool changer desired new tool Var

// Tool changer Last tool loaded is saved globally in this Var
#define LAST_TOOL_VAR 8   	//  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle
#define TOOL_DISK_FILE "c:\\Temp\\ToolChangerData.txt"


#define CLAMP_TIME 3    	// seconds to wait for the clamp/unclamp
#define TOOL_HEIGHT_BIT	143	//bit to read tool height plate (Kanalog INPUT 143)

#define SAFE_HEIGHT_Z -80   // absolute distance in mm to move to clear the top of the tool taper
#define TOOL_RETRACT_SPEED_Z 5.0	//speed in mm/second to move spindle up after tool has been ejected

#define SlowSpeed 2.0 //mm/sec

#define CNT_PER_MM_X 800.0 
#define CNT_PER_MM_Y 800.0 
#define CNT_PER_MM_Z 800.0 

// function prototypes
int DoToolChange(int ToolSlot);
int GetCurrentTool(int *tool);
int SaveCurrentTool(int tool);
BOOL ToolNumberValid(int tool);
float ToolPositionX(int tool);
float ToolPositionY(int tool);
int MoveXY(float x, float y, float Speed);
int MoveX(float x, float Speed);
int MoveY(float y, float Speed);
int MoveZ(float z, float Speed);
int UnloadTool(int CurrentTool);
int LoadNewTool(int Tool);
int EjectTool(void);



main()
{
	int ToolSlot = persist.UserData[TOOL_VAR];  // Requested tool to load (value stored an integer) 

	if (DoToolChange(ToolSlot))  // perform Tool Change
	{
		// error, Halt Job
		DoPC(PC_COMM_HALT);
	}
}

// Perform Tool Change.  Return 0=Success, 1=Failure
int DoToolChange(int ToolSlot)
{
	int CurrentTool;

	if (GetCurrentTool(&CurrentTool)) return 1;  //  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle

	printf("Load Tool Slot %d requested, Current Tool %d\n",ToolSlot, CurrentTool);
	
	if (!ToolNumberValid(ToolSlot))  // check if invalid
	{
		char s[80];
		sprintf(s,"Invalid Tool Change Number %d\n",ToolSlot);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}
	
	if (CurrentTool!=-1) // is there a tool in the Spindle??
		if (UnloadTool(CurrentTool)) return 1;  // yes, unload it
		
	// Now Spindle is empty, load requested tool
	if (LoadNewTool(ToolSlot)) return 1;
	
	SaveCurrentTool(ToolSlot);  // save the one that has been loaded
	return 0;  // success
}


// - Load new Tool (Spindle must be empty)
int LoadNewTool(int Tool)
{
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(HOLDER_X,ToolPositionY(Tool),SlowSpeed)) return 1;

	// - open claw if picking up a tool with spindle empty
	SetBit(CLAW_EJECT);

	// - Move to tool Z position at TOOL_RETRACT_SPEED_Z
	if (MoveZ(HOLDER_Z,SlowSpeed)) return 1;

	// - Engage new tool
	// - CLAW_EJECT bit is currently high from tool removal operation
	// - Turn off CLAW_EJECT bit to engage tool
	ClearBit(CLAW_EJECT);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);

	// - Check to see if CLAW_LOOSE and TOOL_SENSE are high; if either are not, 
	//		something has gone wrong; halt everything and display message indicating failure
	// - Tool has been engaged
	if (!ReadBit(CLAW_LOOSE))
	{
		printf("Claw Still Loose Error\n");
		MsgBox("Claw Still Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	if (!ReadBit(TOOL_SENSE))
	{
		printf("Tool Sense Error\n");
		MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}

	// - Leave tool holder by moving Z axis to TOOL_CHANGE_SAFE_POS_Z then X axis by the positive value of X_AXIS_SAFE_DISTANCE
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveZ(TOOL_CHANGE_SAFE_POS_Z,SlowSpeed)) return 1;
	if (MoveX(AXIS_SAFE_DISTANCE_X,SlowSpeed)) return 1;

	// - Rapid to Z home
	if (MoveZ(0.0,SlowSpeed)) return 1;

	return 0; //success
}


// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{
	// - Rapid to Z Home to clear any work that may be on the table
	if (MoveZ(0.0,SlowSpeed)) return 1;

	// - Rapid to TOOL_CHANGE_SAFE_POS to execute a safe negative Z move
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,TOOL_CHANGE_SAFE_POS_Y,SlowSpeed)) return 1;
	
	// - Approach tool holder by matching Z height of tool flange currently in spindle
	if (MoveZ(TOOL_CHANGE_SAFE_POS_Z,SlowSpeed)) return 1;

	// - After matching height above, approach tool holder by moving to holder Y position
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,ToolPositionY(CurrentTool),SlowSpeed)) return 1;

	// - After match tool Y position, match X position
	if (MoveX(HOLDER_X,SlowSpeed)) return 1;

	// - Eject tool
	if (EjectTool()) return 1;


	return 0; //success
}


// - Eject tool
int EjectTool(void)
{ 
	// - Turn on CLAW_EJECT bit to remove tool from spindle
	SetBit(CLAW_EJECT);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);
	
	// - Read CLAW_LOOSE bit to see whether the tool is loose, to make a safe Z move without  
	//   destroying tool holder
	// - If CLAW_LOOSE bit is high, something has gone wrong;
	//		halt everything and display message indicating failure
	if (ReadBit(CLAW_LOOSE))
	{
		printf("Claw Loose Error\n");
		MsgBox("Claw Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}

	// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z_SAFE_HEIGHT
	if (MoveZ(SAFE_HEIGHT_Z,TOOL_RETRACT_SPEED_Z)) return 1;

	// - Read TOOL_SENSE bit to see whether the tool has been successfully ejected from the spindle
	// - If TOOL_SENSE bit is high, something has gone wrong; 
	//		halt everything and display message indicating failure
	if (ReadBit(TOOL_SENSE))
	{
		printf("Tool Sense Release Error\n");
		MsgBox("Tool Sense Release Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	return 0; // success
}



//return y position of tool holder as a function of the tool
float ToolPositionY(int tool)
{
	return (HOLDER_Y_2+HOLDER_Y_1)*(tool-1) + HOLDER_Y_1;
}



// Get the last loaded tool.  Parameter points to where to return tool
// First try to get from KFLOP memory
// if memory is invalid, try to read from disk
// if can't read disk then ask Operator
// returns 0 on success, 1 on fail or Operator asked to abort

int GetCurrentTool(int *ptool)
{
	int success,Answer,result,tool;
	float value;

	tool = persist.UserData[LAST_TOOL_VAR];
	success = ToolNumberValid(tool);  // check if valid

	if (!success)   // invalid after power up, try to read from PC Disk File
	{
		// Try to open file
		FILE *f=fopen(TOOL_DISK_FILE,"rt");
		if (f)  // did file open?
		{
			// read a line and convert it
			result=fscanf(f,"%d",&tool);
			fclose(f);
			
			if (result==1 && ToolNumberValid(tool))
			{
				printf("Read Disk File Value of %d\n",tool);
				success=TRUE; // success if one value converted
			}
		}
		
		if (!success) printf("Unable to open/read file:%s\n",TOOL_DISK_FILE);  
	}

	if (!success)   // if still no success ask Operator
	{
		Answer = InputBox("Tool in Spindle or -1",&value);
		if (Answer)
		{
			printf("Operator Canceled\n");
			return 1;
		}
		else
		{
			tool=value;
			printf("Operator Entered Value of %d\n",tool);
		}
	}

	if (!ToolNumberValid(tool))  // check if invalid
	{
		char s[80];
		sprintf(s,"Invalid Current Tool Number %d\n",tool);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}
	
	printf("Current tool = %d\n",tool);
	*ptool = tool;  // return result to caller
	return 0;  //success
}

// save the tool number to KFLOP global Variable and to PC Disk file in case we loose power
int SaveCurrentTool(int tool)
{
	persist.UserData[LAST_TOOL_VAR]=tool;
	FILE *f=fopen(TOOL_DISK_FILE,"wt");
	fprintf(f,"%d\n",tool);
	fclose(f);
	return 0;
}

// check if Current Tool number Valid
// -1 = no tool loaded
// 1-3 = valid tool
BOOL ToolNumberValid(int tool)
{
	return tool == -1 || (tool>=1 && tool<=3);
}

// Move Axis XY at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveXY(float x, float y, float Speed)
{
printf("MoveXY X=%fmm Y=%fmm X=%fcnts Y=%fcnts\n",x,y,x * CNT_PER_MM_X,y * CNT_PER_MM_Y);
	MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
	MoveAtVel(AXISY, y * CNT_PER_MM_Y, Speed * CNT_PER_MM_Y);
	
	while (!CheckDone(AXISX) || !CheckDone(AXISY))
	{
		if (!chan[AXISX].Enable)
		{
			printf("Error X Axis Disabled\n");
			MsgBox("Error X Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
		if (!chan[AXISY].Enable)
		{
			printf("Error Y Axis Disabled\n");
			MsgBox("Error Y Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;  //success
}

// Move Axis Z at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveZ(float z, float Speed)
{
	MoveAtVel(AXISZ, z * CNT_PER_MM_Z, Speed * CNT_PER_MM_Z);
	
	while (!CheckDone(AXISZ))
	{
		if (!chan[AXISZ].Enable)
		{
			printf("Error Z Axis Disabled\n");
			MsgBox("Error Z Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;  //success
}


Thanks a bunch! Again. Lol

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: C noob having a rough time tweaking the linear tool changer code

Post by TomKerekes » Fri Jul 15, 2022 9:30 pm

Regarding offset: Do you have exactly the same counts/inch in the KMotionCNC | Tool Setup | Trajectory Planner | Axis Parameters | Cnts/inch?

800 counts per mm would be 20320 counts per inch.

You might switch KMotionCNC DROs to machine coordinates to avoid any offset issues.

I see the DROs are shown in yellow. This normally indicates the axis are disabled. Are they? What is shown on the KMotion.exe Axis Screen?

Yes you must create a function to move only X if you need to do so. Copy the MoveZ functions and change all Z's to X's.
Regards,

Tom Kerekes
Dynomotion, Inc.

Post Reply