src/modules/tools/endstops/Endstops.cpp

   1 /*
   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/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "modules/communication/utils/Gcode.h"
  11 #include "modules/robot/Conveyor.h"
  12 #include "Endstops.h"
  13 #include "libs/nuts_bolts.h"
  14 #include "libs/Pin.h"
  15 #include "libs/StepperMotor.h"
  16 #include "wait_api.h" // mbed.h lib
  17
  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
  89 Endstops::Endstops()
  90 {
  91     this->status = NOT_HOMING;
  92     home_offset[0] = home_offset[1] = home_offset[2] = 0.0F;
  93 }
  94
  95 void Endstops::on_module_loaded()
  96 {
  97     // Do not do anything if not enabled
  98     if ( this->kernel->config->value( endstops_module_enable_checksum )->by_default(true)->as_bool() == false ) {
  99         return;
 100     }
 101
 102     register_for_event(ON_CONFIG_RELOAD);
 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
 110     // Settings
 111     this->on_config_reload(this);
 112
 113 }
 114
 115 // Get config
 116 void Endstops::on_config_reload(void *argument)
 117 {
 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();
 124
 125     // we need to know steps per mm for M206, also use them for all settings
 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
 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
 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];
 150
 151     this->debounce_count  = this->kernel->config->value(endstop_debounce_count_checksum    )->by_default(100)->as_number();
 152
 153
 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());
 156     this->home_direction[0]         = home_dir != home_to_max_checksum;
 157
 158     home_dir                        = get_checksum(this->kernel->config->value(beta_homing_direction_checksum)->by_default("home_to_min")->as_string());
 159     this->home_direction[1]         = home_dir != home_to_max_checksum;
 160
 161     home_dir                        = get_checksum(this->kernel->config->value(gamma_homing_direction_checksum)->by_default("home_to_min")->as_string());
 162     this->home_direction[2]         = home_dir != home_to_max_checksum;
 163
 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();;
 167
 168     this->is_corexy                 =  this->kernel->config->value(corexy_homing_checksum)->by_default(false)->as_bool();
 169     this->is_delta                  =  this->kernel->config->value(delta_homing_checksum)->by_default(false)->as_bool();
 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
 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;
 179 }
 180
 181 void Endstops::wait_for_homed(char axes_to_move)
 182 {
 183     bool running = true;
 184     unsigned int debounce[3] = {0, 0, 0};
 185     while (running) {
 186         running = false;
 187         this->kernel->call_event(ON_IDLE);
 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 ) {
 192                         debounce[c - 'X'] ++;
 193                         running = true;
 194                     } else if ( this->steppers[c - 'X']->moving ) {
 195                         this->steppers[c - 'X']->move(0, 0);
 196                     }
 197                 } else {
 198                     // The endstop was not hit yet
 199                     running = true;
 200                     debounce[c - 'X'] = 0;
 201                 }
 202             }
 203         }
 204     }
 205 }
 206
 207 // this homing works for cartesian and delta printers, not for HBots/CoreXY
 208 void Endstops::do_homing(char axes_to_move)
 209 {
 210     // Start moving the axes to the origin
 211     this->status = MOVING_TO_ORIGIN_FAST;
 212     for ( char c = 'X'; c <= 'Z'; c++ ) {
 213         if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
 214             this->steppers[c - 'X']->set_speed(this->fast_rates[c - 'X']);
 215             this->steppers[c - 'X']->move(this->home_direction[c - 'X'], 10000000);
 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;
 225     for ( char c = 'X'; c <= 'Z'; c++ ) {
 226         if ( ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
 227             inverted_dir = !this->home_direction[c - 'X'];
 228             this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
 229             this->steppers[c - 'X']->move(inverted_dir, this->retract_steps[c - 'X']);
 230         }
 231     }
 232
 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 ) {
 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;
 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);
 248         }
 249     }
 250
 251     // Wait for all axes to have homed
 252     this->wait_for_homed(axes_to_move);
 253
 254     if (this->is_delta) {
 255         // move for soft trim
 256         this->status = MOVING_BACK;
 257         for ( char c = 'X'; c <= 'Z'; c++ ) {
 258             if ( this->trim[c - 'X'] != 0 && ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
 259                 inverted_dir = !this->home_direction[c - 'X'];
 260                 // move up or down depending on sign of trim
 261                 if (this->trim[c - 'X'] < 0) inverted_dir = !inverted_dir;
 262                 this->steppers[c - 'X']->set_speed(this->slow_rates[c - 'X']);
 263                 this->steppers[c - 'X']->move(inverted_dir, this->trim[c - 'X']);
 264             }
 265         }
 266
 267         // Wait for moves to be done
 268         for ( char c = 'X'; c <= 'Z'; c++ ) {
 269             if (  ( axes_to_move >> ( c - 'X' ) ) & 1 ) {
 270                 //this->kernel->streams->printf("axis %c \r\n", c );
 271                 while ( this->steppers[c - 'X']->moving ) {
 272                     this->kernel->call_event(ON_IDLE);
 273                 }
 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
 286 void Endstops::wait_for_homed_corexy(int axis)
 287 {
 288     bool running = true;
 289     unsigned int debounce[3] = {0, 0, 0};
 290     while (running) {
 291         running = false;
 292         this->kernel->call_event(ON_IDLE);
 293         if ( this->pins[axis + (this->home_direction[axis] ? 0 : 3)].get() ) {
 294             if ( debounce[axis] < debounce_count ) {
 295                 debounce[axis] ++;
 296                 running = true;
 297             } else {
 298                 // turn both off if running
 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);
 301             }
 302         } else {
 303             // The endstop was not hit yet
 304             running = true;
 305             debounce[axis] = 0;
 306         }
 307     }
 308 }
 309
 310 void 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);
 317
 318     // wait for primary axis
 319     this->wait_for_homed_corexy(home_axis);
 320
 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);
 334     }
 335
 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);
 342
 343     // wait for primary axis
 344     this->wait_for_homed_corexy(home_axis);
 345 }
 346
 347 // this homing works for HBots/CoreXY
 348 void 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]);
 358     }
 359
 360     if (axes_to_move & 0x04) { // move Z
 361         do_homing(0x04); // just home normally for Z
 362     }
 363
 364     // Homing is done
 365     this->status = NOT_HOMING;
 366 }
 367
 368 // Start homing sequences by response to GCode commands
 369 void Endstops::on_gcode_received(void *argument)
 370 {
 371     Gcode *gcode = static_cast<Gcode *>(argument);
 372     if ( gcode->has_g) {
 373         if ( gcode->g == 28 ) {
 374             gcode->mark_as_taken();
 375             // G28 is received, we have homing to do
 376
 377             // First wait for the queue to be empty
 378             this->kernel->conveyor->wait_for_empty_queue();
 379
 380             // Do we move select axes or all of them
 381             char axes_to_move = 0;
 382             // only enable homing if the endstop is defined, deltas always home all axis
 383             bool home_all = this->is_delta || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
 384
 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                 }
 389             }
 390
 391             // Enable the motors
 392             this->kernel->stepper->turn_enable_pins_on();
 393
 394             // do the actual homing
 395             if (is_corexy)
 396                 do_homing_corexy(axes_to_move);
 397             else
 398                 do_homing(axes_to_move);
 399
 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);
 404                 }
 405             }
 406         }
 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]);
 439                 }
 440                 gcode->mark_as_taken();
 441                 break;
 442
 443             case 665: { // M665 - set max gamma/z height
 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');
 448                 }
 449                 gcode->stream->printf("Max Z %8.3f ", gamma_max);
 450                 gcode->add_nl = true;
 451             }
 452             break;
 453
 454
 455             case 666: { // M666 - set trim for each axis in mm
 456                 double mm[3];
 457                 trim2mm(mm);
 458
 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');
 462
 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;
 475
 476         }
 477     }
 478 }
 479
 480 void 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