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/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
10 #include "modules/communication/utils/Gcode.h"
11 #include "modules/robot/Conveyor.h"
13 #include "libs/nuts_bolts.h"
15 #include "libs/StepperMotor.h"
16 #include "wait_api.h" // mbed.h lib
19 this->status
= NOT_HOMING
;
22 void Endstops::on_module_loaded() {
23 // Do not do anything if not enabled
24 if( this->kernel
->config
->value( endstops_module_enable_checksum
)->by_default(true)->as_bool() == false ){ return; }
26 register_for_event(ON_CONFIG_RELOAD
);
27 this->register_for_event(ON_GCODE_RECEIVED
);
29 // Take StepperMotor objects from Robot and keep them here
30 this->steppers
[0] = this->kernel
->robot
->alpha_stepper_motor
;
31 this->steppers
[1] = this->kernel
->robot
->beta_stepper_motor
;
32 this->steppers
[2] = this->kernel
->robot
->gamma_stepper_motor
;
35 this->on_config_reload(this);
40 void Endstops::on_config_reload(void* argument
){
41 this->pins
[0].from_string( this->kernel
->config
->value(alpha_min_endstop_checksum
)->by_default("nc" )->as_string())->as_input();
42 this->pins
[1].from_string( this->kernel
->config
->value(beta_min_endstop_checksum
)->by_default("nc" )->as_string())->as_input();
43 this->pins
[2].from_string( this->kernel
->config
->value(gamma_min_endstop_checksum
)->by_default("nc" )->as_string())->as_input();
44 this->pins
[3].from_string( this->kernel
->config
->value(alpha_max_endstop_checksum
)->by_default("nc" )->as_string())->as_input();
45 this->pins
[4].from_string( this->kernel
->config
->value(beta_max_endstop_checksum
)->by_default("nc" )->as_string())->as_input();
46 this->pins
[5].from_string( this->kernel
->config
->value(gamma_max_endstop_checksum
)->by_default("nc" )->as_string())->as_input();
47 this->fast_rates
[0] = this->kernel
->config
->value(alpha_fast_homing_rate_checksum
)->by_default(500 )->as_number();
48 this->fast_rates
[1] = this->kernel
->config
->value(beta_fast_homing_rate_checksum
)->by_default(500 )->as_number();
49 this->fast_rates
[2] = this->kernel
->config
->value(gamma_fast_homing_rate_checksum
)->by_default(5 )->as_number();
50 this->slow_rates
[0] = this->kernel
->config
->value(alpha_slow_homing_rate_checksum
)->by_default(100 )->as_number();
51 this->slow_rates
[1] = this->kernel
->config
->value(beta_slow_homing_rate_checksum
)->by_default(100 )->as_number();
52 this->slow_rates
[2] = this->kernel
->config
->value(gamma_slow_homing_rate_checksum
)->by_default(5 )->as_number();
53 this->retract_steps
[0] = this->kernel
->config
->value(alpha_homing_retract_checksum
)->by_default(30 )->as_number();
54 this->retract_steps
[1] = this->kernel
->config
->value(beta_homing_retract_checksum
)->by_default(30 )->as_number();
55 this->retract_steps
[2] = this->kernel
->config
->value(gamma_homing_retract_checksum
)->by_default(10 )->as_number();
56 this->trim
[0] = this->kernel
->config
->value(alpha_trim_checksum
)->by_default(0 )->as_number();
57 this->trim
[1] = this->kernel
->config
->value(beta_trim_checksum
)->by_default(0 )->as_number();
58 this->trim
[2] = this->kernel
->config
->value(gamma_trim_checksum
)->by_default(0 )->as_number();
59 this->debounce_count
= this->kernel
->config
->value(endstop_debounce_count_checksum
)->by_default(100 )->as_number();
61 // we need to know steps per mm for M206, TODO should probably use them for all settings
62 this->steps_per_mm
[0] = this->kernel
->config
->value(alpha_steps_per_mm_checksum
)->as_number();
63 this->steps_per_mm
[1] = this->kernel
->config
->value(beta_steps_per_mm_checksum
)->as_number();
64 this->steps_per_mm
[2] = this->kernel
->config
->value(gamma_steps_per_mm_checksum
)->as_number();
66 // get homing direction and convert to boolean where true is home to min, and false is home to max
67 int home_dir
= get_checksum(this->kernel
->config
->value(alpha_homing_direction_checksum
)->by_default("home_to_min")->as_string());
68 this->home_direction
[0] = home_dir
!= home_to_max_checksum
;
70 home_dir
= get_checksum(this->kernel
->config
->value(beta_homing_direction_checksum
)->by_default("home_to_min")->as_string());
71 this->home_direction
[1] = home_dir
!= home_to_max_checksum
;
73 home_dir
= get_checksum(this->kernel
->config
->value(gamma_homing_direction_checksum
)->by_default("home_to_min")->as_string());
74 this->home_direction
[2] = home_dir
!= home_to_max_checksum
;
76 this->homing_position
[0] = this->home_direction
[0]?this->kernel
->config
->value(alpha_min_checksum
)->by_default(0)->as_number():this->kernel
->config
->value(alpha_max_checksum
)->by_default(200)->as_number();
77 this->homing_position
[1] = this->home_direction
[1]?this->kernel
->config
->value(beta_min_checksum
)->by_default(0)->as_number():this->kernel
->config
->value(beta_max_checksum
)->by_default(200)->as_number();;
78 this->homing_position
[2] = this->home_direction
[2]?this->kernel
->config
->value(gamma_min_checksum
)->by_default(0)->as_number():this->kernel
->config
->value(gamma_max_checksum
)->by_default(200)->as_number();;
81 void Endstops::wait_for_homed(char axes_to_move
){
83 unsigned int debounce
[3] = {0,0,0};
86 this->kernel
->call_event(ON_IDLE
);
87 for( char c
= 'X'; c
<= 'Z'; c
++ ){
88 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
89 if( this->pins
[c
- 'X' + (this->home_direction
[c
- 'X']?0:3)].get() ){
90 if( debounce
[c
- 'X'] < debounce_count
) {
93 } else if ( this->steppers
[c
- 'X']->moving
){
94 this->steppers
[c
- 'X']->move(0,0);
95 //this->kernel->streams->printf("move done %c\r\n", c);
98 // The endstop was not hit yet
100 debounce
[c
- 'X'] = 0;
107 // Start homing sequences by response to GCode commands
108 void Endstops::on_gcode_received(void* argument
)
110 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
115 gcode
->mark_as_taken();
116 // G28 is received, we have homing to do
118 // First wait for the queue to be empty
119 this->kernel
->conveyor
->wait_for_empty_queue();
121 // Do we move select axes or all of them
122 char axes_to_move
= 0;
123 // only enable homing if the endstop is defined
124 bool home_all
= !( gcode
->has_letter('X') || gcode
->has_letter('Y') || gcode
->has_letter('Z') );
126 for( char c
= 'X'; c
<= 'Z'; c
++ ){
127 if( (home_all
|| gcode
->has_letter(c
)) && this->pins
[c
- 'X' + (this->home_direction
[c
- 'X']?0:3)].connected() ){ axes_to_move
+= ( 1 << (c
- 'X' ) ); }
131 this->kernel
->stepper
->turn_enable_pins_on();
133 // Start moving the axes to the origin
134 this->status
= MOVING_TO_ORIGIN_FAST
;
135 for( char c
= 'X'; c
<= 'Z'; c
++ ){
136 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
137 gcode
->stream
->printf("homing axis %c\r\n", c
);
138 this->steppers
[c
- 'X']->set_speed(this->fast_rates
[c
- 'X']);
139 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
143 // Wait for all axes to have homed
144 this->wait_for_homed(axes_to_move
);
146 gcode
->stream
->printf("test a\r\n");
147 // Move back a small distance
148 this->status
= MOVING_BACK
;
150 for( char c
= 'X'; c
<= 'Z'; c
++ ){
151 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
152 inverted_dir
= !this->home_direction
[c
- 'X'];
153 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
154 this->steppers
[c
- 'X']->move(inverted_dir
,this->retract_steps
[c
- 'X']);
158 gcode
->stream
->printf("test b\r\n");
159 // Wait for moves to be done
160 for( char c
= 'X'; c
<= 'Z'; c
++ ){
161 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
162 this->kernel
->streams
->printf("axis %c \r\n", c
);
163 while( this->steppers
[c
- 'X']->moving
){
164 this->kernel
->call_event(ON_IDLE
);
169 gcode
->stream
->printf("test c\r\n");
171 // Start moving the axes to the origin slowly
172 this->status
= MOVING_TO_ORIGIN_SLOW
;
173 for( char c
= 'X'; c
<= 'Z'; c
++ ){
174 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
175 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
-'X']);
176 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
180 // Wait for all axes to have homed
181 this->wait_for_homed(axes_to_move
);
183 // move for soft trim
184 this->status
= MOVING_BACK
;
185 for( char c
= 'X'; c
<= 'Z'; c
++ ){
186 if( this->trim
[c
- 'X'] != 0 && ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
187 inverted_dir
= !this->home_direction
[c
- 'X'];
188 // move up or down depending on sign of trim
189 if(this->trim
[c
- 'X'] < 0) inverted_dir
= !inverted_dir
;
190 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
191 this->steppers
[c
- 'X']->move(inverted_dir
,this->trim
[c
- 'X']);
195 // Wait for moves to be done
196 for( char c
= 'X'; c
<= 'Z'; c
++ ){
197 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
198 this->kernel
->streams
->printf("axis %c \r\n", c
);
199 while( this->steppers
[c
- 'X']->moving
){
200 this->kernel
->call_event(ON_IDLE
);
206 this->status
= NOT_HOMING
;
208 // Zero the ax(i/e)s position
209 for( char c
= 'X'; c
<= 'Z'; c
++ ){
210 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
212 this->kernel
->robot
->reset_axis_position(this->homing_position
[c
- 'X'], c
- 'X');
218 else if (gcode
->has_m
){
223 int px
= this->home_direction
[0] ? 0 : 3;
224 int py
= this->home_direction
[1] ? 1 : 4;
225 int pz
= this->home_direction
[2] ? 2 : 5;
226 const char* mx
= this->home_direction
[0] ? "min" : "max";
227 const char* my
= this->home_direction
[1] ? "min" : "max";
228 const char* mz
= this->home_direction
[2] ? "min" : "max";
230 gcode
->stream
->printf("X %s:%d Y %s:%d Z %s:%d\n", mx
, this->pins
[px
].get(), my
, this->pins
[py
].get(), mz
, this->pins
[pz
].get());
231 gcode
->mark_as_taken();
235 case 206: // M206 - set trim for each axis in mm
237 int dirx
= (this->home_direction
[0] ? 1 : -1);
238 int diry
= (this->home_direction
[1] ? 1 : -1);
239 int dirz
= (this->home_direction
[2] ? 1 : -1);
241 mm
[0]= trim
[0]/steps_per_mm
[0] * dirx
; // convert to mm
242 mm
[1]= trim
[1]/steps_per_mm
[1] * diry
;
243 mm
[2]= trim
[2]/steps_per_mm
[2] * dirz
;
245 if(gcode
->has_letter('X')) mm
[0]= gcode
->get_value('X');
246 if(gcode
->has_letter('Y')) mm
[1]= gcode
->get_value('Y');
247 if(gcode
->has_letter('Z')) mm
[2]= gcode
->get_value('Z');
249 trim
[0]= lround(mm
[0]*steps_per_mm
[0]) * dirx
; // convert back to steps
250 trim
[1]= lround(mm
[1]*steps_per_mm
[1]) * diry
;
251 trim
[2]= lround(mm
[2]*steps_per_mm
[2]) * dirz
;
253 // print the current trim values in mm and steps
255 int n
= snprintf(buf
, sizeof(buf
), "X:%5.3f (%d) Y:%5.3f (%d) Z:%5.3f (%d) ", mm
[0], trim
[0], mm
[1], trim
[1], mm
[2], trim
[2]);
256 gcode
->txt_after_ok
.append(buf
, n
);
257 gcode
->mark_as_taken();