HCoop / clinton / Smoothieware.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
added an append to file streamthat appends and printfs to a file
[clinton/Smoothieware.git] / src / modules / tools / endstops / Endstops.cpp
CommitLineData
df27a6a3 1/*
201bcb94
AW		 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.
df27a6a3 5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
201bcb94
AW		 6*/
7
8#include "libs/Module.h"
9#include "libs/Kernel.h"
10#include "modules/communication/utils/Gcode.h"
3fceb8eb 11#include "modules/robot/Conveyor.h"
201bcb94
AW		 12#include "Endstops.h"
13#include "libs/nuts_bolts.h"
750277f8 14#include "libs/Pin.h"
670fa10b 15#include "libs/StepperMotor.h"
201bcb94
AW		 16#include "wait_api.h" // mbed.h lib
17
33e4cc02
JM		 18#define ALPHA_AXIS 0
19#define BETA_AXIS 1
20#define GAMMA_AXIS 2
21
22#define NOT_HOMING 0
23#define MOVING_TO_ORIGIN_FAST 1
24#define MOVING_BACK 2
25#define MOVING_TO_ORIGIN_SLOW 3
26
27#define endstops_module_enable_checksum CHECKSUM("endstops_enable")
28#define corexy_homing_checksum CHECKSUM("corexy_homing")
29#define delta_homing_checksum CHECKSUM("delta_homing")
30
31#define alpha_min_endstop_checksum CHECKSUM("alpha_min_endstop")
32#define beta_min_endstop_checksum CHECKSUM("beta_min_endstop")
33#define gamma_min_endstop_checksum CHECKSUM("gamma_min_endstop")
34
35#define alpha_max_endstop_checksum CHECKSUM("alpha_max_endstop")
36#define beta_max_endstop_checksum CHECKSUM("beta_max_endstop")
37#define gamma_max_endstop_checksum CHECKSUM("gamma_max_endstop")
38
39#define alpha_trim_checksum CHECKSUM("alpha_trim")
40#define beta_trim_checksum CHECKSUM("beta_trim")
41#define gamma_trim_checksum CHECKSUM("gamma_trim")
42
43// these values are in steps and should be deprecated
44#define alpha_fast_homing_rate_checksum CHECKSUM("alpha_fast_homing_rate")
45#define beta_fast_homing_rate_checksum CHECKSUM("beta_fast_homing_rate")
46#define gamma_fast_homing_rate_checksum CHECKSUM("gamma_fast_homing_rate")
47
48#define alpha_slow_homing_rate_checksum CHECKSUM("alpha_slow_homing_rate")
49#define beta_slow_homing_rate_checksum CHECKSUM("beta_slow_homing_rate")
50#define gamma_slow_homing_rate_checksum CHECKSUM("gamma_slow_homing_rate")
51
52#define alpha_homing_retract_checksum CHECKSUM("alpha_homing_retract")
53#define beta_homing_retract_checksum CHECKSUM("beta_homing_retract")
54#define gamma_homing_retract_checksum CHECKSUM("gamma_homing_retract")
55#define endstop_debounce_count_checksum CHECKSUM("endstop_debounce_count")
56
57// same as above but in user friendly mm/s and mm
58#define alpha_fast_homing_rate_mm_checksum CHECKSUM("alpha_fast_homing_rate_mm_s")
59#define beta_fast_homing_rate_mm_checksum CHECKSUM("beta_fast_homing_rate_mm_s")
60#define gamma_fast_homing_rate_mm_checksum CHECKSUM("gamma_fast_homing_rate_mm_s")
61
62#define alpha_slow_homing_rate_mm_checksum CHECKSUM("alpha_slow_homing_rate_mm_s")
63#define beta_slow_homing_rate_mm_checksum CHECKSUM("beta_slow_homing_rate_mm_s")
64#define gamma_slow_homing_rate_mm_checksum CHECKSUM("gamma_slow_homing_rate_mm_s")
65
66#define alpha_homing_retract_mm_checksum CHECKSUM("alpha_homing_retract_mm")
67#define beta_homing_retract_mm_checksum CHECKSUM("beta_homing_retract_mm")
68#define gamma_homing_retract_mm_checksum CHECKSUM("gamma_homing_retract_mm")
69
70#define endstop_debounce_count_checksum CHECKSUM("endstop_debounce_count")
71
72#define alpha_homing_direction_checksum CHECKSUM("alpha_homing_direction")
73#define beta_homing_direction_checksum CHECKSUM("beta_homing_direction")
74#define gamma_homing_direction_checksum CHECKSUM("gamma_homing_direction")
75#define home_to_max_checksum CHECKSUM("home_to_max")
76#define home_to_min_checksum CHECKSUM("home_to_min")
77#define alpha_min_checksum CHECKSUM("alpha_min")
78#define beta_min_checksum CHECKSUM("beta_min")
79#define gamma_min_checksum CHECKSUM("gamma_min")
80
81#define alpha_max_checksum CHECKSUM("alpha_max")
82#define beta_max_checksum CHECKSUM("beta_max")
83#define gamma_max_checksum CHECKSUM("gamma_max")
84
85#define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
86#define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
87#define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
88
89Endstops::Endstops()
90{
201bcb94 91 this->status = NOT_HOMING;
33e4cc02 92 home_offset[0] = home_offset[1] = home_offset[2] = 0.0F;
201bcb94
AW		 93}
94
33e4cc02
JM		 95void Endstops::on_module_loaded()
96{
7dee696d 97 // Do not do anything if not enabled
33e4cc02
JM		 98 if ( this->kernel->config->value( endstops_module_enable_checksum )->by_default(true)->as_bool() == false ) {
99 return;
100 }
7d62445b 101
476dcb96 102 register_for_event(ON_CONFIG_RELOAD);
201bcb94
AW		 103 this->register_for_event(ON_GCODE_RECEIVED);
104
105 // Take StepperMotor objects from Robot and keep them here
106 this->steppers[0] = this->kernel->robot->alpha_stepper_motor;
107 this->steppers[1] = this->kernel->robot->beta_stepper_motor;
108 this->steppers[2] = this->kernel->robot->gamma_stepper_motor;
109
750277f8
AW		 110 // Settings
111 this->on_config_reload(this);
c339d634 112
201bcb94
AW		 113}
114
750277f8 115// Get config
33e4cc02
JM		 116void Endstops::on_config_reload(void *argument)
117{
cc183397
A		 118 this->pins[0].from_string( this->kernel->config->value(alpha_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
119 this->pins[1].from_string( this->kernel->config->value(beta_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
120 this->pins[2].from_string( this->kernel->config->value(gamma_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
121 this->pins[3].from_string( this->kernel->config->value(alpha_max_endstop_checksum )->by_default("nc" )->as_string())->as_input();
122 this->pins[4].from_string( this->kernel->config->value(beta_max_endstop_checksum )->by_default("nc" )->as_string())->as_input();
123 this->pins[5].from_string( this->kernel->config->value(gamma_max_endstop_checksum )->by_default("nc" )->as_string())->as_input();
5de98d7c
JM		 124
125 // we need to know steps per mm for M206, also use them for all settings
47bbe224
JM		 126 this->steps_per_mm[0] = this->kernel->config->value(alpha_steps_per_mm_checksum )->as_number();
127 this->steps_per_mm[1] = this->kernel->config->value(beta_steps_per_mm_checksum )->as_number();
128 this->steps_per_mm[2] = this->kernel->config->value(gamma_steps_per_mm_checksum )->as_number();
129
5de98d7c
JM		 130 this->fast_rates[0] = this->kernel->config->value(alpha_fast_homing_rate_checksum )->by_default(4000 )->as_number();
131 this->fast_rates[1] = this->kernel->config->value(beta_fast_homing_rate_checksum )->by_default(4000 )->as_number();
132 this->fast_rates[2] = this->kernel->config->value(gamma_fast_homing_rate_checksum )->by_default(6400 )->as_number();
133 this->slow_rates[0] = this->kernel->config->value(alpha_slow_homing_rate_checksum )->by_default(2000 )->as_number();
134 this->slow_rates[1] = this->kernel->config->value(beta_slow_homing_rate_checksum )->by_default(2000 )->as_number();
135 this->slow_rates[2] = this->kernel->config->value(gamma_slow_homing_rate_checksum )->by_default(3200 )->as_number();
136 this->retract_steps[0] = this->kernel->config->value(alpha_homing_retract_checksum )->by_default(400 )->as_number();
137 this->retract_steps[1] = this->kernel->config->value(beta_homing_retract_checksum )->by_default(400 )->as_number();
138 this->retract_steps[2] = this->kernel->config->value(gamma_homing_retract_checksum )->by_default(1600 )->as_number();
139
140 // 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
33e4cc02
JM		 141 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];
142 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];
143 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];
144 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];
145 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];
146 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];
147 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];
148 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];
149 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];
5de98d7c 150
5de98d7c
JM		 151 this->debounce_count = this->kernel->config->value(endstop_debounce_count_checksum )->by_default(100)->as_number();
152
153
409ff5b3
JM		 154 // get homing direction and convert to boolean where true is home to min, and false is home to max
155 int home_dir = get_checksum(this->kernel->config->value(alpha_homing_direction_checksum)->by_default("home_to_min")->as_string());
eb09fdbf 156 this->home_direction[0] = home_dir != home_to_max_checksum;
47bbe224 157
409ff5b3 158 home_dir = get_checksum(this->kernel->config->value(beta_homing_direction_checksum)->by_default("home_to_min")->as_string());
eb09fdbf 159 this->home_direction[1] = home_dir != home_to_max_checksum;
47bbe224 160
409ff5b3 161 home_dir = get_checksum(this->kernel->config->value(gamma_homing_direction_checksum)->by_default("home_to_min")->as_string());
eb09fdbf 162 this->home_direction[2] = home_dir != home_to_max_checksum;
47bbe224 163
33e4cc02
JM		 164 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();
165 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();;
166 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();;
f29b0272
JM		 167
168 this->is_corexy = this->kernel->config->value(corexy_homing_checksum)->by_default(false)->as_bool();
959ab59c 169 this->is_delta = this->kernel->config->value(delta_homing_checksum)->by_default(false)->as_bool();
e678365e
JM		 170
171 // endstop trim used by deltas to do soft adjusting, in mm, convert to steps, and negate depending on homing direction
172 // eg on a delta homing to max, a negative trim value will move the carriage down, and a positive will move it up
33e4cc02
JM		 173 int dirx = (this->home_direction[0] ? 1 : -1);
174 int diry = (this->home_direction[1] ? 1 : -1);
175 int dirz = (this->home_direction[2] ? 1 : -1);
176 this->trim[0] = this->kernel->config->value(alpha_trim_checksum )->by_default(0 )->as_number() * steps_per_mm[0] * dirx;
177 this->trim[1] = this->kernel->config->value(beta_trim_checksum )->by_default(0 )->as_number() * steps_per_mm[1] * diry;
178 this->trim[2] = this->kernel->config->value(gamma_trim_checksum )->by_default(0 )->as_number() * steps_per_mm[2] * dirz;
a0e0d592
BG		 179}
180
33e4cc02
JM		 181void Endstops::wait_for_homed(char axes_to_move)
182{
a0e0d592 183 bool running = true;
33e4cc02
JM		 184 unsigned int debounce[3] = {0, 0, 0};
185 while (running) {
a0e0d592 186 running = false;
f3e93777 187 this->kernel->call_event(ON_IDLE);
33e4cc02
JM		 188 for ( char c = 'X'; c <= 'Z'; c++ ) {
189 if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
190 if ( this->pins[c - 'X' + (this->home_direction[c - 'X'] ? 0 : 3)].get() ) {
191 if ( debounce[c - 'X'] < debounce_count ) {
a0e0d592
BG		 192 debounce[c - 'X'] ++;
193 running = true;
33e4cc02
JM		 194 } else if ( this->steppers[c - 'X']->moving ) {
195 this->steppers[c - 'X']->move(0, 0);
a0e0d592 196 }
33e4cc02 197 } else {
a0e0d592
BG		 198 // The endstop was not hit yet
199 running = true;
200 debounce[c - 'X'] = 0;
323cca60 201 }
a0e0d592
BG		 202 }
203 }
204 }
750277f8 205}
201bcb94 206
f29b0272 207// this homing works for cartesian and delta printers, not for HBots/CoreXY
33e4cc02
JM		 208void Endstops::do_homing(char axes_to_move)
209{
f29b0272
JM		 210 // Start moving the axes to the origin
211 this->status = MOVING_TO_ORIGIN_FAST;
33e4cc02
JM		 212 for ( char c = 'X'; c <= 'Z'; c++ ) {
213 if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
f29b0272 214 this->steppers[c - 'X']->set_speed(this->fast_rates[c - 'X']);
33e4cc02 215 this->steppers[c - 'X']->move(this->home_direction[c - 'X'], 10000000);
f29b0272
JM		 216 }
217 }
218
219 // Wait for all axes to have homed
220 this->wait_for_homed(axes_to_move);
221
222 // Move back a small distance
223 this->status = MOVING_BACK;
224 bool inverted_dir;
33e4cc02
JM		 225 for ( char c = 'X'; c <= 'Z'; c++ ) {
226 if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
f29b0272
JM		 227 inverted_dir = !this->home_direction[c - 'X'];
228 this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
33e4cc02 229 this->steppers[c - 'X']->move(inverted_dir, this->retract_steps[c - 'X']);
f29b0272
JM		 230 }
231 }
232
33e4cc02
JM		 233 // Wait for moves to be done
234 for ( char c = 'X'; c <= 'Z'; c++ ) {
235 if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
236 while ( this->steppers[c - 'X']->moving ) {
f29b0272
JM		 237 this->kernel->call_event(ON_IDLE);
238 }
239 }
240 }
241
242 // Start moving the axes to the origin slowly
243 this->status = MOVING_TO_ORIGIN_SLOW;
33e4cc02
JM		 244 for ( char c = 'X'; c <= 'Z'; c++ ) {
245 if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
246 this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
247 this->steppers[c - 'X']->move(this->home_direction[c - 'X'], 10000000);
f29b0272
JM		 248 }
249 }
250
251 // Wait for all axes to have homed
252 this->wait_for_homed(axes_to_move);
253
33e4cc02 254 if (this->is_delta) {
959ab59c
JM		 255 // move for soft trim
256 this->status = MOVING_BACK;
33e4cc02
JM		 257 for ( char c = 'X'; c <= 'Z'; c++ ) {
258 if ( this->trim[c - 'X'] != 0 && ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
959ab59c
JM		 259 inverted_dir = !this->home_direction[c - 'X'];
260 // move up or down depending on sign of trim
33e4cc02 261 if (this->trim[c - 'X'] < 0) inverted_dir = !inverted_dir;
959ab59c 262 this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
33e4cc02 263 this->steppers[c - 'X']->move(inverted_dir, this->trim[c - 'X']);
959ab59c 264 }
f29b0272 265 }
f29b0272 266
959ab59c 267 // Wait for moves to be done
33e4cc02
JM		 268 for ( char c = 'X'; c <= 'Z'; c++ ) {
269 if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
959ab59c 270 //this->kernel->streams->printf("axis %c \r\n", c );
33e4cc02 271 while ( this->steppers[c - 'X']->moving ) {
959ab59c
JM		 272 this->kernel->call_event(ON_IDLE);
273 }
f29b0272
JM		 274 }
275 }
276 }
277
278 // Homing is done
279 this->status = NOT_HOMING;
280}
281
282#define X_AXIS 0
283#define Y_AXIS 1
284#define Z_AXIS 2
285
33e4cc02
JM		 286void Endstops::wait_for_homed_corexy(int axis)
287{
3db88866 288 bool running = true;
33e4cc02
JM		 289 unsigned int debounce[3] = {0, 0, 0};
290 while (running) {
3db88866
JM		 291 running = false;
292 this->kernel->call_event(ON_IDLE);
33e4cc02
JM		 293 if ( this->pins[axis + (this->home_direction[axis] ? 0 : 3)].get() ) {
294 if ( debounce[axis] < debounce_count ) {
3db88866
JM		 295 debounce[axis] ++;
296 running = true;
297 } else {
298 // turn both off if running
33e4cc02
JM		 299 if (this->steppers[X_AXIS]->moving) this->steppers[X_AXIS]->move(0, 0);
300 if (this->steppers[Y_AXIS]->moving) this->steppers[Y_AXIS]->move(0, 0);
3db88866 301 }
33e4cc02 302 } else {
3db88866
JM		 303 // The endstop was not hit yet
304 running = true;
305 debounce[axis] = 0;
306 }
307 }
308}
309
33e4cc02
JM		 310void Endstops::corexy_home(int home_axis, bool dirx, bool diry, double fast_rate, double slow_rate, unsigned int retract_steps)
311{
312 this->status = MOVING_TO_ORIGIN_FAST;
313 this->steppers[X_AXIS]->set_speed(fast_rate);
314 this->steppers[X_AXIS]->move(dirx, 10000000);
315 this->steppers[Y_AXIS]->set_speed(fast_rate);
316 this->steppers[Y_AXIS]->move(diry, 10000000);
f29b0272 317
33e4cc02
JM		 318 // wait for primary axis
319 this->wait_for_homed_corexy(home_axis);
f29b0272 320
33e4cc02
JM		 321 // Move back a small distance
322 this->status = MOVING_BACK;
323 this->steppers[X_AXIS]->set_speed(slow_rate);
324 this->steppers[X_AXIS]->move(!dirx, retract_steps);
325 this->steppers[Y_AXIS]->set_speed(slow_rate);
326 this->steppers[Y_AXIS]->move(!diry, retract_steps);
327
328 // wait until done
329 while ( this->steppers[X_AXIS]->moving ) {
330 this->kernel->call_event(ON_IDLE);
331 }
332 while ( this->steppers[Y_AXIS]->moving ) {
333 this->kernel->call_event(ON_IDLE);
f29b0272
JM		 334 }
335
33e4cc02
JM		 336 // Start moving the axes to the origin slowly
337 this->status = MOVING_TO_ORIGIN_SLOW;
338 this->steppers[X_AXIS]->set_speed(slow_rate);
339 this->steppers[X_AXIS]->move(dirx, 10000000);
340 this->steppers[Y_AXIS]->set_speed(slow_rate);
341 this->steppers[Y_AXIS]->move(diry, 10000000);
f29b0272 342
33e4cc02
JM		 343 // wait for primary axis
344 this->wait_for_homed_corexy(home_axis);
345}
f29b0272 346
33e4cc02
JM		 347// this homing works for HBots/CoreXY
348void Endstops::do_homing_corexy(char axes_to_move)
349{
350 // Home Y first so the X limit swicth canbe in a fixed pplace on the frame not on the X Gantry
351 // TODO should really make order configurable
352 if (axes_to_move & 0x02) { // Home Y, which means both X and Y in different directions
353 corexy_home(Y_AXIS, true, false, this->fast_rates[Y_AXIS], this->slow_rates[Y_AXIS], this->retract_steps[Y_AXIS]);
354 }
355
356 if (axes_to_move & 0x01) { // Home X, which means both X and Y in same direction
357 corexy_home(X_AXIS, true, true, this->fast_rates[X_AXIS], this->slow_rates[X_AXIS], this->retract_steps[X_AXIS]);
f29b0272
JM		 358 }
359
33e4cc02 360 if (axes_to_move & 0x04) { // move Z
f29b0272
JM		 361 do_homing(0x04); // just home normally for Z
362 }
363
364 // Homing is done
365 this->status = NOT_HOMING;
366}
367
201bcb94 368// Start homing sequences by response to GCode commands
33e4cc02 369void Endstops::on_gcode_received(void *argument)
c339d634 370{
33e4cc02
JM		 371 Gcode *gcode = static_cast<Gcode *>(argument);
372 if ( gcode->has_g) {
373 if ( gcode->g == 28 ) {
74b6303c 374 gcode->mark_as_taken();
df27a6a3 375 // G28 is received, we have homing to do
201bcb94
AW		 376
377 // First wait for the queue to be empty
40de2562 378 this->kernel->conveyor->wait_for_empty_queue();
201bcb94
AW		 379
380 // Do we move select axes or all of them
7dee696d 381 char axes_to_move = 0;
959ab59c 382 // only enable homing if the endstop is defined, deltas always home all axis
33e4cc02 383 bool home_all = this->is_delta || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
47bbe224 384
33e4cc02
JM		 385 for ( char c = 'X'; c <= 'Z'; c++ ) {
386 if ( (home_all || gcode->has_letter(c)) && this->pins[c - 'X' + (this->home_direction[c - 'X'] ? 0 : 3)].connected() ) {
387 axes_to_move += ( 1 << (c - 'X' ) );
388 }
df27a6a3 389 }
3b948656 390
f6c04440
AW		 391 // Enable the motors
392 this->kernel->stepper->turn_enable_pins_on();
393
f29b0272 394 // do the actual homing
33e4cc02 395 if (is_corexy)
f29b0272
JM		 396 do_homing_corexy(axes_to_move);
397 else
398 do_homing(axes_to_move);
3ffe27fb 399
33e4cc02
JM		 400 // Zero the ax(i/e)s position, add in the home offset
401 for ( int c = 0; c <= 2; c++ ) {
402 if ( (axes_to_move >> c) & 1 ) {
403 this->kernel->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
3ffe27fb
AV		 404 }
405 }
c339d634 406 }
33e4cc02
JM		 407 } else if (gcode->has_m) {
408 switch (gcode->m) {
409 case 119: {
410
411 int px = this->home_direction[0] ? 0 : 3;
412 int py = this->home_direction[1] ? 1 : 4;
413 int pz = this->home_direction[2] ? 2 : 5;
414 const char *mx = this->home_direction[0] ? "min" : "max";
415 const char *my = this->home_direction[1] ? "min" : "max";
416 const char *mz = this->home_direction[2] ? "min" : "max";
417
418 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());
419 gcode->mark_as_taken();
420 }
421 break;
422
423 case 206: // M206 - set homing offset
424 if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X');
425 if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y');
426 if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z');
427 gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
428 gcode->mark_as_taken();
429 break;
430
431 case 500: // save settings
432 case 503: // print settings
433 gcode->stream->printf(";Home offset (mm):\nM206 X%1.2f Y%1.2f Z%1.2f\n", home_offset[0], home_offset[1], home_offset[2]);
434 if (is_delta) {
435 double mm[3];
436 trim2mm(mm);
437 gcode->stream->printf(";Trim (mm):\nM666 X%1.2f Y%1.2f Z%1.2f\n", mm[0], mm[1], mm[2]);
438 gcode->stream->printf(";Max Z\nM665 Z%1.2f\n", this->homing_position[2]);
7a8fe6e0 439 }
33e4cc02 440 gcode->mark_as_taken();
c339d634 441 break;
47bbe224 442
ec4773e5 443 case 665: { // M665 - set max gamma/z height
33e4cc02
JM		 444 gcode->mark_as_taken();
445 double gamma_max = this->homing_position[2];
446 if (gcode->has_letter('Z')) {
447 this->homing_position[2] = gamma_max = gcode->get_value('Z');
ec4773e5 448 }
33e4cc02
JM		 449 gcode->stream->printf("Max Z %8.3f ", gamma_max);
450 gcode->add_nl = true;
451 }
452 break;
ec4773e5 453
47bbe224 454
33e4cc02
JM		 455 case 666: { // M666 - set trim for each axis in mm
456 double mm[3];
457 trim2mm(mm);
47bbe224 458
33e4cc02
JM		 459 if (gcode->has_letter('X')) mm[0] = gcode->get_value('X');
460 if (gcode->has_letter('Y')) mm[1] = gcode->get_value('Y');
461 if (gcode->has_letter('Z')) mm[2] = gcode->get_value('Z');
47bbe224 462
33e4cc02
JM		 463 int dirx = (this->home_direction[0] ? 1 : -1);
464 int diry = (this->home_direction[1] ? 1 : -1);
465 int dirz = (this->home_direction[2] ? 1 : -1);
466 trim[0] = lround(mm[0] * steps_per_mm[0]) * dirx; // convert back to steps
467 trim[1] = lround(mm[1] * steps_per_mm[1]) * diry;
468 trim[2] = lround(mm[2] * steps_per_mm[2]) * dirz;
469
470 // print the current trim values in mm and steps
471 gcode->stream->printf("X %5.3f (%d) Y %5.3f (%d) Z %5.3f (%d)\n", mm[0], trim[0], mm[1], trim[1], mm[2], trim[2]);
472 gcode->mark_as_taken();
473 }
474 break;
47bbe224 475
201bcb94
AW		 476 }
477 }
478}
479
33e4cc02
JM		 480void Endstops::trim2mm(double *mm)
481{
482 int dirx = (this->home_direction[0] ? 1 : -1);
483 int diry = (this->home_direction[1] ? 1 : -1);
484 int dirz = (this->home_direction[2] ? 1 : -1);
485
486 mm[0] = this->trim[0] / this->steps_per_mm[0] * dirx; // convert to mm
487 mm[1] = this->trim[1] / this->steps_per_mm[1] * diry;
488 mm[2] = this->trim[2] / this->steps_per_mm[2] * dirz;
489}
490
Unnamed repository; edit this file 'description' to name the repository.