Commit | Line | Data |
---|---|---|
14ecdbd7 AW |
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> | |
18067d90 | 13 | #include "ToolManager.h" |
1440c258 | 14 | #include "Config.h" |
5966b7d0 | 15 | #include "Robot.h" |
1440c258 | 16 | #include "ConfigValue.h" |
8adf2390 | 17 | #include "Conveyor.h" |
1440c258 L |
18 | #include "checksumm.h" |
19 | #include "PublicData.h" | |
20 | #include "Gcode.h" | |
21 | ||
5966b7d0 AT |
22 | #include "libs/SerialMessage.h" |
23 | #include "libs/StreamOutput.h" | |
24 | #include "FileStream.h" | |
25 | ||
1440c258 | 26 | #include "modules/robot/RobotPublicAccess.h" |
14ecdbd7 | 27 | |
f4a68aec L |
28 | #define return_error_on_unhandled_gcode_checksum CHECKSUM("return_error_on_unhandled_gcode") |
29 | ||
1440c258 L |
30 | #define X_AXIS 0 |
31 | #define Y_AXIS 1 | |
32 | #define Z_AXIS 2 | |
33 | ||
92031952 | 34 | ToolManager::ToolManager(){ |
8adf2390 L |
35 | active_tool = 0; |
36 | } | |
14ecdbd7 | 37 | |
92031952 | 38 | void ToolManager::on_module_loaded(){ |
f4a68aec L |
39 | this->on_config_reload(this); |
40 | ||
41 | this->register_for_event(ON_CONFIG_RELOAD); | |
8adf2390 | 42 | this->register_for_event(ON_GCODE_RECEIVED); |
8adf2390 L |
43 | } |
44 | ||
f4a68aec L |
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 | ||
92031952 | 49 | void ToolManager::on_gcode_received(void *argument){ |
8adf2390 L |
50 | Gcode *gcode = static_cast<Gcode*>(argument); |
51 | ||
52 | if( gcode->has_letter('T') ){ | |
53 | int new_tool = gcode->get_value('T'); | |
5a6dd23d L |
54 | bool make_move = false; |
55 | if ( gcode->has_letter('F') ){ | |
56 | make_move = true; | |
57 | } | |
8adf2390 | 58 | gcode->mark_as_taken(); |
c974b296 | 59 | if(new_tool >= (int)this->tools.size() || new_tool < 0){ |
8adf2390 | 60 | // invalid tool |
f4a68aec L |
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 | } | |
8adf2390 | 66 | } else { |
c974b296 L |
67 | if(new_tool != this->active_tool){ |
68 | void *returned_data; | |
1440c258 | 69 | THEKERNEL->conveyor->wait_for_empty_queue(); |
c974b296 L |
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(); | |
14ecdbd7 | 89 | |
c974b296 | 90 | char buf[32]; |
5966b7d0 AT |
91 | //snprintf(buf, 31, "G92 X%g Y%g Z%g", new_pos[X_AXIS], new_pos[Y_AXIS], new_pos[Z_AXIS]); |
92 | //string s = buf; | |
93 | //Gcode *g = new Gcode(s, gcode->stream); | |
94 | //THEKERNEL->call_event(ON_GCODE_RECEIVED, g); | |
95 | //delete g; | |
96 | THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]); | |
8adf2390 | 97 | |
c974b296 | 98 | if(make_move){ |
5966b7d0 AT |
99 | stream->printf("BX Offset is: %1.3f\n", new_tool_offset[0]); |
100 | bool ok = THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]); | |
101 | ||
c974b296 L |
102 | //move to old position |
103 | snprintf(buf, 31, "G0 X%g Y%g Z%g", current_pos[X_AXIS], current_pos[Y_AXIS], current_pos[Z_AXIS]); | |
104 | s = buf; | |
105 | g = new Gcode(s, gcode->stream); | |
5a6dd23d L |
106 | THEKERNEL->call_event(ON_GCODE_RECEIVED, g); |
107 | delete g; | |
5a6dd23d | 108 | } |
8adf2390 L |
109 | } |
110 | } | |
111 | } | |
112 | } | |
113 | } | |
14ecdbd7 AW |
114 | |
115 | // Add a tool to the tool list | |
92031952 | 116 | void ToolManager::add_tool(Tool* tool_to_add){ |
c974b296 L |
117 | if(this->tools.size() == 0){ |
118 | tool_to_add->enable(); | |
119 | } else { | |
120 | tool_to_add->disable(); | |
121 | } | |
14ecdbd7 AW |
122 | this->tools.push_back( tool_to_add ); |
123 | } | |
124 | ||
125 | ||
126 |