C noob having a rough time tweaking the linear tool changer code
Moderators: TomKerekes, dynomotion
C noob having a rough time tweaking the linear tool changer code
So I have 3 parking spots that are an almost perfect transplant for the provided linear tool change code.
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.
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.
- TomKerekes
- Posts: 2529
- Joined: Mon Dec 04, 2017 1:49 am
Re: C noob having a rough time tweaking the linear tool changer code
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.
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.
Tom Kerekes
Dynomotion, Inc.
Re: C noob having a rough time tweaking the linear tool changer code
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 and my tool file
Thanks for any advice. I'm pretty stuck on what else to try.
Here's the tool setup screen and my tool file
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
}
- TomKerekes
- Posts: 2529
- Joined: Mon Dec 04, 2017 1:49 am
Re: C noob having a rough time tweaking the linear tool changer code
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:
Then make a tool change to slot 1 again and tell us what is printed on the Console Screen.
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);
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
Re: C noob having a rough time tweaking the linear tool changer code
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... as the machine hits the Y limit switch.
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... as the machine hits the Y limit switch.
- TomKerekes
- Posts: 2529
- Joined: Mon Dec 04, 2017 1:49 am
Re: C noob having a rough time tweaking the linear tool changer code
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.
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.
Tom Kerekes
Dynomotion, Inc.
Re: C noob having a rough time tweaking the linear tool changer 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 -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
}
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 get a slightly different value from the print command on Y when I call tool1 but it is still comically incorrect. 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.
- TomKerekes
- Posts: 2529
- Joined: Mon Dec 04, 2017 1:49 am
Re: C noob having a rough time tweaking the linear tool changer code
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
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.
Tom Kerekes
Dynomotion, Inc.
Re: C noob having a rough time tweaking the linear tool changer code
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.
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
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
Thanks a bunch! Again. Lol
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
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
}
- TomKerekes
- Posts: 2529
- Joined: Mon Dec 04, 2017 1:49 am
Re: C noob having a rough time tweaking the linear tool changer code
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.
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.
Tom Kerekes
Dynomotion, Inc.