X-Git-Url: https://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/4210bdd3a1f685949baa0b297661c707964cc267..HEAD:/src/modules/tools/toolmanager/ToolManager.cpp

diff --git a/src/modules/tools/toolmanager/ToolManager.cpp b/src/modules/tools/toolmanager/ToolManager.cpp
index 25f4302b..72e4985d 100644
--- a/src/modules/tools/toolmanager/ToolManager.cpp
+++ b/src/modules/tools/toolmanager/ToolManager.cpp
@@ -7,10 +7,8 @@
 
 #include "libs/Module.h"
 #include "libs/Kernel.h"
-#include <math.h>
-using namespace std;
-#include <vector>
 #include "ToolManager.h"
+#include "Tool.h"
 #include "PublicDataRequest.h"
 #include "ToolManagerPublicAccess.h"
 #include "Config.h"
@@ -25,92 +23,107 @@ using namespace std;
 #include "libs/StreamOutput.h"
 #include "FileStream.h"
 
-#include "modules/robot/RobotPublicAccess.h"
-
-#define return_error_on_unhandled_gcode_checksum    CHECKSUM("return_error_on_unhandled_gcode")
-
-#define X_AXIS      0
-#define Y_AXIS      1
-#define Z_AXIS      2
+#include <math.h>
 
-ToolManager::ToolManager(){
+ToolManager::ToolManager()
+{
     active_tool = 0;
     current_tool_name = CHECKSUM("hotend");
 }
 
-void ToolManager::on_module_loaded(){
-    this->on_config_reload(this);
+void ToolManager::on_module_loaded()
+{
 
-    this->register_for_event(ON_CONFIG_RELOAD);
     this->register_for_event(ON_GCODE_RECEIVED);
     this->register_for_event(ON_GET_PUBLIC_DATA);
     this->register_for_event(ON_SET_PUBLIC_DATA);
 }
 
-void ToolManager::on_config_reload(void *argument){
-    return_error_on_unhandled_gcode = THEKERNEL->config->value( return_error_on_unhandled_gcode_checksum )->by_default(false)->as_bool();
-}
-
-void ToolManager::on_gcode_received(void *argument){
+void ToolManager::on_gcode_received(void *argument)
+{
     Gcode *gcode = static_cast<Gcode*>(argument);
 
-    if( gcode->has_letter('T') ){
+    if( gcode->has_letter('T') && !gcode->has_m) {
         int new_tool = gcode->get_value('T');
-        gcode->mark_as_taken();
-        if(new_tool >= (int)this->tools.size() || new_tool < 0){
+	char buff[32]; // should be big enough for any status
+	int n = snprintf(buff, sizeof(buff), "T%d,T%d switching ", this->active_tool, new_tool);
+	gcode->txt_after_ok.append(buff, n);
+        if(new_tool >= (int)this->tools.size() || new_tool < 0) {
             // invalid tool
-            if( return_error_on_unhandled_gcode ) {
-                char buf[32]; // should be big enough for any status
-                int n= snprintf(buf, sizeof(buf), "T%d invalid tool ", new_tool);
-                gcode->txt_after_ok.append(buf, n);
-            }
+            char buf[32]; // should be big enough for any status
+            int n = snprintf(buf, sizeof(buf), "T%d invalid tool ", new_tool);
+            gcode->txt_after_ok.append(buf, n);
+
         } else {
-            if(new_tool != this->active_tool){
-                this->tools[active_tool]->disable();
+            if(new_tool != this->active_tool) {
+                // We must wait for an empty queue before we can disable the current extruder
+                THEKERNEL->conveyor->wait_for_idle();
+                this->tools[active_tool]->deselect();
                 this->active_tool = new_tool;
                 this->current_tool_name = this->tools[active_tool]->get_name();
-                this->tools[active_tool]->enable();
+                this->tools[active_tool]->select();
 
                 //send new_tool_offsets to robot
-                float *new_tool_offset = tools[new_tool]->get_offset();
-                THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]);
+                const float *new_tool_offset = tools[new_tool]->get_offset();
+                THEROBOT->setToolOffset(new_tool_offset);
             }
         }
     }
 }
 
-void ToolManager::on_get_public_data(void* argument){
+void ToolManager::on_get_public_data(void* argument)
+{
     PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
 
     if(!pdr->starts_with(tool_manager_checksum)) return;
 
-    // ok this is targeted at us, so send back the requested data
-    // this must be static as it will be accessed long after we have returned
-    static struct pad_toolmanager tool_return;
-    tool_return.current_tool_name= this->current_tool_name;
+    if(pdr->second_element_is(is_active_tool_checksum)) {
+
+        // check that we control the given tool
+        bool managed = false;
+        for(auto t : tools) {
+            uint16_t n = t->get_name();
+            if(pdr->third_element_is(n)) {
+                managed = true;
+                break;
+            }
+        }
 
-    pdr->set_data_ptr(&tool_return);
-    pdr->set_taken();
+        // we are not managing this tool so do not answer
+        if(!managed) return;
+
+        pdr->set_data_ptr(&this->current_tool_name);
+        pdr->set_taken();
+
+    }else if(pdr->second_element_is(get_active_tool_checksum)) {
+        pdr->set_data_ptr(&this->active_tool);
+        pdr->set_taken();
+    }
 }
 
-void ToolManager::on_set_public_data(void* argument){
+void ToolManager::on_set_public_data(void* argument)
+{
     PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
 
     if(!pdr->starts_with(tool_manager_checksum)) return;
 
     // ok this is targeted at us, so change tools
-    uint16_t tool_name= *static_cast<float*>(pdr->get_data_ptr());
+    //uint16_t tool_name= *static_cast<float*>(pdr->get_data_ptr());
     // TODO: fire a tool change gcode
-    pdr->set_taken();
+    //pdr->set_taken();
 }
 
 // Add a tool to the tool list
-void ToolManager::add_tool(Tool* tool_to_add){
-    if(this->tools.size() == 0){
-        tool_to_add->enable();
+void ToolManager::add_tool(Tool* tool_to_add)
+{
+    if(this->tools.size() == 0) {
+        tool_to_add->select();
         this->current_tool_name = tool_to_add->get_name();
+        //send new_tool_offsets to robot
+        const float *new_tool_offset = tool_to_add->get_offset();
+        THEROBOT->setToolOffset(new_tool_offset);
     } else {
-        tool_to_add->disable();
+        tool_to_add->deselect();
     }
     this->tools.push_back( tool_to_add );
 }