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 // endstop trim used by deltas to do soft adjusting, in mm, convert to steps
75 this->trim
[0] = this->kernel
->config
->value(alpha_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[0];
76 this->trim
[1] = this->kernel
->config
->value(beta_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[0];
77 this->trim
[2] = this->kernel
->config
->value(gamma_trim_checksum
)->by_default(0 )->as_number() * steps_per_mm
[0];
79 this->debounce_count
= this->kernel
->config
->value(endstop_debounce_count_checksum
)->by_default(100)->as_number();
82 // get homing direction and convert to boolean where true is home to min, and false is home to max
83 int home_dir
= get_checksum(this->kernel
->config
->value(alpha_homing_direction_checksum
)->by_default("home_to_min")->as_string());
84 this->home_direction
[0] = home_dir
!= home_to_max_checksum
;
86 home_dir
= get_checksum(this->kernel
->config
->value(beta_homing_direction_checksum
)->by_default("home_to_min")->as_string());
87 this->home_direction
[1] = home_dir
!= home_to_max_checksum
;
89 home_dir
= get_checksum(this->kernel
->config
->value(gamma_homing_direction_checksum
)->by_default("home_to_min")->as_string());
90 this->home_direction
[2] = home_dir
!= home_to_max_checksum
;
92 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();
93 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();;
94 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();;
96 this->is_corexy
= this->kernel
->config
->value(corexy_homing_checksum
)->by_default(false)->as_bool();
99 void Endstops::wait_for_homed(char axes_to_move
){
101 unsigned int debounce
[3] = {0,0,0};
104 this->kernel
->call_event(ON_IDLE
);
105 for( char c
= 'X'; c
<= 'Z'; c
++ ){
106 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
107 if( this->pins
[c
- 'X' + (this->home_direction
[c
- 'X']?0:3)].get() ){
108 if( debounce
[c
- 'X'] < debounce_count
) {
109 debounce
[c
- 'X'] ++;
111 } else if ( this->steppers
[c
- 'X']->moving
){
112 this->steppers
[c
- 'X']->move(0,0);
115 // The endstop was not hit yet
117 debounce
[c
- 'X'] = 0;
124 // this homing works for cartesian and delta printers, not for HBots/CoreXY
125 void Endstops::do_homing(char axes_to_move
) {
126 // Start moving the axes to the origin
127 this->status
= MOVING_TO_ORIGIN_FAST
;
128 for( char c
= 'X'; c
<= 'Z'; c
++ ){
129 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
130 this->steppers
[c
- 'X']->set_speed(this->fast_rates
[c
- 'X']);
131 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
135 // Wait for all axes to have homed
136 this->wait_for_homed(axes_to_move
);
138 // Move back a small distance
139 this->status
= MOVING_BACK
;
141 for( char c
= 'X'; c
<= 'Z'; c
++ ){
142 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
143 inverted_dir
= !this->home_direction
[c
- 'X'];
144 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
145 this->steppers
[c
- 'X']->move(inverted_dir
,this->retract_steps
[c
- 'X']);
149 // Wait for moves to be done
150 for( char c
= 'X'; c
<= 'Z'; c
++ ){
151 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
152 while( this->steppers
[c
- 'X']->moving
){
153 this->kernel
->call_event(ON_IDLE
);
158 // Start moving the axes to the origin slowly
159 this->status
= MOVING_TO_ORIGIN_SLOW
;
160 for( char c
= 'X'; c
<= 'Z'; c
++ ){
161 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
162 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
-'X']);
163 this->steppers
[c
- 'X']->move(this->home_direction
[c
- 'X'],10000000);
167 // Wait for all axes to have homed
168 this->wait_for_homed(axes_to_move
);
170 // move for soft trim
171 this->status
= MOVING_BACK
;
172 for( char c
= 'X'; c
<= 'Z'; c
++ ){
173 if( this->trim
[c
- 'X'] != 0 && ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
174 inverted_dir
= !this->home_direction
[c
- 'X'];
175 // move up or down depending on sign of trim
176 if(this->trim
[c
- 'X'] < 0) inverted_dir
= !inverted_dir
;
177 this->steppers
[c
- 'X']->set_speed(this->slow_rates
[c
- 'X']);
178 this->steppers
[c
- 'X']->move(inverted_dir
,this->trim
[c
- 'X']);
182 // Wait for moves to be done
183 for( char c
= 'X'; c
<= 'Z'; c
++ ){
184 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
185 this->kernel
->streams
->printf("axis %c \r\n", c
);
186 while( this->steppers
[c
- 'X']->moving
){
187 this->kernel
->call_event(ON_IDLE
);
193 this->status
= NOT_HOMING
;
200 // this homing works for HBots/CoreXY
201 void Endstops::do_homing_corexy(char axes_to_move
) {
202 // Start moving the axes to the origin
203 if(axes_to_move
& 0x01) { // Home X, which means both X and Y in same direction
204 this->status
= MOVING_TO_ORIGIN_FAST
;
205 this->steppers
[X_AXIS
]->set_speed(this->fast_rates
[X_AXIS
]);
206 this->steppers
[X_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
207 this->steppers
[Y_AXIS
]->set_speed(this->fast_rates
[X_AXIS
]);
208 this->steppers
[Y_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
211 this->wait_for_homed(0x01);
213 // Move back a small distance
214 this->status
= MOVING_BACK
;
215 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
216 this->steppers
[X_AXIS
]->move(!this->home_direction
[X_AXIS
], this->retract_steps
[X_AXIS
]);
217 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
218 this->steppers
[Y_AXIS
]->move(!this->home_direction
[X_AXIS
], this->retract_steps
[X_AXIS
]);
221 while( this->steppers
[X_AXIS
]->moving
){ this->kernel
->call_event(ON_IDLE
); }
223 // Start moving the axes to the origin slowly
224 this->status
= MOVING_TO_ORIGIN_SLOW
;
225 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
226 this->steppers
[X_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
227 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[X_AXIS
]);
228 this->steppers
[Y_AXIS
]->move(this->home_direction
[X_AXIS
], 10000000);
231 this->wait_for_homed(0x01);
234 if(axes_to_move
& 0x02) { // Home Y, which means both X and Y in different directions
235 this->status
= MOVING_TO_ORIGIN_FAST
;
236 this->steppers
[X_AXIS
]->set_speed(this->fast_rates
[Y_AXIS
]);
237 this->steppers
[X_AXIS
]->move(this->home_direction
[Y_AXIS
], 10000000);
238 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
239 this->steppers
[Y_AXIS
]->move(!this->home_direction
[Y_AXIS
], 10000000);
242 this->wait_for_homed(0x02);
244 // Move back a small distance
245 this->status
= MOVING_BACK
;
246 this->steppers
[X_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
247 this->steppers
[X_AXIS
]->move(!this->home_direction
[Y_AXIS
], this->retract_steps
[Y_AXIS
]);
248 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
249 this->steppers
[Y_AXIS
]->move(this->home_direction
[Y_AXIS
], this->retract_steps
[Y_AXIS
]);
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
[Y_AXIS
]);
257 this->steppers
[X_AXIS
]->move(this->home_direction
[Y_AXIS
], 10000000);
258 this->steppers
[Y_AXIS
]->set_speed(this->slow_rates
[Y_AXIS
]);
259 this->steppers
[Y_AXIS
]->move(!this->home_direction
[Y_AXIS
], 10000000);
262 this->wait_for_homed(0x02);
265 if(axes_to_move
& 0x04) { // move Z
266 do_homing(0x04); // just home normally for Z
270 this->status
= NOT_HOMING
;
273 // Start homing sequences by response to GCode commands
274 void Endstops::on_gcode_received(void* argument
)
276 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
281 gcode
->mark_as_taken();
282 // G28 is received, we have homing to do
284 // First wait for the queue to be empty
285 this->kernel
->conveyor
->wait_for_empty_queue();
287 // Do we move select axes or all of them
288 char axes_to_move
= 0;
289 // only enable homing if the endstop is defined
290 bool home_all
= !( gcode
->has_letter('X') || gcode
->has_letter('Y') || gcode
->has_letter('Z') );
292 for( char c
= 'X'; c
<= 'Z'; c
++ ){
293 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' ) ); }
297 this->kernel
->stepper
->turn_enable_pins_on();
299 // do the actual homing
301 do_homing_corexy(axes_to_move
);
303 do_homing(axes_to_move
);
305 // Zero the ax(i/e)s position
306 for( char c
= 'X'; c
<= 'Z'; c
++ ){
307 if( ( axes_to_move
>> ( c
- 'X' ) ) & 1 ){
309 this->kernel
->robot
->reset_axis_position(this->homing_position
[c
- 'X'], c
- 'X');
315 else if (gcode
->has_m
){
320 int px
= this->home_direction
[0] ? 0 : 3;
321 int py
= this->home_direction
[1] ? 1 : 4;
322 int pz
= this->home_direction
[2] ? 2 : 5;
323 const char* mx
= this->home_direction
[0] ? "min" : "max";
324 const char* my
= this->home_direction
[1] ? "min" : "max";
325 const char* mz
= this->home_direction
[2] ? "min" : "max";
327 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());
328 gcode
->mark_as_taken();
332 case 206: // M206 - set trim for each axis in mm
334 int dirx
= (this->home_direction
[0] ? 1 : -1);
335 int diry
= (this->home_direction
[1] ? 1 : -1);
336 int dirz
= (this->home_direction
[2] ? 1 : -1);
338 mm
[0]= trim
[0]/steps_per_mm
[0] * dirx
; // convert to mm
339 mm
[1]= trim
[1]/steps_per_mm
[1] * diry
;
340 mm
[2]= trim
[2]/steps_per_mm
[2] * dirz
;
342 if(gcode
->has_letter('X')) mm
[0]= gcode
->get_value('X');
343 if(gcode
->has_letter('Y')) mm
[1]= gcode
->get_value('Y');
344 if(gcode
->has_letter('Z')) mm
[2]= gcode
->get_value('Z');
346 trim
[0]= lround(mm
[0]*steps_per_mm
[0]) * dirx
; // convert back to steps
347 trim
[1]= lround(mm
[1]*steps_per_mm
[1]) * diry
;
348 trim
[2]= lround(mm
[2]*steps_per_mm
[2]) * dirz
;
350 // print the current trim values in mm and steps
352 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]);
353 gcode
->txt_after_ok
.append(buf
, n
);
354 gcode
->mark_as_taken();