Ok I have spent a few days on this one and I am properly stuck. I am attempting to copy paste the Z probe function from Probemain into the linear4toolholder so that after picking up a tool the z homes back to zero and then moves over and touches off on the setter. I also copied the tooltableset code in after the zprobe in the hopes that after touching off the tool length will get measured and saved to the tool table. I got the tool change working perfectly before hacking this extra code in, but the probe code is a lot more complex and I can't seem to fix the compile errors by just copying more probe code over.
Code: Select all
#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "C:\KMotion435f\C Programs\KflopToKMotionCNCFunctions.c"
//-----------------------------------------
// LINEAR TOOL CHANGING
//-----------------------------------------
#define AXISX 0
#define AXISY 1
#define AXISZ 2
//---------Absolute position of tool holders
#define HOLDER_Y_1 -150
#define HOLDER_Y_2 -300
#define HOLDER_Y_3 -450
#define HOLDER_Y_4 -500
#define HOLDER_X -719
#define HOLDER_Z -169
// absolute position of the tool height setting plate
#define TOOL_HEIGHT_PLATE_X -642
#define TOOL_HEIGHT_PLATE_Y -568
// absolute position to move to that is permanently unobstructed, and safe to move down in Z
#define TOOL_CHANGE_SAFE_POS_X -647
#define TOOL_CHANGE_SAFE_POS_Y -50
#define TOOL_CHANGE_SAFE_POS_Z -80
#define AXIS_SAFE_DISTANCE_X -647 // 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 PROBE_ACTIVE 0 // triggered state = 0 or 1
#define SAFE_HEIGHT_Z -95 // 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 ZAXIS 2 // #114
#define SlowSpeed 18.0 //mm/sec
#define PROBESPEED 20.0f // #111 Probespeed in in/min
#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);
void FindZ();
int MoveDirection(int Direction, double Distance, int CheckProbe);
void Probe(int Direction);
// Globals
float xRes, yRes, zRes;
// float StockWidth, StockDepth;
float RESETDRO;
float FSpeed, SSpeed, MSpeed;
double STOCKWIDTH, STOCKDEPTH, ToUserUnits;
double nTouch, sTouch, wTouch, eTouch, tempX, tempY, Trig;
float CalcStockWidth, CalcStockDepth;
int Units, TWORD, HWORD, DWORD;
int Direction, Axis;
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)
{
if (ReadBit(TOOL_SENSE))
{
printf("Tool Sense Error\n");
MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
return 1;
}
DoPCInt(PC_COMM_GETAXISRES, TMP);
xRes = *(float *)&persist.UserData[TMP];
yRes = *(float *)&persist.UserData[TMP+1];
zRes = *(float *)&persist.UserData[TMP+2];
if (Units == CANON_UNITS_INCHES)
{
printf("Units are Inches\n");
ToUserUnits = 1.0;
}
else
{
printf("Units are millimeters\n");
ToUserUnits = 25.4;
}
// - move to Z Home to clear any work that may be on the table
if (MoveZ(0.0,SlowSpeed)) return 1;
// - 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 SAFE_HEIGHT_Z position at TOOL_RETRACT_SPEED_Z
if (MoveZ(SAFE_HEIGHT_Z,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
// - 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 low and TOOL_SENSE 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
if (MoveX(AXIS_SAFE_DISTANCE_X,SlowSpeed)) return 1;
// - Rapid to Z home
if (MoveZ(0.0,SlowSpeed)) return 1;
// - Move to position of tool setter
// - Rapid move to absolute position of tool setter only in X and Y
if (MoveXY(TOOL_HEIGHT_PLATE_X,TOOL_HEIGHT_PLATE_Y,SlowSpeed)) return 1;
// - Move to SAFE_HEIGHT_Z position at TOOL_RETRACT_SPEED_Z
if (MoveZ(SAFE_HEIGHT_Z,SlowSpeed)) return 1;
void FindZ()
// Find Z Height
Probe(5); // Find top of stock
MoveAtVel(ZAXIS, Trig, PROBESPEED * zRes / 60);
int FixtureIndex,Units, TWORD, HWORD, DWORD;
double NewToolLength,OriginOffsetZ,AxisOffsetZ;
double Machinex,Machiney,Machinez,Machinea,Machineb,Machinec;
GetMiscSettings(&Units, &TWORD, &HWORD, &DWORD);
GetFixtureIndex(&FixtureIndex);
GetOriginOffset(&OriginOffsetZ, FixtureIndex, Zaxis);
GetAxisOffset(&AxisOffsetZ, Zaxis);
GetMachine(&Machinex,&Machiney,&Machinez,&Machinea,&Machineb,&Machinec);
// Compute Tool Offset to make DRO zero when Tool Length selected and enabled
//
// Since Machine = DRO + OriginOffset + AxisOffset + ToolOffset
//
// Set DRO = 0 and solve for ToolOffset
//
NewToolLength = RoundToReasonable(Machinez - OriginOffsetZ - AxisOffsetZ,Units);
// Change Currently Selected Tool Length
SetToolLength(TWORD,NewToolLength);
}
return 0; //success
}
// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{
if (!ReadBit(TOOL_SENSE))
{
printf("Tool Sense Error\n");
MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
return 1;
}
// - move to Z Home to clear any work that may be on the table
if (MoveZ(0.0,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;
// - Approach tool holder by matching Z height of tool holder
if (MoveZ(HOLDER_Z,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 low, 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 to SAFE_HEIGHT_Z position at TOOL_RETRACT_SPEED_Z
if (MoveZ(SAFE_HEIGHT_Z,SlowSpeed)) return 1;
// - Turn off CLAW_EJECT bit to save air
ClearBit(CLAW_EJECT);
// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z safe height
if (MoveZ(TOOL_CHANGE_SAFE_POS_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 -150*tool;
}
// 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-4 = valid tool
BOOL ToolNumberValid(int tool)
{
return tool == -1 || (tool>=1 && tool<=4);
}
// 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
}
// Move Axis X at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveX(float x, float Speed)
{
MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
while (!CheckDone(AXISX))
{
if (!chan[AXISX].Enable)
{
printf("Error X Axis Disabled\n");
MsgBox("Error X Axis Disabled\n", MB_ICONHAND | MB_OK);
return 1;
}
}
return 0; //success
}
int MoveDirection(int Direction, double Distance, int CheckProbe)
{
double tmpDist;
char* cDir;
if (Direction == 1)
{
MSpeed = MOVESPEED * yRes / 60;
tmpDist = Distance * yRes;
Axis = YAXIS;
cDir = "North";
}
if (Direction == 2)
{
MSpeed = MOVESPEED * xRes / 60;
tmpDist = Distance * xRes;
Axis = XAXIS;
cDir = "East";
}
if (Direction == 3)
{
MSpeed = MOVESPEED * yRes / 60;
tmpDist = Distance * yRes * -1;
Axis = YAXIS;
cDir = "South";
}
if (Direction == 4)
{
MSpeed = MOVESPEED * xRes / 60;
tmpDist = Distance * xRes * -1;
Axis = XAXIS;
cDir = "West";
}
if (Direction == 5)
{
MSpeed = MOVESPEED * zRes / 60;
tmpDist = Distance * zRes * -1;
Axis = ZAXIS;
cDir = "Down";
}
if (Direction == 6)
{
MSpeed = MOVESPEED * zRes / 60;
tmpDist = Distance * zRes;
Axis = ZAXIS;
cDir = "Up";
}
printf("Moving %s ", cDir);
printf("%f mm\n", Distance * ToUserUnits);
if (CheckProbe == 0)
{
MoveRelAtVel(Axis, tmpDist, MSpeed);
while (!CheckDone(Axis));
return 0;
}
else
{
MoveRelAtVel(Axis, tmpDist, MSpeed);
while (!CheckDone(Axis) && ReadBit(PROBEBIT) != PROBE_ACTIVE);
if (ReadBit(PROBEBIT) == PROBE_ACTIVE)
{
Jog(Axis,0);
MsgBox("Probe hit something!",MB_OK|MB_ICONEXCLAMATION);
return 1;
}
else
return 0;
}
}
void Probe(int Direction)
{
if (Direction == 1)
{
FSpeed = PROBESPEED * yRes / 60;
SSpeed = SLOWSPEED * yRes / 60;
Axis = YAXIS;
}
if (Direction == 2)
{
FSpeed = PROBESPEED * xRes / 60;
SSpeed = SLOWSPEED * xRes / 60;
Axis = XAXIS;
}
if (Direction == 3)
{
FSpeed = PROBESPEED * -1.0f * yRes / 60;
SSpeed = SLOWSPEED * -1.0f * yRes / 60;
Axis = YAXIS;
}
if (Direction == 4)
{
FSpeed = PROBESPEED * -1.0f * xRes / 60;
SSpeed = SLOWSPEED * -1.0f * xRes / 60;
Axis = XAXIS;
}
if (Direction == 5)
{
FSpeed = PROBESPEED * -1.0f * zRes / 60;
SSpeed = SLOWSPEED * -1.0f * zRes / 60;
Axis = ZAXIS;
}
printf("Jogging at speed:%f\n", FSpeed);
Jog(Axis, FSpeed);
printf("Waiting for probe to activate|\n");
while (ReadBit(PROBEBIT) != PROBE_ACTIVE);
printf("Stopping\n");
Jog(Axis, 0);
printf("Waiting for motion to stop\n");
while (!CheckDone(Axis));
printf("Jogging at speed:%f\n", -SSpeed);
Jog(Axis, -SSpeed);
printf("Waiting for probe to reset\n");
while (ReadBit(PROBEBIT) == PROBE_ACTIVE);
printf("Stopping\n");
Jog(Axis, 0);
printf("Waiting for motion to stop\n");
while (!CheckDone(Axis));
printf("Jogging at speed:%f\n", SSpeed);
Jog(Axis, SSpeed);
printf("Waiting for probe to activate\n");
while (ReadBit(PROBEBIT) != PROBE_ACTIVE);
Trig = chan[Axis].Dest;
printf("Stopping\n");
Jog(Axis, 0);
printf("Waiting for motion to stop\n");
while (!CheckDone(Axis));
}
I get a declaration list expected for the probe line, I tried to copy all the relevant defines and such but I clearly am still missing something for the probe code to compile
