src/modules/tools/toolmanager/ToolManager.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   3       Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
   4       Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include <math.h>
  11 using namespace std;
  12 #include <vector>
  13 #include "ToolManager.h"
  14 #include "Conveyor.h"
  15
  16 ToolManager::ToolManager(){
  17     active_tool = 0;
  18 }
  19
  20 void ToolManager::on_module_loaded(){
  21     this->register_for_event(ON_GCODE_RECEIVED);
  22 }
  23
  24 void ToolManager::on_gcode_received(void *argument){
  25     Gcode *gcode = static_cast<Gcode*>(argument);
  26
  27     if( gcode->has_letter('T') ){
  28         int new_tool = gcode->get_value('T');
  29         bool make_move = false;
  30         if ( gcode->has_letter('F') ){
  31             make_move = true;
  32         }
  33         gcode->mark_as_taken();
  34         if(new_tool >= (int)this->tools.size() || new_tool < 0){
  35             // invalid tool
  36             // TODO: report invalid selection
  37             gcode->stream->printf(";;T%d invalid tool\r\n", new_tool);
  38         } else {
  39             if(new_tool != this->active_tool){
  40                 void *returned_data;
  41                 bool ok = THEKERNEL->public_data->get_value( robot_checksum, current_position_checksum, &returned_data );
  42                 if(ok){
  43                     // save current position to return to after applying extruder offset
  44                     float *pos = static_cast<float *>(returned_data);
  45                     float current_pos[3];
  46                     for(int i=0;i<3;i++){
  47                         current_pos[i] = pos[i];
  48                     }
  49                     // update virtual tool position to the offset of the new tool and select it as active
  50                     float new_pos[3];
  51                     float *active_tool_offset = tools[this->active_tool]->get_offset();
  52                     float *new_tool_offset = tools[new_tool]->get_offset();
  53                     for(int i=0;i<3;i++){
  54                         new_pos[i] = current_pos[i] - active_tool_offset[i] + new_tool_offset[i];
  55                     }
  56
  57                     this->tools[active_tool]->disable();
  58                     this->active_tool = new_tool;
  59                     this->tools[active_tool]->enable();
  60
  61                     char buf[32];
  62                     snprintf(buf, 31, "G92 X%g Y%g Z%g", new_pos[X_AXIS], new_pos[Y_AXIS], new_pos[Z_AXIS]);
  63                     string s = buf;
  64                     Gcode *g = new Gcode(s, gcode->stream);
  65                     THEKERNEL->call_event(ON_GCODE_RECEIVED, g);
  66                     delete g;
  67
  68                     if(make_move){
  69                         //move to old position
  70                         snprintf(buf, 31, "G0 X%g Y%g Z%g", current_pos[X_AXIS], current_pos[Y_AXIS], current_pos[Z_AXIS]);
  71                         s = buf;
  72                         g = new Gcode(s, gcode->stream);
  73                         THEKERNEL->call_event(ON_GCODE_RECEIVED, g);
  74                         delete g;
  75                     }
  76                 }
  77             }
  78         }
  79     }
  80 }
  81
  82 // Add a tool to the tool list
  83 void ToolManager::add_tool(Tool* tool_to_add){
  84     if(this->tools.size() == 0){
  85         tool_to_add->enable();
  86     } else {
  87         tool_to_add->disable();
  88     }
  89     this->tools.push_back( tool_to_add );
  90 }
  91
  92
  93