Difference between revisions of "Main Page"

From Dynomotion

Jump to: navigation, search
(→‎Tool Changer - router linear 4 Tools - C Program)
(→‎Tool Changer - router linear 4 Tools - C Programnone|link=|246x178px)
Line 141: Line 141:
 
:* Place links to pages that explain how you accomplished your particular project.  Write clear explanations that provide background on what you did and how you did it.
 
:* Place links to pages that explain how you accomplished your particular project.  Write clear explanations that provide background on what you did and how you did it.
 
====[[Tool Changer - router linear 4 Tools - C Program]][[File:LINEAT+ATC.jpg|none|link=|246x178px]]====
 
====[[Tool Changer - router linear 4 Tools - C Program]][[File:LINEAT+ATC.jpg|none|link=|246x178px]]====
 +
<pre class="brush:c">#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_X_1 100
 +
#define HOLDER_X_2 200
 +
#define HOLDER_X_3 300
 +
#define HOLDER_X_4 400
 +
#define HOLDER_Y 1300
 +
#define HOLDER_Z -200
 +
// absolute position of the tool height setting plate
 +
#define TOOL_HEIGHT_PLATE_X 50
 +
#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 250 
 +
#define TOOL_CHANGE_SAFE_POS_Y 1200
 +
 +
#define AXIS_SAFE_DISTANCE_Y 100  // distance in mm to approach tool holder
 +
//---------
 +
 +
//--------- Spindle IO bits
 +
#define CLAW_EJECT 58 // IO bit to eject tool from spindle (KONNECT OUTPUT 10)
 +
#define SPINDLE_CLEAN 59 // IO bit to blow air out of spindle taper (KONNECT OUTPUT 11)
 +
#define CLAW_LOOSE 1048 // IO bit to sense whether the claw has ejected (KONNECT INPUT 24)
 +
#define TOOL_SENSE 1049 // IO bit to sense whether the a tool is in the spindle (KONNECT INPUT 24)
 +
//---------
 +
 +
#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 1.0    // seconds to wait for the clamp/unclamp
 +
#define TOOL_HEIGHT_BIT 1055 //bit to read tool height plate (KONNECT INPUT 31)
 +
 +
#define SAFE_HEIGHT_Z 100  // relative distance in mm to move to clear the top of the tool taper
 +
#define TOOL_RETRACT_SPEED_Z 5000 //speed in counts/second to move spindle up after tool has been ejected
 +
 +
#define SlowSpeed 10.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(ToolPositionX(Tool),HOLDER_Y,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 and SPINDLE_CLEAN bits are currently high from tool removal operation
 +
// - Turn off CLAW_EJECT and SPINDLE_CLEAN bits to engage tool
 +
ClearBit(CLAW_EJECT);
 +
ClearBit(SPINDLE_CLEAN);
 +
 +
// - 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 Y axis by the negative value of Y_AXIS_SAFE_DISTANCE
 +
// - Move to position of requested tool
 +
// - Rapid move to absolute position of new tool only in X and Y
 +
