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();
92 this->is_delta
= this->kernel
->config
->value(delta_homing_checksum
)->by_default(false)->as_bool();
94 // endstop trim used by deltas to do soft adjusting, in mm, convert to steps, and negate depending on homing direction
95 // eg on a delta homing to max, a negative trim value will move the carriage down, and a positive will move it up
96 int dirx
= (this->home_direction
[0] ? 1 : -1);
97 int diry
= (this->home_direction
[1] ? 1 : -1);
98 int dirz
= (this->home_direction
[2] ? 1 : -1);
99 this->trim
[0]= this->kernel
->config
->value(alpha_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[0] * dirx
;
100 this->trim
[1]= this->kernel
->config
->value(beta_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[1] * diry
;
101 this->trim
[2]= this->kernel
->config
->value(gamma_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[2] * dirz
;
104 void Endstops::wait_for_homed(char axes_to_move
){
106 unsigned int debounce
[3] = {0,0,0};
109 this->kernel
->call_event(ON_IDLE
);
110 for( char c
= 'X'; c
<= 'Z'; c
++ ){
111 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
112 if( this->pins
[c
- 'X' + (this->home_direction
[c
- 'X']?0:3)].get() ){
113 if( debounce
[c
- 'X'] < debounce_count
) {
114 debounce
[c
- 'X'] ++;
116 } else if ( this->steppers
[c
- 'X']->moving
){
117 this->steppers
[c
- 'X']->move(0,0);
120 // The endstop was not hit yet
122 debounce
[c
- 'X'] = 0;
129 // this homing works for cartesian and delta printers, not for HBots/CoreXY
130 void Endstops::do_homing(char axes_to_move
) {
131 // Start moving the axes to the origin
132 this->status
= MOVING_TO_ORIGIN_FAST
;
133 for( char c
= 'X'; c
<= 'Z'; c
++ ){
134 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
135 this->steppers
[c
- 'X']->set_speed(this->fast_rates
[c
- 'X']);
136 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
140 // Wait for all axes to have homed
141 this->wait_for_homed(axes_to_move
);
143 // Move back a small distance
144 this->status
= MOVING_BACK
;
146 for( char c
= 'X'; c
<= 'Z'; c
++ ){
147 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
148 inverted_dir
= !this->home_direction
[c
- 'X'];
149 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
150 this->steppers
[c
- 'X']->move(inverted_dir
,this->retract_steps
[c
- 'X']);
154 // Wait for moves to be done
155 for( char c
= 'X'; c
<= 'Z'; c
++ ){
156 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
157 while( this->steppers
[c
- 'X']->moving
){
158 this->kernel
->call_event(ON_IDLE
);
163 // Start moving the axes to the origin slowly
164 this->status
= MOVING_TO_ORIGIN_SLOW
;
165 for( char c
= 'X'; c
<= 'Z'; c
++ ){
166 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
167 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
-'X']);
168 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
172 // Wait for all axes to have homed
173 this->wait_for_homed(axes_to_move
);
176 // move for soft trim
177 this->status
= MOVING_BACK
;
178 for( char c
= 'X'; c
<= 'Z'; c
++ ){
179 if( this->trim
[c
- 'X'] != 0 && ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
180 inverted_dir
= !this->home_direction
[c
- 'X'];
181 // move up or down depending on sign of trim
182 if(this->trim
[c
- 'X'] < 0) inverted_dir
= !inverted_dir
;
183 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
184 this->steppers
[c
- 'X']->move(inverted_dir
,this->trim
[c
- 'X']);
188 // Wait for moves to be done
189 for( char c
= 'X'; c
<= 'Z'; c
++ ){
190 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
191 //this->kernel->streams->printf("axis %c \r\n", c );
192 while( this->steppers
[c
- 'X']->moving
){
193 this->kernel
->call_event(ON_IDLE
);
200 this->status
= NOT_HOMING
;
207 void Endstops::wait_for_homed_corexy(int axis
){
209 unsigned int debounce
[3] = {0,0,0};
212 this->kernel
->call_event(ON_IDLE
);
213 if( this->pins
[axis
+ (this->home_direction
[axis
]?0:3)].get() ){
214 if( debounce
[axis
] < debounce_count
) {
218 // turn both off if running
219 if(this->steppers
[X_AXIS
]->moving
) this->steppers
[X_AXIS
]->move(0,0);
220 if(this->steppers
[Y_AXIS
]->moving
) this->steppers
[Y_AXIS
]->move(0,0);
223 // The endstop was not hit yet
230 // this homing works for HBots/CoreXY
231 void Endstops::do_homing_corexy(char axes_to_move
) {
232 // Start moving the axes to the origin
233 if(axes_to_move
& 0x01) { // Home X, which means both X and Y in same direction
234 this->status
= MOVING_TO_ORIGIN_FAST
;
235 this->steppers
[X_AXIS
]->set_speed(this->fast_rates
[X_AXIS
]);
236 this->steppers
[X_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
237 this->steppers
[Y_AXIS
]->set_speed(this->fast_rates
[X_AXIS
]);
238 this->steppers
[Y_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
241 this->wait_for_homed_corexy(X_AXIS
);
243 // Move back a small distance
244 this->status
= MOVING_BACK
;
245 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
246 this->steppers
[X_AXIS
]->move(!this->home_direction
[X_AXIS
], this->retract_steps
[X_AXIS
]);
247 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
248 this->steppers
[Y_AXIS
]->move(!this->home_direction
[X_AXIS
], this->retract_steps
[X_AXIS
]);
251 while( this->steppers
[X_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
252 while( this->steppers
[Y_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
254 // Start moving the axes to the origin slowly
255 this->status
= MOVING_TO_ORIGIN_SLOW
;
256 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
257 this->steppers
[X_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
258 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
259 this->steppers
[Y_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
262 this->wait_for_homed_corexy(X_AXIS
);
265 if(axes_to_move
& 0x02) { // Home Y, which means both X and Y in different directions
266 this->status
= MOVING_TO_ORIGIN_FAST
;
267 this->steppers
[X_AXIS
]->set_speed(this->fast_rates
[Y_AXIS
]);
268 this->steppers
[X_AXIS
]->move(this->home_direction
[Y_AXIS
], 10000000);
269 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
270 this->steppers
[Y_AXIS
]->move(!this->home_direction
[Y_AXIS
], 10000000);
273 this->wait_for_homed_corexy(Y_AXIS
);
275 // Move back a small distance
276 this->status
= MOVING_BACK
;
277 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
278 this->steppers
[X_AXIS
]->move(!this->home_direction
[Y_AXIS
], this->retract_steps
[Y_AXIS
]);
279 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
280 this->steppers
[Y_AXIS
]->move(this->home_direction
[Y_AXIS
], this->retract_steps
[Y_AXIS
]);
283 while( this->steppers
[X_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
284 while( this->steppers
[Y_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
286 // Start moving the axes to the origin slowly
287 this->status
= MOVING_TO_ORIGIN_SLOW
;
288 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
289 this->steppers
[X_AXIS
]->move(this->home_direction
[Y_AXIS
], 10000000);
290 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
291 this->steppers
[Y_AXIS
]->move(!this->home_direction
[Y_AXIS
], 10000000);
294 this->wait_for_homed_corexy(Y_AXIS
);
297 if(axes_to_move
& 0x04) { // move Z
298 do_homing(0x04); // just home normally for Z
302 this->status
= NOT_HOMING
;
305 // Start homing sequences by response to GCode commands
306 void Endstops::on_gcode_received(void* argument
)
308 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
313 gcode
->mark_as_taken();
314 // G28 is received, we have homing to do
316 // First wait for the queue to be empty
317 this->kernel
->conveyor
->wait_for_empty_queue();
319 // Do we move select axes or all of them
320 char axes_to_move
= 0;
321 // only enable homing if the endstop is defined, deltas always home all axis
322 bool home_all
= this->is_delta
|| !( gcode
->has_letter('X') || gcode
->has_letter('Y') || gcode
->has_letter('Z') );
324 for( char c
= 'X'; c
<= 'Z'; c
++ ){
325 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' ) ); }
329 this->kernel
->stepper
->turn_enable_pins_on();
331 // do the actual homing
333 do_homing_corexy(axes_to_move
);
335 do_homing(axes_to_move
);
337 // Zero the ax(i/e)s position
338 for( char c
= 'X'; c
<= 'Z'; c
++ ){
339 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
341 this->kernel
->robot
->reset_axis_position(this->homing_position
[c
- 'X'], c
- 'X');
347 else if (gcode
->has_m
){
352 int px
= this->home_direction
[0] ? 0 : 3;
353 int py
= this->home_direction
[1] ? 1 : 4;
354 int pz
= this->home_direction
[2] ? 2 : 5;
355 const char* mx
= this->home_direction
[0] ? "min" : "max";
356 const char* my
= this->home_direction
[1] ? "min" : "max";
357 const char* mz
= this->home_direction
[2] ? "min" : "max";
359 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());
360 gcode
->mark_as_taken();
364 case 206: // M206 - set trim for each axis in mm
366 int dirx
= (this->home_direction
[0] ? 1 : -1);
367 int diry
= (this->home_direction
[1] ? 1 : -1);
368 int dirz
= (this->home_direction
[2] ? 1 : -1);
370 mm
[0]= trim
[0]/steps_per_mm
[0] * dirx
; // convert to mm
371 mm
[1]= trim
[1]/steps_per_mm
[1] * diry
;
372 mm
[2]= trim
[2]/steps_per_mm
[2] * dirz
;
374 if(gcode
->has_letter('X')) mm
[0]= gcode
->get_value('X');
375 if(gcode
->has_letter('Y')) mm
[1]= gcode
->get_value('Y');
376 if(gcode
->has_letter('Z')) mm
[2]= gcode
->get_value('Z');
378 trim
[0]= lround(mm
[0]*steps_per_mm
[0]) * dirx
; // convert back to steps
379 trim
[1]= lround(mm
[1]*steps_per_mm
[1]) * diry
;
380 trim
[2]= lround(mm
[2]*steps_per_mm
[2]) * dirz
;
382 // print the current trim values in mm and steps
384 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]);
385 gcode
->txt_after_ok
.append(buf
, n
);
386 gcode
->mark_as_taken();