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();
48 // we need to know steps per mm for M206, also use them for all settings
49 this->steps_per_mm
[0] = this->kernel
->config
->value(alpha_steps_per_mm_checksum
)->as_number();
50 this->steps_per_mm
[1] = this->kernel
->config
->value(beta_steps_per_mm_checksum
)->as_number();
51 this->steps_per_mm
[2] = this->kernel
->config
->value(gamma_steps_per_mm_checksum
)->as_number();
53 this->fast_rates
[0] = this->kernel
->config
->value(alpha_fast_homing_rate_checksum
)->by_default(4000 )->as_number();
54 this->fast_rates
[1] = this->kernel
->config
->value(beta_fast_homing_rate_checksum
)->by_default(4000 )->as_number();
55 this->fast_rates
[2] = this->kernel
->config
->value(gamma_fast_homing_rate_checksum
)->by_default(6400 )->as_number();
56 this->slow_rates
[0] = this->kernel
->config
->value(alpha_slow_homing_rate_checksum
)->by_default(2000 )->as_number();
57 this->slow_rates
[1] = this->kernel
->config
->value(beta_slow_homing_rate_checksum
)->by_default(2000 )->as_number();
58 this->slow_rates
[2] = this->kernel
->config
->value(gamma_slow_homing_rate_checksum
)->by_default(3200 )->as_number();
59 this->retract_steps
[0] = this->kernel
->config
->value(alpha_homing_retract_checksum
)->by_default(400 )->as_number();
60 this->retract_steps
[1] = this->kernel
->config
->value(beta_homing_retract_checksum
)->by_default(400 )->as_number();
61 this->retract_steps
[2] = this->kernel
->config
->value(gamma_homing_retract_checksum
)->by_default(1600 )->as_number();
63 // newer mm based config values override the old ones, convert to steps/mm and steps, defaults to what was set in the older config settings above
64 this->fast_rates
[0]= this->kernel
->config
->value(alpha_fast_homing_rate_mm_checksum
)->by_default(this->fast_rates
[0] / steps_per_mm
[0])->as_number() * steps_per_mm
[0];
65 this->fast_rates
[1]= this->kernel
->config
->value(beta_fast_homing_rate_mm_checksum
)->by_default(this->fast_rates
[1] / steps_per_mm
[1])->as_number() * steps_per_mm
[1];
66 this->fast_rates
[2]= this->kernel
->config
->value(gamma_fast_homing_rate_mm_checksum
)->by_default(this->fast_rates
[2] / steps_per_mm
[2])->as_number() * steps_per_mm
[2];
67 this->slow_rates
[0]= this->kernel
->config
->value(alpha_slow_homing_rate_mm_checksum
)->by_default(this->slow_rates
[0] / steps_per_mm
[0])->as_number() * steps_per_mm
[0];
68 this->slow_rates
[1]= this->kernel
->config
->value(beta_slow_homing_rate_mm_checksum
)->by_default(this->slow_rates
[1] / steps_per_mm
[1])->as_number() * steps_per_mm
[1];
69 this->slow_rates
[2]= this->kernel
->config
->value(gamma_slow_homing_rate_mm_checksum
)->by_default(this->slow_rates
[2] / steps_per_mm
[2])->as_number() * steps_per_mm
[2];
70 this->retract_steps
[0]= this->kernel
->config
->value(alpha_homing_retract_mm_checksum
)->by_default(this->retract_steps
[0]/steps_per_mm
[0])->as_number() * steps_per_mm
[0];
71 this->retract_steps
[1]= this->kernel
->config
->value(beta_homing_retract_mm_checksum
)->by_default(this->retract_steps
[1]/steps_per_mm
[1])->as_number() * steps_per_mm
[1];
72 this->retract_steps
[2]= this->kernel
->config
->value(gamma_homing_retract_mm_checksum
)->by_default(this->retract_steps
[2]/steps_per_mm
[2])->as_number() * steps_per_mm
[2];
74 this->debounce_count
= this->kernel
->config
->value(endstop_debounce_count_checksum
)->by_default(100)->as_number();
77 // get homing direction and convert to boolean where true is home to min, and false is home to max
78 int home_dir
= get_checksum(this->kernel
->config
->value(alpha_homing_direction_checksum
)->by_default("home_to_min")->as_string());
79 this->home_direction
[0] = home_dir
!= home_to_max_checksum
;
81 home_dir
= get_checksum(this->kernel
->config
->value(beta_homing_direction_checksum
)->by_default("home_to_min")->as_string());
82 this->home_direction
[1] = home_dir
!= home_to_max_checksum
;
84 home_dir
= get_checksum(this->kernel
->config
->value(gamma_homing_direction_checksum
)->by_default("home_to_min")->as_string());
85 this->home_direction
[2] = home_dir
!= home_to_max_checksum
;
87 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();
88 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();;
89 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();;
91 this->is_corexy
= this->kernel
->config
->value(corexy_homing_checksum
)->by_default(false)->as_bool();
93 // endstop trim used by deltas to do soft adjusting, in mm, convert to steps, and negate depending on homing direction
94 // eg on a delta homing to max, a negative trim value will move the carriage down, and a positive will move it up
95 int dirx
= (this->home_direction
[0] ? 1 : -1);
96 int diry
= (this->home_direction
[1] ? 1 : -1);
97 int dirz
= (this->home_direction
[2] ? 1 : -1);
98 this->trim
[0]= this->kernel
->config
->value(alpha_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[0] * dirx
;
99 this->trim
[1]= this->kernel
->config
->value(beta_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[1] * diry
;
100 this->trim
[2]= this->kernel
->config
->value(gamma_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[2] * dirz
;
103 void Endstops::wait_for_homed(char axes_to_move
){
105 unsigned int debounce
[3] = {0,0,0};
108 this->kernel
->call_event(ON_IDLE
);
109 for( char c
= 'X'; c
<= 'Z'; c
++ ){
110 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
111 if( this->pins
[c
- 'X' + (this->home_direction
[c
- 'X']?0:3)].get() ){
112 if( debounce
[c
- 'X'] < debounce_count
) {
113 debounce
[c
- 'X'] ++;
115 } else if ( this->steppers
[c
- 'X']->moving
){
116 this->steppers
[c
- 'X']->move(0,0);
119 // The endstop was not hit yet
121 debounce
[c
- 'X'] = 0;
128 // this homing works for cartesian and delta printers, not for HBots/CoreXY
129 void Endstops::do_homing(char axes_to_move
) {
130 // Start moving the axes to the origin
131 this->status
= MOVING_TO_ORIGIN_FAST
;
132 for( char c
= 'X'; c
<= 'Z'; c
++ ){
133 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
134 this->steppers
[c
- 'X']->set_speed(this->fast_rates
[c
- 'X']);
135 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
139 // Wait for all axes to have homed
140 this->wait_for_homed(axes_to_move
);
142 // Move back a small distance
143 this->status
= MOVING_BACK
;
145 for( char c
= 'X'; c
<= 'Z'; c
++ ){
146 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
147 inverted_dir
= !this->home_direction
[c
- 'X'];
148 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
149 this->steppers
[c
- 'X']->move(inverted_dir
,this->retract_steps
[c
- 'X']);
153 // Wait for moves to be done
154 for( char c
= 'X'; c
<= 'Z'; c
++ ){
155 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
156 while( this->steppers
[c
- 'X']->moving
){
157 this->kernel
->call_event(ON_IDLE
);
162 // Start moving the axes to the origin slowly
163 this->status
= MOVING_TO_ORIGIN_SLOW
;
164 for( char c
= 'X'; c
<= 'Z'; c
++ ){
165 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
166 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
-'X']);
167 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
171 // Wait for all axes to have homed
172 this->wait_for_homed(axes_to_move
);
174 // move for soft trim
175 this->status
= MOVING_BACK
;
176 for( char c
= 'X'; c
<= 'Z'; c
++ ){
177 if( this->trim
[c
- 'X'] != 0 && ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
178 inverted_dir
= !this->home_direction
[c
- 'X'];
179 // move up or down depending on sign of trim
180 if(this->trim
[c
- 'X'] < 0) inverted_dir
= !inverted_dir
;
181 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
182 this->steppers
[c
- 'X']->move(inverted_dir
,this->trim
[c
- 'X']);
186 // Wait for moves to be done
187 for( char c
= 'X'; c
<= 'Z'; c
++ ){
188 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
189 //this->kernel->streams->printf("axis %c \r\n", c );
190 while( this->steppers
[c
- 'X']->moving
){
191 this->kernel
->call_event(ON_IDLE
);
197 this->status
= NOT_HOMING
;
204 void Endstops::wait_for_homed_corexy(int axis
){
206 unsigned int debounce
[3] = {0,0,0};
209 this->kernel
->call_event(ON_IDLE
);
210 if( this->pins
[axis
+ (this->home_direction
[axis
]?0:3)].get() ){
211 if( debounce
[axis
] < debounce_count
) {
215 // turn both off if running
216 if(this->steppers
[X_AXIS
]->moving
) this->steppers
[X_AXIS
]->move(0,0);
217 if(this->steppers
[Y_AXIS
]->moving
) this->steppers
[Y_AXIS
]->move(0,0);
220 // The endstop was not hit yet
227 // this homing works for HBots/CoreXY
228 void Endstops::do_homing_corexy(char axes_to_move
) {
229 // Start moving the axes to the origin
230 if(axes_to_move
& 0x01) { // Home X, which means both X and Y in same direction
231 this->status
= MOVING_TO_ORIGIN_FAST
;
232 this->steppers
[X_AXIS
]->set_speed(this->fast_rates
[X_AXIS
]);
233 this->steppers
[X_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
234 this->steppers
[Y_AXIS
]->set_speed(this->fast_rates
[X_AXIS
]);
235 this->steppers
[Y_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
238 this->wait_for_homed_corexy(X_AXIS
);
240 // Move back a small distance
241 this->status
= MOVING_BACK
;
242 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
243 this->steppers
[X_AXIS
]->move(!this->home_direction
[X_AXIS
], this->retract_steps
[X_AXIS
]);
244 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
245 this->steppers
[Y_AXIS
]->move(!this->home_direction
[X_AXIS
], this->retract_steps
[X_AXIS
]);
248 while( this->steppers
[X_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
249 while( this->steppers
[Y_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
251 // Start moving the axes to the origin slowly
252 this->status
= MOVING_TO_ORIGIN_SLOW
;
253 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
254 this->steppers
[X_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
255 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
256 this->steppers
[Y_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
259 this->wait_for_homed_corexy(X_AXIS
);
262 if(axes_to_move
& 0x02) { // Home Y, which means both X and Y in different directions
263 this->status
= MOVING_TO_ORIGIN_FAST
;
264 this->steppers
[X_AXIS
]->set_speed(this->fast_rates
[Y_AXIS
]);
265 this->steppers
[X_AXIS
]->move(this->home_direction
[Y_AXIS
], 10000000);
266 this->steppers
[Y_AXIS
]->set_speed(this->fast_rates
[Y_AXIS
]); // yes I use X_axis speed as they need to go at the same speed
267 this->steppers
[Y_AXIS
]->move(!this->home_direction
[Y_AXIS
], 10000000);
270 this->wait_for_homed_corexy(Y_AXIS
);
272 // Move back a small distance
273 this->status
= MOVING_BACK
;
274 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
275 this->steppers
[X_AXIS
]->move(!this->home_direction
[Y_AXIS
], this->retract_steps
[Y_AXIS
]);
276 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
277 this->steppers
[Y_AXIS
]->move(this->home_direction
[Y_AXIS
], this->retract_steps
[Y_AXIS
]);
280 while( this->steppers
[X_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
281 while( this->steppers
[Y_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
283 // Start moving the axes to the origin slowly
284 this->status
= MOVING_TO_ORIGIN_SLOW
;
285 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
286 this->steppers
[X_AXIS
]->move(this->home_direction
[Y_AXIS
], 10000000);
287 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
288 this->steppers
[Y_AXIS
]->move(!this->home_direction
[Y_AXIS
], 10000000);
291 this->wait_for_homed_corexy(Y_AXIS
);
294 if(axes_to_move
& 0x04) { // move Z
295 do_homing(0x04); // just home normally for Z
299 this->status
= NOT_HOMING
;
302 // Start homing sequences by response to GCode commands
303 void Endstops::on_gcode_received(void* argument
)
305 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
310 gcode
->mark_as_taken();
311 // G28 is received, we have homing to do
313 // First wait for the queue to be empty
314 this->kernel
->conveyor
->wait_for_empty_queue();
316 // Do we move select axes or all of them
317 char axes_to_move
= 0;
318 // only enable homing if the endstop is defined
319 bool home_all
= !( gcode
->has_letter('X') || gcode
->has_letter('Y') || gcode
->has_letter('Z') );
321 for( char c
= 'X'; c
<= 'Z'; c
++ ){
322 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' ) ); }
326 this->kernel
->stepper
->turn_enable_pins_on();
328 // do the actual homing
330 do_homing_corexy(axes_to_move
);
332 do_homing(axes_to_move
);
334 // Zero the ax(i/e)s position
335 for( char c
= 'X'; c
<= 'Z'; c
++ ){
336 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
338 this->kernel
->robot
->reset_axis_position(this->homing_position
[c
- 'X'], c
- 'X');
344 else if (gcode
->has_m
){
349 int px
= this->home_direction
[0] ? 0 : 3;
350 int py
= this->home_direction
[1] ? 1 : 4;
351 int pz
= this->home_direction
[2] ? 2 : 5;
352 const char* mx
= this->home_direction
[0] ? "min" : "max";
353 const char* my
= this->home_direction
[1] ? "min" : "max";
354 const char* mz
= this->home_direction
[2] ? "min" : "max";
356 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());
357 gcode
->mark_as_taken();
361 case 206: // M206 - set trim for each axis in mm
363 int dirx
= (this->home_direction
[0] ? 1 : -1);
364 int diry
= (this->home_direction
[1] ? 1 : -1);
365 int dirz
= (this->home_direction
[2] ? 1 : -1);
367 mm
[0]= trim
[0]/steps_per_mm
[0] * dirx
; // convert to mm
368 mm
[1]= trim
[1]/steps_per_mm
[1] * diry
;
369 mm
[2]= trim
[2]/steps_per_mm
[2] * dirz
;
371 if(gcode
->has_letter('X')) mm
[0]= gcode
->get_value('X');
372 if(gcode
->has_letter('Y')) mm
[1]= gcode
->get_value('Y');
373 if(gcode
->has_letter('Z')) mm
[2]= gcode
->get_value('Z');
375 trim
[0]= lround(mm
[0]*steps_per_mm
[0]) * dirx
; // convert back to steps
376 trim
[1]= lround(mm
[1]*steps_per_mm
[1]) * diry
;
377 trim
[2]= lround(mm
[2]*steps_per_mm
[2]) * dirz
;
379 // print the current trim values in mm and steps
381 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]);
382 gcode
->txt_after_ok
.append(buf
, n
);
383 gcode
->mark_as_taken();