if (MoveXY(ToolPositionX(Tool),HOLDER_Y+AXIS_SAFE_DISTANCE_Y,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 X position
 +
if (MoveXY(ToolPositionX(CurrentTool),TOOL_CHANGE_SAFE_POS_Y,SlowSpeed)) return 1;
 +
 +
// - After matching X position, match tool Y position
 +
if (MoveXY(ToolPositionX(CurrentTool),HOLDER_Y,SlowSpeed)) return 1;
 +
 +
// - Move only in Y position until current position matches tool holder position (maybe disable X)                          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);
 +
 +
// - Turn on SPINDLE_CLEAN bit to remove any debris from taper and tools
 +
SetBit(SPINDLE_CLEAN);
 +
 +
// - 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 x position of tool holder as a function of the tool
 +
float ToolPositionX(int tool)
 +
{
 +
return (HOLDER_X_2-HOLDER_X_1)*(tool-1) + HOLDER_X_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-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)
 +
{
 +
MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
 +
MoveAtVel(AXISZ, 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
 +
}
 +
</pre>

Revision as of 12:10, 3 November 2015

Dynomotion wiki

Welcome to our wiki where you can find and share knowledge on Dynomotion's systems.  To begin adding information to this wiki, simply create an account, verify your email, and start editing (click pencil icon). To make editing pages easier for everyone, we have installed a visual editor by default.  This visual editor allows you to edit a wiki page much like you would a standard word processing document.  For the seasoned wiki editors, standard MediaWiki editing may also be used.


Warning: Use at your own risk.  Information is for example purposes only and may contain errors.  It is up to the User to verify the information is correct and is safe to use. In no case will any contributor or Dynomotion be liable for incorrect information.  Dynomotion reserves the right to modify or delete any contributions.


Thanks for Contributing!

Official Dynomotion Site

Editing and Creating Wiki Pages

Unintuitively, you first create a link to the page you want to make before you can make the page.  To make a new page: while editing, right-click where you want your link to be, insert a link with a descriptive title and link text, save the page, click on the link you created, then click on "Empty Page" to begin editing your new page.

Here are a few other quick tips:

  • You can insert links or images by Right-Clicking with the mouse on the line where you want the link or image to appear.
  • For Spell Correction you can use Ctrl-Right-Click. 
  • To insert a YouTube Video use an EmbedVideo command such as  {{#ev:youtube|xxxxx}} with xxxxx changed to the Video ID.
  • Choose a descriptive page name and link text when creating a new page: e.g. Not "My Machine", but rather "Brand X Three Axis Milling Machine Retrofit"
  • Create pages in an organized manner: Describe what the page is about, create clear sections that flow

For more information:

How to create a new wiki page

How to Edit and insert Media into your wiki pages

Dynomotion Software Topics

Installation Topics


KMotion.exe Executive Software Topics

KMotion.exe is the main program that is used to configure settings in KFLOP, initialize axes, and to write C programs to configure and setup KFLOP to control your system. In this program you can plot step response, test movement, view Bode plots, monitor I/O, set filter parameters, and generally set the pertinent parameters.


Configuration Topics

Channels, Channels, Channels what are they?

Read Article Here

KMotionCNC Software Topics

KMotionCNC is Dynomotion's CNC program used as a graphical user interface to your KFLOP-enabled machine tool. KMotionCNC has all of the basic functionality you need to run a machine of up to 6 axes, in addition to spindle control.

  • Insert pages for other KMotionCNC-related general topics here


Using KMotionCNC

  • Homing discussion - page to be created
  • Insert other KMotionCNC use pages here


Customize KMotionCNC

KMotionCNC is written as a Windows C++ MFC (Microsoft Foundation Classes) program developed in Visual Studio.  Source code is provided and may be customized using Visual Studio.  MFC Support is a requirement. 

General Information
Compiling KMotionCNC
The KMotionCNC's Visual Studio Project Solution (\PC VC Programs\KMotionCNC\KMotionCNC.sln) is currently written for Visual Studio 2008 Standard.  This version can be used for the simplest compatibility.  Projects can be upgraded to newer version of VS with minimal effort.  Including Microsoft's Free Visual Studio 2013 Community.  More info on PC Example Applications.
KMotionCNC Customization Examples and Applications
Links to examples of projects that explain KMotionCNC customizations
Troy (tmday7) created some helpful documents listed on the Yahoo Group Files Section here.

PC Example Applications

A number of PC Applications using the KMotion Libraries are available in the Software download.  Visual Studio should be used to modify/compile the applications. 

Currently the projects are compatible with VS 2008 Standard but later Versions can upgrade the projects and can be used including the free Microsoft Version of VS 2013 Community.  In some cases MFC capability needs to be added as a separate download.

More information.


KFLOP C Programs

C Programs provide a powerful and flexible capability to perform almost any sequence of operations within KFLOP.   In most cases after you have tested and tuned all your hardware using the KMotion.exe setup program all the settings and initialization steps required for your system can be placed into an Initialization C Program so that your system can be fully initialized simply by executing the Initialization program.

More Information

Dynomotion Hardware Topics

General Hardware Information

KFLOP Hardware Info

KStep Hardware Info

Kanalog Hardware Info

SnapAmp Hardware Info

Konnect Hardware Info


Wiring Diagrams

Basic Kanalog DAC and Encoder Connections 3 Axis

Basic Kanalog DAC and Encoder Connections 3 Axis #2

KFLOP+SnapAmp DC Brush Motors with MPG

KFLOP JP5 Open Collector Step/Dir Connections to Power Step PSD5042-2P Drives


Wiring Topics

Place links to pages on wiring inputs and outputs specific to your experiences and projects.  Be descriptive with page titles and links.


MPG Wiring and Interface

MPGs (Manual Pulse Generators) should be connected directly to KFLOP for guaranteed real-time response (not USB based or connected to the PC).  MPGs are handled by a C Program that monitors the MPG and creates motion based on the MPG Encoder changes and switch selections for axis, speed, and so forth.  See the MPG C Program Examples.  Here is a Discussion with other links.

https://groups.yahoo.com/neo/groups/DynoMotion/conversations/messages/12287

MPG4 from cnc4pc.jpg

Axes Servo Tuning and Trajectory Planner

Velocity, Acceleration, and Jerk

The Velocity, Acceleration, and Jerk in the Step Response Screen (KFLOP parameter settings) and the Acceleration and Velocity in the KMotionCNC | Tool Setup | Trajectory Planner | Axis Parameters are both used for different things.  The two sets of parameters are independent. 
 
The KFLOP parameters are used for 3rd order motions.   These include things like:  Jogging, Homing, and GCode Rapids (G0).  The units in KFLOP are in counts or steps.

The KMotionCNC parameters are used for 2nd order (infinite Jerk) coordinated motion paths.  These are GCode G1,G2,G3 continuous paths.  The units are in Inches (or in some cases degrees).

Tests to find optimal settings for both types of moves should be made using the Step Response Screen.  The Step Response Screen always performs 3rd order motion but 2nd order motion can be simulated by temporarily setting the Jerk to a huge value (1000X the acceleration value).  In general reducing the Jerk value will result in longer times to perform motions, but often the improved smoothness will permit higher maximum accelerations and velocities to be used resulting in overall shorter motion times.

Make sure when testing the size of move is long enough for full acceleration and velocity are achieved.  As a common mistake is to have Acceleration or Velocity Settings set to too high for your system but when testing a short move there is no indication of a problem.  The plot mode of Velocity Output  vs Time can be helpful to verify full Velocity is being achieved.

Noisy GCode - Trajectory Planner Smoothing

An analysis of Quantization Noise in GCode and Trajectory Planner Settings to generate smooth motion.
GCode Noisy Path.png
See Analysis and Solution

Problems and Resolutions

General

  • Dealing with noise on inputs.  If you experience issues with inputs misreading, the issue may be noise.  This page provides examples on what might be causing noise issues and examples of how to possibly deal with noise.
  • Other

Software-Specific Problems and Resolutions

  • Place links to pages explaining resolutions to problems that are largely software-related here

Hardware-Specific Problems and Resolutions

  • Place links to pages explaining resolutions to problems that are largely hardware-related

Applications and Projects

  • Place links to pages that explain how you accomplished your particular project.  Write clear explanations that provide background on what you did and how you did it.

Tool Changer - router linear 4 Tools - C Program
LINEAT+ATC.jpg

#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_X_1 100
#define HOLDER_X_2 200
#define HOLDER_X_3 300
#define HOLDER_X_4 400
#define HOLDER_Y 1300
#define HOLDER_Z -200
// absolute position of the tool height setting plate
#define TOOL_HEIGHT_PLATE_X 50 
#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 250  
#define TOOL_CHANGE_SAFE_POS_Y 1200

#define AXIS_SAFE_DISTANCE_Y 100  // distance in mm to approach tool holder
//---------

//--------- Spindle IO bits
#define CLAW_EJECT 58		// IO bit to eject tool from spindle (KONNECT OUTPUT 10)
#define SPINDLE_CLEAN 59	// IO bit to blow air out of spindle taper (KONNECT OUTPUT 11)
#define CLAW_LOOSE 1048		// IO bit to sense whether the claw has ejected (KONNECT INPUT 24)
#define TOOL_SENSE 1049		// IO bit to sense whether the a tool is in the spindle (KONNECT INPUT 24)
//---------

#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 1.0    	// seconds to wait for the clamp/unclamp
#define TOOL_HEIGHT_BIT	1055	//bit to read tool height plate (KONNECT INPUT 31)

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

#define SlowSpeed 10.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(ToolPositionX(Tool),HOLDER_Y,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 and SPINDLE_CLEAN bits are currently high from tool removal operation
	// - Turn off CLAW_EJECT and SPINDLE_CLEAN bits to engage tool
	ClearBit(CLAW_EJECT);
	ClearBit(SPINDLE_CLEAN);

	// - 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 Y axis by the negative value of Y_AXIS_SAFE_DISTANCE
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(ToolPositionX(Tool),HOLDER_Y+AXIS_SAFE_DISTANCE_Y,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 X position
	if (MoveXY(ToolPositionX(CurrentTool),TOOL_CHANGE_SAFE_POS_Y,SlowSpeed)) return 1;

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

	// - Move only in Y position until current position matches tool holder position (maybe disable X)                          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);

	// - Turn on SPINDLE_CLEAN bit to remove any debris from taper and tools
	SetBit(SPINDLE_CLEAN);

	// - 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 x position of tool holder as a function of the tool
float ToolPositionX(int tool)
{
	return (HOLDER_X_2-HOLDER_X_1)*(tool-1) + HOLDER_X_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-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)
{
	MoveAtVel(AXISX, x * CNT_PER_MM_X, Speed * CNT_PER_MM_X);
	MoveAtVel(AXISZ, 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
}
Retrieved from "https://www.dynomotion.com/wiki/index.php?title=Main_Page&oldid=215"