If it still doesn't work as intended see diff between this and old patch
of file canon.cc.
There are only 2 places (you found it earler ) that can be changed
further to satisfy your configuration.
regards,
Michael
>From 3134ab2b2ecf909e46c7b563a7389d56473bb0b7 Mon Sep 17 00:00:00 2001
From: Michael Geszkiewicz <[email protected]>
Date: Mon, 27 Jul 2009 16:00:23 +0200
Subject: [PATCH] Rach tool changer
Signed-off-by: Michael Geszkiewicz <[email protected]>
---
configs/sim/axis_mm.ini | 9 +++-
src/emc/ini/initool.cc | 22 +++++++++
src/emc/iotask/ioControl.cc | 40 ++++++++++++++++
src/emc/nml_intf/emc.cc | 34 +++++++++++++
src/emc/nml_intf/emc.hh | 4 ++
src/emc/nml_intf/emc_nml.hh | 24 ++++++++++
src/emc/nml_intf/emcglb.c | 2 +
src/emc/nml_intf/emcglb.h | 3 +
src/emc/task/emccanon.cc | 108 +++++++++++++++++++++++++++++++++++++++++--
src/emc/task/emctaskmain.cc | 26 ++++++++++
src/emc/task/iotaskintf.cc | 22 +++++++++
11 files changed, 289 insertions(+), 5 deletions(-)
diff --git a/configs/sim/axis_mm.ini b/configs/sim/axis_mm.ini
index ff96de2..6b3cecc 100644
--- a/configs/sim/axis_mm.ini
+++ b/configs/sim/axis_mm.ini
@@ -207,4 +207,11 @@ CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = sim_mm.tbl
-TOOL_CHANGE_POSITION = 0 0 50.8
+
+TOOL_CHANGER_TYPE = 1
+
+TOOL_CHANGE_POSITION = 100 100 50
+
+TOOL_SLOT_DELTA = 0 10 0
+
+TOOL_HOLDER_CLEAR = 100 300 100
diff --git a/src/emc/ini/initool.cc b/src/emc/ini/initool.cc
index 5048132..02d548b 100644
--- a/src/emc/ini/initool.cc
+++ b/src/emc/ini/initool.cc
@@ -136,6 +136,28 @@ static int readToolChange(IniFile *toolInifile)
HAVE_TOOL_HOLDER_CLEAR = 0;
}
+ if (NULL !=
+ (inistring = toolInifile->Find("TOOL_SLOT_DELTA", "EMCIO"))) {
+ /* found an entry */
+ if (3 == sscanf(inistring, "%lf %lf %lf",
+ &TOOL_SLOT_DELTA.tran.x,
+ &TOOL_SLOT_DELTA.tran.y,
+ &TOOL_SLOT_DELTA.tran.z)) {
+ /* read them OK */
+ TOOL_SLOT_DELTA.a = 0.0; // not supporting ABC for now
+ TOOL_SLOT_DELTA.b = 0.0;
+ TOOL_SLOT_DELTA.c = 0.0;
+ retval = 0;
+ } else {
+ /* bad format */
+ rcs_print("bad format for TOOL_SLOT_DELTA\n");
+ retval = -1;
+ }
+ }
+
+ tool_changer_type = 0;
+ toolInifile->Find(&tool_changer_type, "TOOL_CHANGER_TYPE", "EMCIO");
+
return retval;
}
diff --git a/src/emc/iotask/ioControl.cc b/src/emc/iotask/ioControl.cc
index 5122a1c..06d1f4f 100644
--- a/src/emc/iotask/ioControl.cc
+++ b/src/emc/iotask/ioControl.cc
@@ -96,6 +96,8 @@ struct iocontrol_str {
hal_bit_t *tool_change; /* output, notifies a tool-change should happen (emc should be in the tool-change position) */
hal_bit_t *tool_changed; /* input, notifies tool has been changed */
+ hal_bit_t *tool_locked; /* tool holder lock */
+ hal_bit_t *tool_flush; /* tool holder flush */
// note: spindle control has been moved to motion
} * iocontrol_data; //pointer to the HAL-struct
@@ -622,6 +624,29 @@ int iocontrol_hal_init(void)
hal_exit(comp_id);
return -1;
}
+
+ rtapi_snprintf(name, HAL_NAME_LEN, "iocontrol.%d.tool-locked", n);
+ retval =
+ hal_pin_bit_new(name, HAL_OUT, &(iocontrol_data->tool_locked), comp_id);
+ if (retval != HAL_SUCCESS) {
+ rtapi_print_msg(RTAPI_MSG_ERR,
+ "IOCONTROL: ERROR: iocontrol %d pin tool-locked export failed with err=%i\n",
+ n, retval);
+ hal_exit(comp_id);
+ return -1;
+ }
+
+ rtapi_snprintf(name, HAL_NAME_LEN, "iocontrol.%d.tool-flush", n);
+ retval =
+ hal_pin_bit_new(name, HAL_OUT, &(iocontrol_data->tool_flush), comp_id);
+ if (retval != HAL_SUCCESS) {
+ rtapi_print_msg(RTAPI_MSG_ERR,
+ "IOCONTROL: ERROR: iocontrol %d pin tool-flush export failed with err=%i\n",
+ n, retval);
+ hal_exit(comp_id);
+ return -1;
+ }
+
/* STEP 3b: export the in-pin(s) */
// emc-enable-in
@@ -986,6 +1011,21 @@ int main(int argc, char *argv[])
}
break;
+ case EMC_TOOL_SET_LOCK_TYPE:
+ {
+ char mode = ((EMC_TOOL_SET_LOCK *) emcioCommand)->mode;
+ rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_SET_LOCK mode=%d\n", mode);
+ *(iocontrol_data->tool_locked) = mode;
+ }
+ break;
+
+ case EMC_TOOL_SET_FLUSH_TYPE:
+ {
+ char mode = ((EMC_TOOL_SET_FLUSH *) emcioCommand)->mode;
+ rtapi_print_msg(RTAPI_MSG_DBG, "EMC_TOOL_SET_FLUSH mode=%d\n", mode);
+ *(iocontrol_data->tool_flush) = mode;
+ }
+ break;
case EMC_COOLANT_MIST_ON_TYPE:
rtapi_print_msg(RTAPI_MSG_DBG, "EMC_COOLANT_MIST_ON\n");
diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc
index e95cc4c..bf67d56 100644
--- a/src/emc/nml_intf/emc.cc
+++ b/src/emc/nml_intf/emc.cc
@@ -341,6 +341,12 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms)
case EMC_TOOL_SET_NUMBER_TYPE:
((EMC_TOOL_SET_NUMBER *) buffer)->update(cms);
break;
+ case EMC_TOOL_SET_LOCK_TYPE:
+ ((EMC_TOOL_SET_LOCK *) buffer)->update(cms);
+ break;
+ case EMC_TOOL_SET_FLUSH_TYPE:
+ ((EMC_TOOL_SET_FLUSH *) buffer)->update(cms);
+ break;
case EMC_TOOL_STAT_TYPE:
((EMC_TOOL_STAT *) buffer)->update(cms);
break;
@@ -665,6 +671,10 @@ const char *emc_symbol_lookup(long type)
return "EMC_TOOL_SET_OFFSET";
case EMC_TOOL_SET_NUMBER_TYPE:
return "EMC_TOOL_SET_NUMBER";
+ case EMC_TOOL_SET_LOCK_TYPE:
+ return "EMC_TOOL_SET_LOCK";
+ case EMC_TOOL_SET_FLUSH_TYPE:
+ return "EMC_TOOL_SET_FLUSH";
case EMC_TOOL_STAT_TYPE:
return "EMC_TOOL_STAT";
case EMC_TOOL_UNLOAD_TYPE:
@@ -1444,6 +1454,30 @@ void EMC_TOOL_SET_NUMBER::update(CMS * cms)
}
/*
+* NML/CMS Update function for EMC_TOOL_SET_LOCK
+* Manually generated by Michal Geszkiewicz
+* on 09.02.2009 12:00:22
+*/
+void EMC_TOOL_SET_LOCK::update(CMS * cms)
+{
+
+ EMC_TOOL_CMD_MSG::update(cms);
+ cms->update(mode);
+}
+
+/*
+* NML/CMS Update function for EMC_TOOL_SET_LOCK
+* Manually generated by Michal Geszkiewicz
+* on 09.02.2009 12:00:22
+*/
+void EMC_TOOL_SET_FLUSH::update(CMS * cms)
+{
+
+ EMC_TOOL_CMD_MSG::update(cms);
+ cms->update(mode);
+}
+
+/*
* NML/CMS Update function for EMC_SPINDLE_STAT
* Automatically generated by NML CodeGen Java Applet.
* on Sat Oct 11 13:45:16 UTC 2003
diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh
index fd1b152..950c53d 100644
--- a/src/emc/nml_intf/emc.hh
+++ b/src/emc/nml_intf/emc.hh
@@ -187,6 +187,8 @@ class PM_CARTESIAN;
#define EMC_TOOL_LOAD_TOOL_TABLE_TYPE ((NMLTYPE) 1107)
#define EMC_TOOL_SET_OFFSET_TYPE ((NMLTYPE) 1108)
#define EMC_TOOL_SET_NUMBER_TYPE ((NMLTYPE) 1109)
+#define EMC_TOOL_SET_LOCK_TYPE ((NMLTYPE) 1110)
+#define EMC_TOOL_SET_FLUSH_TYPE ((NMLTYPE) 1111)
#define EMC_TOOL_STAT_TYPE ((NMLTYPE) 1199)
@@ -516,6 +518,8 @@ extern int emcToolSetOffset(int id, double zoffset, double xoffset,
double diameter, double frontangle,
double backangle, int orientation);
extern int emcToolSetNumber(int number);
+extern int emcToolSetLock(int mode);
+extern int emcToolSetFlush(int mode);
extern int emcToolSetToolTableFile(const char *file);
diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh
index 2acbb51..932cd6e 100644
--- a/src/emc/nml_intf/emc_nml.hh
+++ b/src/emc/nml_intf/emc_nml.hh
@@ -1511,6 +1511,30 @@ class EMC_TOOL_SET_NUMBER:public EMC_TOOL_CMD_MSG {
int tool; //number to use for currently loaded tool
};
+class EMC_TOOL_SET_LOCK:public EMC_TOOL_CMD_MSG {
+ public:
+ EMC_TOOL_SET_LOCK():EMC_TOOL_CMD_MSG(EMC_TOOL_SET_LOCK_TYPE,
+ sizeof(EMC_TOOL_SET_LOCK)) {
+ };
+
+ // For internal NML/CMS use only.
+ void update(CMS * cms);
+
+ int mode; // lock unlock
+};
+
+class EMC_TOOL_SET_FLUSH:public EMC_TOOL_CMD_MSG {
+ public:
+ EMC_TOOL_SET_FLUSH():EMC_TOOL_CMD_MSG(EMC_TOOL_SET_FLUSH_TYPE,
+ sizeof(EMC_TOOL_SET_FLUSH)) {
+ };
+
+ // For internal NML/CMS use only.
+ void update(CMS * cms);
+
+ int mode; // lock unlock
+};
+
// EMC_TOOL status base class
class EMC_TOOL_STAT_MSG:public RCS_STAT_MSG {
public:
diff --git a/src/emc/nml_intf/emcglb.c b/src/emc/nml_intf/emcglb.c
index 03125ea..33c4fb7 100644
--- a/src/emc/nml_intf/emcglb.c
+++ b/src/emc/nml_intf/emcglb.c
@@ -52,6 +52,8 @@ EmcPose TOOL_CHANGE_POSITION; /* no defaults */
unsigned char HAVE_TOOL_CHANGE_POSITION = 0; /* default is 'not there' */
EmcPose TOOL_HOLDER_CLEAR; /* no defaults */
unsigned char HAVE_TOOL_HOLDER_CLEAR; /* default is 'not there' */
+EmcPose TOOL_SLOT_DELTA;
+int tool_changer_type;
int taskplanopen = 0;
diff --git a/src/emc/nml_intf/emcglb.h b/src/emc/nml_intf/emcglb.h
index fc971af..5803a22 100644
--- a/src/emc/nml_intf/emcglb.h
+++ b/src/emc/nml_intf/emcglb.h
@@ -76,6 +76,9 @@ extern "C" {
extern struct EmcPose TOOL_HOLDER_CLEAR;
extern unsigned char HAVE_TOOL_HOLDER_CLEAR;
+ extern struct EmcPose TOOL_SLOT_DELTA;
+ extern int tool_changer_type;
+
#define DEFAULT_EMCLOG_INCLUDE_HEADER (1)
extern int EMCLOG_INCLUDE_HEADER;
diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc
index 593aece..07a405e 100644
--- a/src/emc/task/emccanon.cc
+++ b/src/emc/task/emccanon.cc
@@ -1667,15 +1667,71 @@ void USE_TOOL_LENGTH_OFFSET(double xoffset, double zoffset, double woffset)
interp_list.append(set_offset_msg);
}
+void generate_toolchange_move(double x, double y, double z,
+ double a, double b, double c,
+ double u, double v, double w)
+{
+ EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
+ double vel = getStraightVelocity(x, y, z, a, b, c, u, v, w);
+ double acc = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
+ linearMoveMsg.end = to_ext_pose(x, y, z, a, b, c, u, v, w);
+
+ linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel);
+ linearMoveMsg.acc = toExtAcc(acc);
+ linearMoveMsg.type = EMC_MOTION_TYPE_TOOLCHANGE;
+ linearMoveMsg.feed_mode = 0;
+ if(vel && acc)
+ interp_list.append(linearMoveMsg);
+ canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
+}
+
+void tool_holder_clear_move(void)
+{
+ double a, b, c, u, v, w;
+ a = b = c = u = v = w = 0;
+
+ if (HAVE_TOOL_HOLDER_CLEAR) {
+ generate_toolchange_move(
+ canonEndPoint.x, canonEndPoint.y, FROM_EXT_LEN(TOOL_HOLDER_CLEAR.tran.z),
+ a, b, c, u, v, w);
+ generate_toolchange_move(
+ FROM_EXT_LEN(TOOL_HOLDER_CLEAR.tran.x), canonEndPoint.y, canonEndPoint.z,
+ a, b, c, u, v, w);
+ generate_toolchange_move(
+ canonEndPoint.x, FROM_EXT_LEN(TOOL_HOLDER_CLEAR.tran.y), canonEndPoint.z,
+ a, b, c, u, v, w);
+ }
+}
+
+void tool_getput_move(int slot)
+{
+ double a, b, c, u, v, w;
+ a = b = c = u = v = w = 0;
+
+ generate_toolchange_move(
+ FROM_EXT_LEN(TOOL_CHANGE_POSITION.tran.x), canonEndPoint.y, canonEndPoint.z,
+ a, b, c, u, v, w);
+
+ generate_toolchange_move(canonEndPoint.x,
+ FROM_EXT_LEN(TOOL_CHANGE_POSITION.tran.y) + slot*FROM_EXT_LEN(TOOL_SLOT_DELTA.tran.y),
+ canonEndPoint.z, a, b, c, u, v, w);
+
+ generate_toolchange_move(
+ canonEndPoint.x, canonEndPoint.y, FROM_EXT_LEN(TOOL_CHANGE_POSITION.tran.z),
+ a, b, c, u, v, w);
+}
+
/* CHANGE_TOOL results from M6, for example */
void CHANGE_TOOL(int slot)
{
- EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
- linearMoveMsg.feed_mode = feed_mode;
- EMC_TOOL_LOAD load_tool_msg;
+ if (tool_changer_type == 0) {
+ EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
+ linearMoveMsg.feed_mode = feed_mode;
+ EMC_TOOL_LOAD load_tool_msg;
- flush_segments();
+ flush_segments();
+
/* optional move to tool change position. This
* is a mess because we really want a configurable chain
* of events to happen when a tool change is called for.
@@ -1724,6 +1780,50 @@ void CHANGE_TOOL(int slot)
/* regardless of optional moves above, we'll always send a load tool
message */
interp_list.append(load_tool_msg);
+
+ } else if (tool_changer_type == 1) {
+ EMC_TOOL_LOAD load_tool_msg;
+ EMC_TOOL_SET_LOCK lock_tool_msg;
+ EMC_TOOL_SET_FLUSH flush_tool_msg;
+ int last_slot = emcStatus->io.tool.toolInSpindle;
+
+ flush_segments();
+
+ tool_holder_clear_move();
+ DWELL(0);
+
+ if (last_slot > 0) {
+ tool_getput_move(last_slot);
+ lock_tool_msg.mode = 1;
+ interp_list.append(lock_tool_msg);
+ DWELL(0.5);
+ tool_holder_clear_move();
+ DWELL(0);
+ }
+ tool_getput_move(slot);
+
+ lock_tool_msg.mode = 1;
+ interp_list.append(lock_tool_msg);
+
+ flush_tool_msg.mode = 1;
+ interp_list.append(flush_tool_msg);
+ DWELL(1);
+ flush_tool_msg.mode = 0;
+ interp_list.append(flush_tool_msg);
+
+ generate_toolchange_move(
+ canonEndPoint.x, canonEndPoint.y, canonEndPoint.z - 6, 0, 0, 0, 0, 0, 0);
+
+ lock_tool_msg.mode = 0;
+ interp_list.append(lock_tool_msg);
+ DWELL(0.5);
+
+ /* regardless of optional moves above, we'll always send a load tool
+ message */
+ interp_list.append(load_tool_msg);
+
+ tool_holder_clear_move();
+ }
}
/* SELECT_TOOL results from T1, for example */
diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc
index f5642c8..dd6dea4 100644
--- a/src/emc/task/emctaskmain.cc
+++ b/src/emc/task/emctaskmain.cc
@@ -358,6 +358,8 @@ static EMC_TOOL_PREPARE *tool_prepare_msg;
static EMC_TOOL_LOAD_TOOL_TABLE *load_tool_table_msg;
static EMC_TOOL_SET_OFFSET *emc_tool_set_offset_msg;
static EMC_TOOL_SET_NUMBER *emc_tool_set_number_msg;
+static EMC_TOOL_SET_LOCK *emc_tool_set_lock_msg;
+static EMC_TOOL_SET_FLUSH *emc_tool_set_flush_msg;
static EMC_TASK_SET_MODE *mode_msg;
static EMC_TASK_SET_STATE *state_msg;
static EMC_TASK_PLAN_RUN *run_msg;
@@ -595,6 +597,11 @@ static int emcTaskPlan(void)
emcTaskQueueCommand(&taskPlanSynchCmd);
break;
+ case EMC_TOOL_SET_LOCK_TYPE:
+ case EMC_TOOL_SET_FLUSH_TYPE:
+ emcTaskQueueCommand(emcCommand);
+ break;
+
default:
emcOperatorError(0,
_
@@ -709,6 +716,8 @@ static int emcTaskPlan(void)
break;
case EMC_TOOL_SET_NUMBER_TYPE:
+ case EMC_TOOL_SET_LOCK_TYPE:
+ case EMC_TOOL_SET_FLUSH_TYPE:
// send to IO
emcTaskQueueCommand(emcCommand);
// then resynch interpreter
@@ -1384,6 +1393,11 @@ static int emcTaskCheckPreconditions(NMLmsg * cmd)
return EMC_TASK_EXEC_WAITING_FOR_MOTION_AND_IO;
break;
+ case EMC_TOOL_SET_LOCK_TYPE:
+ case EMC_TOOL_SET_FLUSH_TYPE:
+ return EMC_TASK_EXEC_WAITING_FOR_MOTION_AND_IO;
+ break;
+
case EMC_TOOL_SET_NUMBER_TYPE:
return EMC_TASK_EXEC_WAITING_FOR_IO;
break;
@@ -1873,6 +1887,16 @@ static int emcTaskIssueCommand(NMLmsg * cmd)
retval = emcToolSetNumber(emc_tool_set_number_msg->tool);
break;
+ case EMC_TOOL_SET_LOCK_TYPE:
+ emc_tool_set_lock_msg = (EMC_TOOL_SET_LOCK *) cmd;
+ retval = emcToolSetLock(emc_tool_set_lock_msg->mode);
+ break;
+
+ case EMC_TOOL_SET_FLUSH_TYPE:
+ emc_tool_set_flush_msg = (EMC_TOOL_SET_FLUSH *) cmd;
+ retval = emcToolSetFlush(emc_tool_set_flush_msg->mode);
+ break;
+
// task commands
case EMC_TASK_INIT_TYPE:
@@ -2166,6 +2190,8 @@ static int emcTaskCheckPostconditions(NMLmsg * cmd)
case EMC_TOOL_LOAD_TOOL_TABLE_TYPE:
case EMC_TOOL_SET_OFFSET_TYPE:
case EMC_TOOL_SET_NUMBER_TYPE:
+ case EMC_TOOL_SET_LOCK_TYPE:
+ case EMC_TOOL_SET_FLUSH_TYPE:
case EMC_SPINDLE_SPEED_TYPE:
case EMC_SPINDLE_ON_TYPE:
case EMC_SPINDLE_OFF_TYPE:
diff --git a/src/emc/task/iotaskintf.cc b/src/emc/task/iotaskintf.cc
index f4825e9..9fbfa70 100644
--- a/src/emc/task/iotaskintf.cc
+++ b/src/emc/task/iotaskintf.cc
@@ -429,6 +429,28 @@ int emcToolSetNumber(int number)
return 0;
}
+int emcToolSetLock(int mode)
+{
+ EMC_TOOL_SET_LOCK toolSetLockMsg;
+
+ toolSetLockMsg.mode = mode;
+
+ sendCommand(&toolSetLockMsg);
+
+ return 0;
+}
+
+int emcToolSetFlush(int mode)
+{
+ EMC_TOOL_SET_FLUSH toolSetFlushMsg;
+
+ toolSetFlushMsg.mode = mode;
+
+ sendCommand(&toolSetFlushMsg);
+
+ return 0;
+}
+
// Status functions
int emcIoUpdate(EMC_IO_STAT * stat)
--
1.6.1.2
------------------------------------------------------------------------------
_______________________________________________
Emc-users mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-users
