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 "Config.h"
  15 #include "Robot.h"
  16 #include "ConfigValue.h"
  17 #include "Conveyor.h"
  18 #include "checksumm.h"
  19 #include "PublicData.h"
  20 #include "Gcode.h"
  21
  22 #include "libs/SerialMessage.h"
  23 #include "libs/StreamOutput.h"
  24 #include "FileStream.h"
  25
  26 #include "modules/robot/RobotPublicAccess.h"
  27
  28 #define return_error_on_unhandled_gcode_checksum    CHECKSUM("return_error_on_unhandled_gcode")
  29
  30 #define X_AXIS      0
  31 #define Y_AXIS      1
  32 #define Z_AXIS      2
  33
  34 ToolManager::ToolManager(){
  35     active_tool = 0;
  36 }
  37
  38 void ToolManager::on_module_loaded(){
  39     this->on_config_reload(this);
  40
  41     this->register_for_event(ON_CONFIG_RELOAD);
  42     this->register_for_event(ON_GCODE_RECEIVED);
  43 }
  44
  45 void ToolManager::on_config_reload(void *argument){
  46     return_error_on_unhandled_gcode = THEKERNEL->config->value( return_error_on_unhandled_gcode_checksum )->by_default(false)->as_bool();
  47 }
  48
  49 void ToolManager::on_gcode_received(void *argument){
  50     Gcode *gcode = static_cast<Gcode*>(argument);
  51
  52     if( gcode->has_letter('T') ){
  53         int new_tool = gcode->get_value('T');
  54         bool make_move = false;
  55         if ( gcode->has_letter('F') ){
  56             make_move = true;
  57         }
  58         gcode->mark_as_taken();
  59         if(new_tool >= (int)this->tools.size() || new_tool < 0){
  60             // invalid tool
  61             if( return_error_on_unhandled_gcode ) {
  62                 char buf[32]; // should be big enough for any status
  63                 int n= snprintf(buf, sizeof(buf), "T%d invalid tool ", new_tool);
  64                 gcode->txt_after_ok.append(buf, n);
  65             }
  66         } else {
  67             if(new_tool != this->active_tool){
  68                 void *returned_data;
  69                 THEKERNEL->conveyor->wait_for_empty_queue();
  70                 bool ok = THEKERNEL->public_data->get_value( robot_checksum, current_position_checksum, &returned_data );
  71                 if(ok){
  72                     // save current position to return to after applying extruder offset
  73                     float *pos = static_cast<float *>(returned_data);
  74                     float current_pos[3];
  75                     for(int i=0;i<3;i++){
  76                         current_pos[i] = pos[i];
  77                     }
  78                     // update virtual tool position to the offset of the new tool and select it as active
  79                     float new_pos[3];
  80                     float *active_tool_offset = tools[this->active_tool]->get_offset();
  81                     float *new_tool_offset = tools[new_tool]->get_offset();
  82                     for(int i=0;i<3;i++){
  83                         new_pos[i] = current_pos[i] - active_tool_offset[i] + new_tool_offset[i];
  84                     }
  85
  86                     this->tools[active_tool]->disable();
  87                     this->active_tool = new_tool;
  88                     this->tools[active_tool]->enable();
  89
  90                     char buf[32];
  91
  92                     THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]);
  93
  94                     if(make_move){
  95                         //move to old position
  96                         snprintf(buf, 31, "G0 X%g Y%g Z%g", current_pos[X_AXIS], current_pos[Y_AXIS], current_pos[Z_AXIS]);
  97                         s = buf;
  98                         g = new Gcode(s, gcode->stream);
  99                         THEKERNEL->call_event(ON_GCODE_RECEIVED, g);
 100                         delete g;
 101                 }
 102             }
 103         }
 104     }
 105 }
 106
 107 // Add a tool to the tool list
 108 void ToolManager::add_tool(Tool* tool_to_add){
 109     if(this->tools.size() == 0){
 110         tool_to_add->enable();
 111     } else {
 112         tool_to_add->disable();
 113     }
 114     this->tools.push_back( tool_to_add );
 115 }
 116
 117
 118