finished basic implementation of tool changing

diff --git a/src/modules/tools/toolmanager/ToolManager.cpp b/src/modules/tools/toolmanager/ToolManager.cpp
index d8ff956..9e9022d 100644 (file)
--- a/src/modules/tools/toolmanager/ToolManager.cpp
+++ b/src/modules/tools/toolmanager/ToolManager.cpp
@@ -19,7 +19,6 @@ ToolManager::ToolManager(){
 
 void ToolManager::on_module_loaded(){
     this->register_for_event(ON_GCODE_RECEIVED);
-    this->register_for_event(ON_GCODE_EXECUTE);
 }
 
 void ToolManager::on_gcode_received(void *argument){
@@ -27,49 +26,55 @@ void ToolManager::on_gcode_received(void *argument){
 
     if( gcode->has_letter('T') ){
         int new_tool = gcode->get_value('T');
+        bool make_move = false;
+        if ( gcode->has_letter('F') ){
+            make_move = true;
+        }
         gcode->mark_as_taken();
-        if(new_tool > this->tools.size() || new_tool < 0){
+        if(new_tool > (int)this->tools.size() || new_tool < 0){
             // invalid tool
             gcode->stream->printf(";;T%d invalid tool\r\n", new_tool);
         } else {
-            // pass along to on_gcode_execute
-            THEKERNEL->conveyor->append_gcode(gcode);
-        }
-    }
-}
+            if( gcode->has_letter('T') ){
+                int new_tool = gcode->get_value('T');
+                if(new_tool != this->active_tool){
+                    void *returned_data;
+                    bool ok = THEKERNEL->public_data->get_value( robot_checksum, current_position_checksum, &returned_data );
+                    if(ok){
+                        // save current position to return to after applying extruder offset
+                        float *pos = static_cast<float *>(returned_data);
+                        float current_pos[3];
+                        for(int i=0;i<3;i++){
+                            current_pos[i] = pos[i];
+                        }
+                        // update virtual tool position to the offset of the new tool and select it as active
+                        float new_pos[3];
+                        float *active_tool_offset = tools[this->active_tool]->get_offset();
+                        float *new_tool_offset = tools[new_tool]->get_offset();
+                        for(int i=0;i<3;i++){
+                            new_pos[i] = current_pos[i] - active_tool_offset[i] + new_tool_offset[i];
+                        }
 
-// Compute extrusion speed based on parameters and gcode distance of travel
-void ToolManager::on_gcode_execute(void* argument){
-    Gcode* gcode = static_cast<Gcode*>(argument);
+                        this->tools[active_tool]->disable();
+                        this->active_tool = new_tool;
+                        this->tools[active_tool]->enable();
 
-    if( gcode->has_letter('T') ){
-        int new_tool = gcode->get_value('T');
-        bool make_move = false;
-        if ( gcode->has_letter('F') ){
-            make_move = true;
-        }
-        if(new_tool != this->active_tool){
-            void *returned_data;
-            bool ok = THEKERNEL->public_data->get_value( robot_checksum, current_position_checksum, &returned_data );
-            if(ok){
-                // save current position to return to after applying extruder offset
-                float *pos = static_cast<float *>(returned_data);
-                float current_pos[3];
-                for(int i=0;i<3;i++){
-                    current_pos[i] = pos[i];
-                }
-                // update virtual tool position to the offset of the new tool and select it as active
-                float new_pos[3];
-                float *active_tool_offset = tools[this->active_tool]->get_offset();
-                float *new_tool_offset = tools[new_tool]->get_offset();
-                for(int i=0;i<3;i++){
-                    new_pos[i] = current_pos[i] - active_tool_offset[i] + new_tool_offset[i];
-                }
+                        char buf[32];
+                        snprintf(buf, 31, "G92 X%g Y%g Z%g", new_pos[X_AXIS], new_pos[Y_AXIS], new_pos[Z_AXIS]);
+                        string s = buf;
+                        Gcode *g = new Gcode(s, gcode->stream);
+                        THEKERNEL->call_event(ON_GCODE_RECEIVED, g);
+                        delete g;
 
-                this->active_tool = new_tool;
-                ok = THEKERNEL->public_data->set_value( robot_checksum, current_position_checksum, new_pos );
-                if(ok && make_move){
-                    //TODO:move to old position (stored in current_pos[])
+                        if(make_move){
+                            //move to old position
+                            snprintf(buf, 25, "G0 X%g Y%g Z%g", current_pos[X_AXIS], current_pos[Y_AXIS], current_pos[Z_AXIS]);
+                            s = buf;
+                            g = new Gcode(s, gcode->stream);
+                            THEKERNEL->call_event(ON_GCODE_RECEIVED, g);
+                            delete g;
+                        }
+                    }
                 }
             }
         }

diff --git a/src/modules/tools/toolmanager/ToolManager.h b/src/modules/tools/toolmanager/ToolManager.h
index c89d3e6..ddc9589 100644 (file)
--- a/src/modules/tools/toolmanager/ToolManager.h
+++ b/src/modules/tools/toolmanager/ToolManager.h
@@ -19,7 +19,6 @@ class ToolManager : public Module {
 
         void on_module_loaded();
         void on_gcode_received(void*);
-        void on_gcode_execute(void* argument);
         void add_tool(Tool* tool_to_add);
 
         vector<Tool*> tools;