X-Git-Url: https://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/8fe38353cdf2468954dc5c2e952496c0c2a4823a..927cd05f93b84627a7e56bebf3a7d959cd35f774:/src/modules/tools/endstops/Endstops.cpp diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index a16bd9ed..efc7d154 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -31,91 +31,69 @@ #include "SerialMessage.h" #include +#include -#define ALPHA_AXIS 0 -#define BETA_AXIS 1 -#define GAMMA_AXIS 2 -#define X_AXIS 0 -#define Y_AXIS 1 -#define Z_AXIS 2 - +// OLD deprecated syntax #define endstops_module_enable_checksum CHECKSUM("endstops_enable") -#define corexy_homing_checksum CHECKSUM("corexy_homing") -#define delta_homing_checksum CHECKSUM("delta_homing") -#define rdelta_homing_checksum CHECKSUM("rdelta_homing") -#define scara_homing_checksum CHECKSUM("scara_homing") - -#define alpha_min_endstop_checksum CHECKSUM("alpha_min_endstop") -#define beta_min_endstop_checksum CHECKSUM("beta_min_endstop") -#define gamma_min_endstop_checksum CHECKSUM("gamma_min_endstop") - -#define alpha_max_endstop_checksum CHECKSUM("alpha_max_endstop") -#define beta_max_endstop_checksum CHECKSUM("beta_max_endstop") -#define gamma_max_endstop_checksum CHECKSUM("gamma_max_endstop") - -#define alpha_trim_checksum CHECKSUM("alpha_trim") -#define beta_trim_checksum CHECKSUM("beta_trim") -#define gamma_trim_checksum CHECKSUM("gamma_trim") - -#define alpha_max_travel_checksum CHECKSUM("alpha_max_travel") -#define beta_max_travel_checksum CHECKSUM("beta_max_travel") -#define gamma_max_travel_checksum CHECKSUM("gamma_max_travel") - -// these values are in steps and should be deprecated -#define alpha_fast_homing_rate_checksum CHECKSUM("alpha_fast_homing_rate") -#define beta_fast_homing_rate_checksum CHECKSUM("beta_fast_homing_rate") -#define gamma_fast_homing_rate_checksum CHECKSUM("gamma_fast_homing_rate") - -#define alpha_slow_homing_rate_checksum CHECKSUM("alpha_slow_homing_rate") -#define beta_slow_homing_rate_checksum CHECKSUM("beta_slow_homing_rate") -#define gamma_slow_homing_rate_checksum CHECKSUM("gamma_slow_homing_rate") - -#define alpha_homing_retract_checksum CHECKSUM("alpha_homing_retract") -#define beta_homing_retract_checksum CHECKSUM("beta_homing_retract") -#define gamma_homing_retract_checksum CHECKSUM("gamma_homing_retract") -// same as above but in user friendly mm/s and mm -#define alpha_fast_homing_rate_mm_checksum CHECKSUM("alpha_fast_homing_rate_mm_s") -#define beta_fast_homing_rate_mm_checksum CHECKSUM("beta_fast_homing_rate_mm_s") -#define gamma_fast_homing_rate_mm_checksum CHECKSUM("gamma_fast_homing_rate_mm_s") +#define ENDSTOP_CHECKSUMS(X) { \ + CHECKSUM(X "_min_endstop"), \ + CHECKSUM(X "_max_endstop"), \ + CHECKSUM(X "_max_travel"), \ + CHECKSUM(X "_fast_homing_rate_mm_s"), \ + CHECKSUM(X "_slow_homing_rate_mm_s"), \ + CHECKSUM(X "_homing_retract_mm"), \ + CHECKSUM(X "_homing_direction"), \ + CHECKSUM(X "_min"), \ + CHECKSUM(X "_max"), \ + CHECKSUM(X "_limit_enable"), \ +} -#define alpha_slow_homing_rate_mm_checksum CHECKSUM("alpha_slow_homing_rate_mm_s") -#define beta_slow_homing_rate_mm_checksum CHECKSUM("beta_slow_homing_rate_mm_s") -#define gamma_slow_homing_rate_mm_checksum CHECKSUM("gamma_slow_homing_rate_mm_s") +// checksum defns +enum DEFNS {MIN_PIN, MAX_PIN, MAX_TRAVEL, FAST_RATE, SLOW_RATE, RETRACT, DIRECTION, MIN, MAX, LIMIT, NDEFNS}; -#define alpha_homing_retract_mm_checksum CHECKSUM("alpha_homing_retract_mm") -#define beta_homing_retract_mm_checksum CHECKSUM("beta_homing_retract_mm") -#define gamma_homing_retract_mm_checksum CHECKSUM("gamma_homing_retract_mm") +// global config settings +#define corexy_homing_checksum CHECKSUM("corexy_homing") +#define delta_homing_checksum CHECKSUM("delta_homing") +#define rdelta_homing_checksum CHECKSUM("rdelta_homing") +#define scara_homing_checksum CHECKSUM("scara_homing") #define endstop_debounce_count_checksum CHECKSUM("endstop_debounce_count") #define endstop_debounce_ms_checksum CHECKSUM("endstop_debounce_ms") -#define alpha_homing_direction_checksum CHECKSUM("alpha_homing_direction") -#define beta_homing_direction_checksum CHECKSUM("beta_homing_direction") -#define gamma_homing_direction_checksum CHECKSUM("gamma_homing_direction") - -#define alpha_min_checksum CHECKSUM("alpha_min") -#define beta_min_checksum CHECKSUM("beta_min") -#define gamma_min_checksum CHECKSUM("gamma_min") - -#define alpha_max_checksum CHECKSUM("alpha_max") -#define beta_max_checksum CHECKSUM("beta_max") -#define gamma_max_checksum CHECKSUM("gamma_max") - -#define alpha_limit_enable_checksum CHECKSUM("alpha_limit_enable") -#define beta_limit_enable_checksum CHECKSUM("beta_limit_enable") -#define gamma_limit_enable_checksum CHECKSUM("gamma_limit_enable") - #define home_z_first_checksum CHECKSUM("home_z_first") #define homing_order_checksum CHECKSUM("homing_order") #define move_to_origin_checksum CHECKSUM("move_to_origin_after_home") +#define alpha_trim_checksum CHECKSUM("alpha_trim_mm") +#define beta_trim_checksum CHECKSUM("beta_trim_mm") +#define gamma_trim_checksum CHECKSUM("gamma_trim_mm") + +// new config syntax +// endstop.xmin.enable true +// endstop.xmin.pin 1.29 +// endstop.xmin.axis X +// endstop.xmin.homing_direction home_to_min + +#define endstop_checksum CHECKSUM("endstop") +#define enable_checksum CHECKSUM("enable") +#define pin_checksum CHECKSUM("pin") +#define axis_checksum CHECKSUM("axis") +#define direction_checksum CHECKSUM("homing_direction") +#define position_checksum CHECKSUM("homing_position") +#define fast_rate_checksum CHECKSUM("fast_rate") +#define slow_rate_checksum CHECKSUM("slow_rate") +#define max_travel_checksum CHECKSUM("max_travel") +#define retract_checksum CHECKSUM("retract") +#define limit_checksum CHECKSUM("limit_enable") + #define STEPPER THEROBOT->actuators #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) + // Homing States -enum { +enum STATES { MOVING_TO_ENDSTOP_FAST, // homing move MOVING_TO_ENDSTOP_SLOW, // homing move MOVING_BACK, // homing move @@ -128,132 +106,110 @@ enum { Endstops::Endstops() { this->status = NOT_HOMING; - home_offset[0] = home_offset[1] = home_offset[2] = 0.0F; - debounce.fill(0); } void Endstops::on_module_loaded() { - // Do not do anything if not enabled - if ( THEKERNEL->config->value( endstops_module_enable_checksum )->by_default(true)->as_bool() == false ) { - delete this; - return; + // Do not do anything if not enabled or if no pins are defined + if (THEKERNEL->config->value( endstops_module_enable_checksum )->by_default(false)->as_bool()) { + if(!load_old_config()) { + delete this; + return; + } + + }else{ + // check for new config syntax + if(!load_config()) { + delete this; + return; + } } register_for_event(ON_GCODE_RECEIVED); register_for_event(ON_GET_PUBLIC_DATA); register_for_event(ON_SET_PUBLIC_DATA); - // Settings - this->load_config(); THEKERNEL->slow_ticker->attach(1000, this, &Endstops::read_endstops); } -// Get config -void Endstops::load_config() +// Get config using old deprecated syntax Does not support ABC +bool Endstops::load_old_config() { - this->pins[0].from_string( THEKERNEL->config->value(alpha_min_endstop_checksum )->by_default("nc" )->as_string())->as_input(); - this->pins[1].from_string( THEKERNEL->config->value(beta_min_endstop_checksum )->by_default("nc" )->as_string())->as_input(); - this->pins[2].from_string( THEKERNEL->config->value(gamma_min_endstop_checksum )->by_default("nc" )->as_string())->as_input(); - this->pins[3].from_string( THEKERNEL->config->value(alpha_max_endstop_checksum )->by_default("nc" )->as_string())->as_input(); - this->pins[4].from_string( THEKERNEL->config->value(beta_max_endstop_checksum )->by_default("nc" )->as_string())->as_input(); - this->pins[5].from_string( THEKERNEL->config->value(gamma_max_endstop_checksum )->by_default("nc" )->as_string())->as_input(); - - // These are the old ones in steps still here for backwards compatibility - this->fast_rates[0] = THEKERNEL->config->value(alpha_fast_homing_rate_checksum )->by_default(4000 )->as_number() / STEPS_PER_MM(0); - this->fast_rates[1] = THEKERNEL->config->value(beta_fast_homing_rate_checksum )->by_default(4000 )->as_number() / STEPS_PER_MM(1); - this->fast_rates[2] = THEKERNEL->config->value(gamma_fast_homing_rate_checksum )->by_default(6400 )->as_number() / STEPS_PER_MM(2); - this->slow_rates[0] = THEKERNEL->config->value(alpha_slow_homing_rate_checksum )->by_default(2000 )->as_number() / STEPS_PER_MM(0); - this->slow_rates[1] = THEKERNEL->config->value(beta_slow_homing_rate_checksum )->by_default(2000 )->as_number() / STEPS_PER_MM(1); - this->slow_rates[2] = THEKERNEL->config->value(gamma_slow_homing_rate_checksum )->by_default(3200 )->as_number() / STEPS_PER_MM(2); - this->retract_mm[0] = THEKERNEL->config->value(alpha_homing_retract_checksum )->by_default(400 )->as_number() / STEPS_PER_MM(0); - this->retract_mm[1] = THEKERNEL->config->value(beta_homing_retract_checksum )->by_default(400 )->as_number() / STEPS_PER_MM(1); - this->retract_mm[2] = THEKERNEL->config->value(gamma_homing_retract_checksum )->by_default(1600 )->as_number() / STEPS_PER_MM(2); - - // 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 - this->fast_rates[0] = THEKERNEL->config->value(alpha_fast_homing_rate_mm_checksum )->by_default(this->fast_rates[0])->as_number(); - this->fast_rates[1] = THEKERNEL->config->value(beta_fast_homing_rate_mm_checksum )->by_default(this->fast_rates[1])->as_number(); - this->fast_rates[2] = THEKERNEL->config->value(gamma_fast_homing_rate_mm_checksum )->by_default(this->fast_rates[2])->as_number(); - this->slow_rates[0] = THEKERNEL->config->value(alpha_slow_homing_rate_mm_checksum )->by_default(this->slow_rates[0])->as_number(); - this->slow_rates[1] = THEKERNEL->config->value(beta_slow_homing_rate_mm_checksum )->by_default(this->slow_rates[1])->as_number(); - this->slow_rates[2] = THEKERNEL->config->value(gamma_slow_homing_rate_mm_checksum )->by_default(this->slow_rates[2])->as_number(); - this->retract_mm[0] = THEKERNEL->config->value(alpha_homing_retract_mm_checksum )->by_default(this->retract_mm[0])->as_number(); - this->retract_mm[1] = THEKERNEL->config->value(beta_homing_retract_mm_checksum )->by_default(this->retract_mm[1])->as_number(); - this->retract_mm[2] = THEKERNEL->config->value(gamma_homing_retract_mm_checksum )->by_default(this->retract_mm[2])->as_number(); - - // NOTE the debouce count is in milliseconds so probably does not need to beset anymore - this->debounce_ms= THEKERNEL->config->value(endstop_debounce_ms_checksum )->by_default(0)->as_number(); - this->debounce_count= THEKERNEL->config->value(endstop_debounce_count_checksum )->by_default(100)->as_number(); - - // get homing direction and convert to boolean where true is home to min, and false is home to max - this->home_direction[0]= THEKERNEL->config->value(alpha_homing_direction_checksum)->by_default("home_to_min")->as_string() != "home_to_max"; - this->home_direction[1]= THEKERNEL->config->value(beta_homing_direction_checksum)->by_default("home_to_min")->as_string() != "home_to_max"; - this->home_direction[2]= THEKERNEL->config->value(gamma_homing_direction_checksum)->by_default("home_to_min")->as_string() != "home_to_max"; - - this->homing_position[0]= this->home_direction[0] ? THEKERNEL->config->value(alpha_min_checksum)->by_default(0)->as_number() : THEKERNEL->config->value(alpha_max_checksum)->by_default(200)->as_number(); - this->homing_position[1]= this->home_direction[1] ? THEKERNEL->config->value(beta_min_checksum )->by_default(0)->as_number() : THEKERNEL->config->value(beta_max_checksum )->by_default(200)->as_number(); - this->homing_position[2]= this->home_direction[2] ? THEKERNEL->config->value(gamma_min_checksum)->by_default(0)->as_number() : THEKERNEL->config->value(gamma_max_checksum)->by_default(200)->as_number(); - - // used to set maximum movement on homing, set by alpha_max_travel if defined - // for backward compatibility uses alpha_max if not defined. - // TO BE deprecated - this->alpha_max= THEKERNEL->config->value(alpha_max_checksum)->by_default(500)->as_number(); - this->beta_max= THEKERNEL->config->value(beta_max_checksum)->by_default(500)->as_number(); - this->gamma_max= THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); - - this->alpha_max= THEKERNEL->config->value(alpha_max_travel_checksum)->by_default(alpha_max*2)->as_number(); - this->beta_max= THEKERNEL->config->value(beta_max_travel_checksum)->by_default(beta_max*2)->as_number(); - this->gamma_max= THEKERNEL->config->value(gamma_max_travel_checksum)->by_default(gamma_max*2)->as_number(); - - this->is_corexy = THEKERNEL->config->value(corexy_homing_checksum)->by_default(false)->as_bool(); - this->is_delta = THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool(); - this->is_rdelta = THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool(); - this->is_scara = THEKERNEL->config->value(scara_homing_checksum)->by_default(false)->as_bool(); - - this->home_z_first = THEKERNEL->config->value(home_z_first_checksum)->by_default(false)->as_bool(); - - // see if an order has been specified, must be three characters, XYZ or YXZ etc - string order = THEKERNEL->config->value(homing_order_checksum)->by_default("")->as_string(); - this->homing_order = 0; - if(order.size() == 3 && !(this->is_delta || this->is_rdelta)) { - int shift = 0; - for(auto c : order) { - uint8_t i = toupper(c) - 'X'; - if(i > 2) { // bad value - this->homing_order = 0; - break; + uint16_t const checksums[][NDEFNS] = { + ENDSTOP_CHECKSUMS("alpha"), // X + ENDSTOP_CHECKSUMS("beta"), // Y + ENDSTOP_CHECKSUMS("gamma") // Z + }; + + bool limit_enabled= false; + for (int i = X_AXIS; i <= Z_AXIS; ++i) { // X_AXIS to Z_AXIS + homing_info_t hinfo; + + // init homing struct + hinfo.home_offset= 0; + hinfo.homed= false; + hinfo.axis= 'X'+i; + hinfo.axis_index= i; + hinfo.pin_info= nullptr; + + // rates in mm/sec + hinfo.fast_rate= THEKERNEL->config->value(checksums[i][FAST_RATE])->by_default(100)->as_number(); + hinfo.slow_rate= THEKERNEL->config->value(checksums[i][SLOW_RATE])->by_default(10)->as_number(); + + // retract in mm + hinfo.retract= THEKERNEL->config->value(checksums[i][RETRACT])->by_default(5)->as_number(); + + // get homing direction and convert to boolean where true is home to min, and false is home to max + hinfo.home_direction= THEKERNEL->config->value(checksums[i][DIRECTION])->by_default("home_to_min")->as_string() != "home_to_max"; + + // homing cartesian position + hinfo.homing_position= hinfo.home_direction ? THEKERNEL->config->value(checksums[i][MIN])->by_default(0)->as_number() : THEKERNEL->config->value(checksums[i][MAX])->by_default(200)->as_number(); + + // used to set maximum movement on homing, set by alpha_max_travel if defined + hinfo.max_travel= THEKERNEL->config->value(checksums[i][MAX_TRAVEL])->by_default(500)->as_number(); + + + // pin definitions for endstop pins + for (int j = MIN_PIN; j <= MAX_PIN; ++j) { + endstop_info_t *info= new endstop_info_t; + info->pin.from_string(THEKERNEL->config->value(checksums[i][j])->by_default("nc" )->as_string())->as_input(); + if(!info->pin.connected()){ + // no pin defined try next + delete info; + continue; } - homing_order |= (i << shift); - shift += 2; + + // enter into endstop array + endstops.push_back(info); + + // add index to the homing struct if this is the one used for homing + if((hinfo.home_direction && j == MIN_PIN) || (!hinfo.home_direction && j == MAX_PIN)) hinfo.pin_info= info; + + // init struct + info->debounce= 0; + info->axis= 'X'+i; + info->axis_index= i; + + // limits enabled + info->limit_enable= THEKERNEL->config->value(checksums[i][LIMIT])->by_default(false)->as_bool(); + limit_enabled |= info->limit_enable; } - } - // endstop trim used by deltas to do soft adjusting - // on a delta homing to max, a negative trim value will move the carriage down, and a positive will move it up - this->trim_mm[0] = THEKERNEL->config->value(alpha_trim_checksum )->by_default(0 )->as_number(); - this->trim_mm[1] = THEKERNEL->config->value(beta_trim_checksum )->by_default(0 )->as_number(); - this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum )->by_default(0 )->as_number(); + homing_axis.push_back(hinfo); + } - // limits enabled - this->limit_enable[X_AXIS] = THEKERNEL->config->value(alpha_limit_enable_checksum)->by_default(false)->as_bool(); - this->limit_enable[Y_AXIS] = THEKERNEL->config->value(beta_limit_enable_checksum)->by_default(false)->as_bool(); - this->limit_enable[Z_AXIS] = THEKERNEL->config->value(gamma_limit_enable_checksum)->by_default(false)->as_bool(); + // if no pins defined then disable the module + if(endstops.empty()) return false; - // set to true by default for deltas due to trim, false on cartesians - this->move_to_origin_after_home = THEKERNEL->config->value(move_to_origin_checksum)->by_default(is_delta)->as_bool(); + get_global_configs(); - if(this->limit_enable[X_AXIS] || this->limit_enable[Y_AXIS] || this->limit_enable[Z_AXIS]) { + if(limit_enabled) { register_for_event(ON_IDLE); - if(this->is_delta || this->is_rdelta) { - // we must enable all the limits not just one - this->limit_enable[X_AXIS] = true; - this->limit_enable[Y_AXIS] = true; - this->limit_enable[Z_AXIS] = true; - } } - // + // sanity check for deltas + /* if(this->is_delta || this->is_rdelta) { // some things must be the same or they will die, so force it here to avoid config errors this->fast_rates[1] = this->fast_rates[2] = this->fast_rates[0]; @@ -263,12 +219,201 @@ void Endstops::load_config() // NOTE homing_position for rdelta is the angle of the actuator not the cartesian position if(!this->is_rdelta) this->homing_position[0] = this->homing_position[1] = 0; } + */ + + return true; } -bool Endstops::debounced_get(int pin) +// Get config using new syntax supports ABC +bool Endstops::load_config() { + bool limit_enabled= false; + size_t max_index= 0; + + std::array temp_axis_array; // needs to be at least XYZ, but allow for ABC + { + homing_info_t t; + t.axis= 0; + t.axis_index= 0; + t.pin_info= nullptr; + + temp_axis_array.fill(t); + } + + // iterate over all endstop.*.* + std::vector modules; + THEKERNEL->config->get_module_list(&modules, endstop_checksum); + for(auto cs : modules ) { + if(!THEKERNEL->config->value(endstop_checksum, cs, enable_checksum )->as_bool()) continue; + + endstop_info_t *pin_info= new endstop_info_t; + pin_info->pin.from_string(THEKERNEL->config->value(endstop_checksum, cs, pin_checksum)->by_default("nc" )->as_string())->as_input(); + if(!pin_info->pin.connected()){ + // no pin defined try next + delete pin_info; + continue; + } + + string axis= THEKERNEL->config->value(endstop_checksum, cs, axis_checksum)->by_default("")->as_string(); + if(axis.empty()){ + // axis is required + delete pin_info; + continue; + } + + size_t i; + switch(toupper(axis[0])) { + case 'X': i= X_AXIS; break; + case 'Y': i= Y_AXIS; break; + case 'Z': i= Z_AXIS; break; + case 'A': i= A_AXIS; break; + case 'B': i= B_AXIS; break; + case 'C': i= C_AXIS; break; + default: // not a recognized axis + delete pin_info; + continue; + } + + // keep track of the maximum index that has been defined + if(i > max_index) max_index= i; + + // init pin struct + pin_info->debounce= 0; + pin_info->axis= toupper(axis[0]); + pin_info->axis_index= i; + + // are limits enabled + pin_info->limit_enable= THEKERNEL->config->value(endstop_checksum, cs, limit_checksum)->by_default(false)->as_bool(); + limit_enabled |= pin_info->limit_enable; + + // enter into endstop array + endstops.push_back(pin_info); + + // check we are not going above the number of defined actuators/axis + if(i >= k_max_actuators) { + // too many axis we only have configured k_max_actuators + continue; + } + + // if set to none it means not used for homing (maybe limit only) so do not add to the homing array + string direction= THEKERNEL->config->value(endstop_checksum, cs, direction_checksum)->by_default("none")->as_string(); + if(direction == "none") { + continue; + } + + // setup the homing array + homing_info_t hinfo; + + // init homing struct + hinfo.home_offset= 0; + hinfo.homed= false; + hinfo.axis= toupper(axis[0]); + hinfo.axis_index= i; + hinfo.pin_info= pin_info; + + // rates in mm/sec + hinfo.fast_rate= THEKERNEL->config->value(endstop_checksum, cs, fast_rate_checksum)->by_default(100)->as_number(); + hinfo.slow_rate= THEKERNEL->config->value(endstop_checksum, cs, slow_rate_checksum)->by_default(10)->as_number(); + + // retract in mm + hinfo.retract= THEKERNEL->config->value(endstop_checksum, cs, retract_checksum)->by_default(5)->as_number(); + + // homing direction and convert to boolean where true is home to min, and false is home to max + hinfo.home_direction= direction == "home_to_min"; + + // homing cartesian position + hinfo.homing_position= THEKERNEL->config->value(endstop_checksum, cs, position_checksum)->by_default(hinfo.home_direction ? 0 : 200)->as_number(); + + // used to set maximum movement on homing, set by max_travel if defined + hinfo.max_travel= THEKERNEL->config->value(endstop_checksum, cs, max_travel_checksum)->by_default(500)->as_number(); + + // stick into array in correct place + temp_axis_array[hinfo.axis_index]= hinfo; + } + + // if no pins defined then disable the module + if(endstops.empty()) return false; + + // copy to the homing_axis array, make sure that undefined entries are filled in as well + // as the order is important and all slots must be filled upto the max_index + for (size_t i = 0; i < temp_axis_array.size(); ++i) { + if(temp_axis_array[i].axis == 0) { + // was not configured above, if it is XYZ then we need to force a dummy entry + if(i <= Z_AXIS) { + homing_info_t t; + t.axis= 'X' + i; + t.axis_index= i; + t.pin_info= nullptr; // this tells it that it cannot be used for homing + homing_axis.push_back(t); + + }else if(i <= max_index) { + // for instance case where we defined C without A or B + homing_info_t t; + t.axis= 'A' + i; + t.axis_index= i; + t.pin_info= nullptr; // this tells it that it cannot be used for homing + homing_axis.push_back(t); + } + + }else{ + homing_axis.push_back(temp_axis_array[i]); + } + } + + // sets some endstop global configs applicable to all endstops + get_global_configs(); + + if(limit_enabled) { + register_for_event(ON_IDLE); + } + + return true; +} + +void Endstops::get_global_configs() +{ + // NOTE the debounce count is in milliseconds so probably does not need to beset anymore + this->debounce_ms= THEKERNEL->config->value(endstop_debounce_ms_checksum)->by_default(0)->as_number(); + this->debounce_count= THEKERNEL->config->value(endstop_debounce_count_checksum)->by_default(100)->as_number(); + + this->is_corexy= THEKERNEL->config->value(corexy_homing_checksum)->by_default(false)->as_bool(); + this->is_delta= THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool(); + this->is_rdelta= THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool(); + this->is_scara= THEKERNEL->config->value(scara_homing_checksum)->by_default(false)->as_bool(); + + this->home_z_first= THEKERNEL->config->value(home_z_first_checksum)->by_default(false)->as_bool(); + + this->trim_mm[0] = THEKERNEL->config->value(alpha_trim_checksum)->by_default(0)->as_number(); + this->trim_mm[1] = THEKERNEL->config->value(beta_trim_checksum)->by_default(0)->as_number(); + this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum)->by_default(0)->as_number(); + + // see if an order has been specified, must be three or more characters, XYZABC or ABYXZ etc + string order = THEKERNEL->config->value(homing_order_checksum)->by_default("")->as_string(); + this->homing_order = 0; + if(order.size() >= 3 && order.size() <= homing_axis.size() && !(this->is_delta || this->is_rdelta)) { + int shift = 0; + for(auto c : order) { + char n= toupper(c); + uint32_t i = n >= 'X' ? n - 'X' : n - 'A' + 3; + i += 1; // So X is 1 + if(i > 6) { // bad value + this->homing_order = 0; + break; + } + homing_order |= (i << shift); + shift += 3; + } + } + + // set to true by default for deltas due to trim, false on cartesians + this->move_to_origin_after_home = THEKERNEL->config->value(move_to_origin_checksum)->by_default(is_delta)->as_bool(); +} + +bool Endstops::debounced_get(Pin *pin) +{ + if(pin == nullptr) return false; uint8_t debounce = 0; - while(this->pins[pin].get()) { + while(pin->get()) { if ( ++debounce >= this->debounce_count ) { // pin triggered return true; @@ -277,30 +422,25 @@ bool Endstops::debounced_get(int pin) return false; } -static const char *endstop_names[] = {"min_x", "min_y", "min_z", "max_x", "max_y", "max_z"}; - +// only called if limits are enabled void Endstops::on_idle(void *argument) { if(this->status == LIMIT_TRIGGERED) { // if we were in limit triggered see if it has been cleared - for( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if(this->limit_enable[c]) { - std::array minmax{{0, 3}}; - // check min and max endstops - for (int i : minmax) { - int n = c + i; - if(this->pins[n].get()) { - // still triggered, so exit - bounce_cnt = 0; - return; - } + for(auto& i : endstops) { + if(i->limit_enable) { + if(i->pin.get()) { + // still triggered, so exit + i->debounce = 0; + return; + } + + if(i->debounce++ > debounce_count) { // can use less as it calls on_idle in between + // clear the state + this->status = NOT_HOMING; } } } - if(++bounce_cnt > 10) { // can use less as it calls on_idle in between - // clear the state - this->status = NOT_HOMING; - } return; } else if(this->status != NOT_HOMING) { @@ -308,20 +448,21 @@ void Endstops::on_idle(void *argument) return; } - for( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if(this->limit_enable[c] && STEPPER[c]->is_moving()) { - std::array minmax{{0, 3}}; + for(auto& i : endstops) { + if(i->limit_enable && STEPPER[i->axis_index]->is_moving()) { // check min and max endstops - for (int i : minmax) { - int n = c + i; - if(debounced_get(n)) { - // endstop triggered - THEKERNEL->streams->printf("Limit switch %s was hit - reset or M999 required\n", endstop_names[n]); - this->status = LIMIT_TRIGGERED; - // disables heaters and motors, ignores incoming Gcode and flushes block queue - THEKERNEL->call_event(ON_HALT, nullptr); - return; + if(debounced_get(&i->pin)) { + // endstop triggered + if(!THEKERNEL->is_grbl_mode()) { + THEKERNEL->streams->printf("Limit switch %c%c was hit - reset or M999 required\n", STEPPER[i->axis_index]->which_direction() ? '-' : '+', i->axis); + }else{ + THEKERNEL->streams->printf("ALARM: Hard limit %c%c\n", STEPPER[i->axis_index]->which_direction() ? '-' : '+', i->axis); } + this->status = LIMIT_TRIGGERED; + i->debounce= 0; + // disables heaters and motors, ignores incoming Gcode and flushes block queue + THEKERNEL->call_event(ON_HALT, nullptr); + return; } } } @@ -329,24 +470,30 @@ void Endstops::on_idle(void *argument) // if limit switches are enabled, then we must move off of the endstop otherwise we won't be able to move // checks if triggered and only backs off if triggered -void Endstops::back_off_home(std::bitset<3> axis) +void Endstops::back_off_home(axis_bitmap_t axis) { std::vector> params; this->status = BACK_OFF_HOME; + float slow_rate= NAN; // default mm/sec + // these are handled differently if(is_delta) { // Move off of the endstop using a regular relative move in Z only - params.push_back({'Z', this->retract_mm[Z_AXIS] * (this->home_direction[Z_AXIS] ? 1 : -1)}); + params.push_back({'Z', THEROBOT->from_millimeters(homing_axis[Z_AXIS].retract * (homing_axis[Z_AXIS].home_direction ? 1 : -1))}); + slow_rate= homing_axis[Z_AXIS].slow_rate; } else { - // cartesians, concatenate all the moves we need to do into one gcode - for( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if(!axis[c]) continue; // only for axes we asked to move + // cartesians concatenate all the moves we need to do into one gcode + for( auto& e : homing_axis) { + if(!axis[e.axis_index]) continue; // only for axes we asked to move // if not triggered no need to move off - if(this->limit_enable[c] && debounced_get(c + (this->home_direction[c] ? 0 : 3)) ) { - params.push_back({c + 'X', this->retract_mm[c] * (this->home_direction[c] ? 1 : -1)}); + if(e.pin_info != nullptr && e.pin_info->limit_enable && debounced_get(&e.pin_info->pin)) { + char ax= e.axis; + params.push_back({ax, THEROBOT->from_millimeters(e.retract * (e.home_direction ? 1 : -1))}); + // select slowest of them all + slow_rate= isnan(slow_rate) ? e.slow_rate : std::min(slow_rate, e.slow_rate); } } } @@ -355,12 +502,11 @@ void Endstops::back_off_home(std::bitset<3> axis) // Move off of the endstop using a regular relative move params.insert(params.begin(), {'G', 0}); // use X slow rate to move, Z should have a max speed set anyway - params.push_back({'F', this->slow_rates[X_AXIS] * 60.0F}); + params.push_back({'F', THEROBOT->from_millimeters(slow_rate * 60.0F)}); char gcode_buf[64]; append_parameters(gcode_buf, params, sizeof(gcode_buf)); Gcode gc(gcode_buf, &(StreamOutput::NullStream)); THEROBOT->push_state(); - THEROBOT->inch_mode = false; // needs to be in mm THEROBOT->absolute_mode = false; // needs to be relative mode THEROBOT->on_gcode_received(&gc); // send to robot directly // Wait for above to finish @@ -372,7 +518,7 @@ void Endstops::back_off_home(std::bitset<3> axis) } // If enabled will move the head to 0,0 after homing, but only if X and Y were set to home -void Endstops::move_to_origin(std::bitset<3> axis) +void Endstops::move_to_origin(axis_bitmap_t axis) { if(!is_delta && (!axis[X_AXIS] || !axis[Y_AXIS])) return; // ignore if X and Y not homing, unless delta @@ -380,13 +526,12 @@ void Endstops::move_to_origin(std::bitset<3> axis) // float pos[3]; THEROBOT->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return; this->status = MOVE_TO_ORIGIN; - // Move to center using a regular move, use slower of X and Y fast rate - float rate = std::min(this->fast_rates[0], this->fast_rates[1]) * 60.0F; + // Move to center using a regular move, use slower of X and Y fast rate in mm/sec + float rate = std::min(homing_axis[X_AXIS].fast_rate, homing_axis[Y_AXIS].fast_rate) * 60.0F; char buf[32]; THEROBOT->push_state(); - THEROBOT->inch_mode = false; // needs to be in mm THEROBOT->absolute_mode = true; - snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", rate); // must use machine coordinates in case G92 or WCS is in effect + snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", THEROBOT->from_millimeters(rate)); // must use machine coordinates in case G92 or WCS is in effect struct SerialMessage message; message.message = buf; message.stream = &(StreamOutput::NullStream); @@ -402,45 +547,36 @@ uint32_t Endstops::read_endstops(uint32_t dummy) { if(this->status != MOVING_TO_ENDSTOP_SLOW && this->status != MOVING_TO_ENDSTOP_FAST) return 0; // not doing anything we need to monitor for - if(!is_corexy) { - // check each axis - for ( int m = X_AXIS; m <= Z_AXIS; m++ ) { - if(STEPPER[m]->is_moving()) { - // if it is moving then we check the associated endstop, and debounce it - if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) { - if(debounce[m] < debounce_ms) { - debounce[m]++; - } else { - // we signal the motor to stop, which will preempt any moves on that axis - STEPPER[m]->stop_moving(); - } + // check each homing endstop + for(auto& e : homing_axis) { // check all axis homing endstops + if(e.pin_info == nullptr) continue; // ignore if not a homing endstop + int m= e.axis_index; - } else { - // The endstop was not hit yet - debounce[m] = 0; - } - } - } + // for corexy homing in X or Y we must only check the associated endstop, works as we only home one axis at a time for corexy + if(is_corexy && (m == X_AXIS || m == Y_AXIS) && !axis_to_home[m]) continue; - } else { - // corexy is different as the actuators are not directly related to the XY axis - // so we check the axis that is currently homing then stop all motors - for ( int m = X_AXIS; m <= Z_AXIS; m++ ) { - if(axis_to_home[m]) { - if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) { - if(debounce[m] < debounce_ms) { - debounce[m]++; - } else { - // we signal all the motors to stop, as on corexy X and Y motors will move for X and Y axis homing and we only hom eone axis at a time + if(STEPPER[m]->is_moving()) { + // if it is moving then we check the associated endstop, and debounce it + if(e.pin_info->pin.get()) { + if(e.pin_info->debounce < debounce_ms) { + e.pin_info->debounce++; + + } else { + if(is_corexy && (m == X_AXIS || m == Y_AXIS)) { + // corexy when moving in X or Y we need to stop both the X and Y motors STEPPER[X_AXIS]->stop_moving(); STEPPER[Y_AXIS]->stop_moving(); - STEPPER[Z_AXIS]->stop_moving(); - } - } else { - // The endstop was not hit yet - debounce[m] = 0; + }else{ + // we signal the motor to stop, which will preempt any moves on that axis + STEPPER[m]->stop_moving(); + } + e.pin_info->triggered= true; } + + } else { + // The endstop was not hit yet + e.pin_info->debounce= 0; } } } @@ -452,37 +588,40 @@ void Endstops::home_xy() { if(axis_to_home[X_AXIS] && axis_to_home[Y_AXIS]) { // Home XY first so as not to slow them down by homing Z at the same time - float delta[3] {alpha_max, beta_max, 0}; - if(this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS]; - if(this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS]; - float feed_rate = std::min(fast_rates[X_AXIS], fast_rates[Y_AXIS]); + float delta[3] {homing_axis[X_AXIS].max_travel, homing_axis[Y_AXIS].max_travel, 0}; + if(homing_axis[X_AXIS].home_direction) delta[X_AXIS]= -delta[X_AXIS]; + if(homing_axis[Y_AXIS].home_direction) delta[Y_AXIS]= -delta[Y_AXIS]; + float feed_rate = std::min(homing_axis[X_AXIS].fast_rate, homing_axis[Y_AXIS].fast_rate); THEROBOT->delta_move(delta, feed_rate, 3); } else if(axis_to_home[X_AXIS]) { // now home X only - float delta[3] {alpha_max, 0, 0}; - if(this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS]; - THEROBOT->delta_move(delta, fast_rates[X_AXIS], 3); + float delta[3] {homing_axis[X_AXIS].max_travel, 0, 0}; + if(homing_axis[X_AXIS].home_direction) delta[X_AXIS]= -delta[X_AXIS]; + THEROBOT->delta_move(delta, homing_axis[X_AXIS].fast_rate, 3); } else if(axis_to_home[Y_AXIS]) { // now home Y only - float delta[3] {0, beta_max, 0}; - if(this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS]; - THEROBOT->delta_move(delta, fast_rates[Y_AXIS], 3); + float delta[3] {0, homing_axis[Y_AXIS].max_travel, 0}; + if(homing_axis[Y_AXIS].home_direction) delta[Y_AXIS]= -delta[Y_AXIS]; + THEROBOT->delta_move(delta, homing_axis[Y_AXIS].fast_rate, 3); } // Wait for axis to have homed THECONVEYOR->wait_for_idle(); } -void Endstops::home(std::bitset<3> a) +void Endstops::home(axis_bitmap_t a) { - // reset debounce counts - debounce.fill(0); + // reset debounce counts for all endstops + for(auto& e : endstops) { + e->debounce= 0; + e->triggered= false; + } - // turn off any compensation transform - auto savect= THEROBOT->compensationTransform; - THEROBOT->compensationTransform= nullptr; + if (is_scara) { + THEROBOT->disable_arm_solution = true; // Polar bots has to home in the actuator space. Arm solution disabled. + } this->axis_to_home= a; @@ -495,157 +634,176 @@ void Endstops::home(std::bitset<3> a) if(axis_to_home[Z_AXIS]) { // now home z - float delta[3] {0, 0, gamma_max}; // we go the max z - if(this->home_direction[Z_AXIS]) delta[Z_AXIS]= -delta[Z_AXIS]; - THEROBOT->delta_move(delta, fast_rates[Z_AXIS], 3); + float delta[3] {0, 0, homing_axis[Z_AXIS].max_travel}; // we go the max z + if(homing_axis[Z_AXIS].home_direction) delta[Z_AXIS]= -delta[Z_AXIS]; + THEROBOT->delta_move(delta, homing_axis[Z_AXIS].fast_rate, 3); // wait for Z THECONVEYOR->wait_for_idle(); } if(home_z_first) home_xy(); - // TODO should check that the endstops were hit and it did not stop short for some reason - // we did not complete movement the full distance if we hit the endstops - THEROBOT->reset_position_from_current_actuator_position(); + // potentially home A B and C individually + if(homing_axis.size() > 3){ + for (size_t i = A_AXIS; i < homing_axis.size(); ++i) { + if(axis_to_home[i]) { + // now home A B or C + float delta[i+1]; + for (size_t j = 0; j <= i; ++j) delta[j]= 0; + delta[i]= homing_axis[i].max_travel; // we go the max + if(homing_axis[i].home_direction) delta[i]= -delta[i]; + THEROBOT->delta_move(delta, homing_axis[i].fast_rate, i+1); + // wait for it + THECONVEYOR->wait_for_idle(); + } + } + } + + // check that the endstops were hit and it did not stop short for some reason + // if the endstop is not triggered then enter ALARM state + // with deltas we check all three axis were triggered, but at least one of XYZ must be set to home + if(axis_to_home[X_AXIS] || axis_to_home[Y_AXIS] || axis_to_home[Z_AXIS]) { + for (size_t i = X_AXIS; i <= Z_AXIS; ++i) { + if((axis_to_home[i] || this->is_delta || this->is_rdelta) && !homing_axis[i].pin_info->triggered) { + this->status = NOT_HOMING; + THEKERNEL->call_event(ON_HALT, nullptr); + return; + } + } + } + + // also check ABC + if(homing_axis.size() > 3){ + for (size_t i = A_AXIS; i < homing_axis.size(); ++i) { + if(axis_to_home[i] && !homing_axis[i].pin_info->triggered) { + this->status = NOT_HOMING; + THEKERNEL->call_event(ON_HALT, nullptr); + return; + } + } + } + + if (!is_scara) { + // Only for non polar bots + // we did not complete movement the full distance if we hit the endstops + // TODO Maybe only reset axis involved in the homing cycle + THEROBOT->reset_position_from_current_actuator_position(); + } // Move back a small distance for all homing axis this->status = MOVING_BACK; - float delta[3]{0,0,0}; - // use minimum feed rate of all three axes that are being homed (sub optimal, but necessary) - float feed_rate= slow_rates[X_AXIS]; - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { + float delta[homing_axis.size()]; + for (size_t i = 0; i < homing_axis.size(); ++i) delta[i]= 0; + + // use minimum feed rate of all axes that are being homed (sub optimal, but necessary) + float feed_rate= homing_axis[X_AXIS].slow_rate; + for (auto& i : homing_axis) { + int c= i.axis_index; if(axis_to_home[c]) { - delta[c]= this->retract_mm[c]; - if(!this->home_direction[c]) delta[c]= -delta[c]; - feed_rate= std::min(slow_rates[c], feed_rate); + delta[c]= i.retract; + if(!i.home_direction) delta[c]= -delta[c]; + feed_rate= std::min(i.slow_rate, feed_rate); } } - THEROBOT->delta_move(delta, feed_rate, 3); + THEROBOT->delta_move(delta, feed_rate, homing_axis.size()); // wait until finished THECONVEYOR->wait_for_idle(); // Start moving the axes towards the endstops slowly this->status = MOVING_TO_ENDSTOP_SLOW; - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { + for (auto& i : homing_axis) { + int c= i.axis_index; if(axis_to_home[c]) { - delta[c]= this->retract_mm[c]*2; // move further than we moved off to make sure we hit it cleanly - if(this->home_direction[c]) delta[c]= -delta[c]; + delta[c]= i.retract*2; // move further than we moved off to make sure we hit it cleanly + if(i.home_direction) delta[c]= -delta[c]; }else{ delta[c]= 0; } } - THEROBOT->delta_move(delta, feed_rate, 3); + THEROBOT->delta_move(delta, feed_rate, homing_axis.size()); // wait until finished THECONVEYOR->wait_for_idle(); - // TODO should check that the endstops were hit and it did not stop short for some reason // we did not complete movement the full distance if we hit the endstops + // TODO Maybe only reset axis involved in the homing cycle THEROBOT->reset_position_from_current_actuator_position(); THEROBOT->disable_segmentation= false; - - // restore compensationTransform - THEROBOT->compensationTransform= savect; + if (is_scara) { + THEROBOT->disable_arm_solution = false; // Arm solution enabled again. + } this->status = NOT_HOMING; } void Endstops::process_home_command(Gcode* gcode) { - if( (gcode->subcode == 0 && THEKERNEL->is_grbl_mode()) || (gcode->subcode == 2 && !THEKERNEL->is_grbl_mode()) ) { - // G28 in grbl mode or G28.2 in normal mode will do a rapid to the predefined position - // TODO spec says if XYZ specified move to them first then move to MCS of specifed axis - THEROBOT->push_state(); - THEROBOT->inch_mode = false; // needs to be in mm - THEROBOT->absolute_mode = true; - char buf[32]; - snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", saved_position[X_AXIS], saved_position[Y_AXIS]); // must use machine coordinates in case G92 or WCS is in effect - struct SerialMessage message; - message.message = buf; - message.stream = &(StreamOutput::NullStream); - THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command - // Wait for above to finish - THECONVEYOR->wait_for_idle(); - THEROBOT->pop_state(); - return; - - } else if(THEKERNEL->is_grbl_mode() && gcode->subcode == 2) { // G28.2 in grbl mode forces homing (triggered by $H) - // fall through so it does homing cycle - - } else if(gcode->subcode == 1) { // G28.1 set pre defined position - // saves current position in absolute machine coordinates - THEROBOT->get_axis_position(saved_position); // Only XY are used - // Note the following is only meant to be used for recovering a saved position from config-override - // Not a standard Gcode and not to be relied on - if (gcode->has_letter('X')) saved_position[X_AXIS] = gcode->get_value('X'); - if (gcode->has_letter('Y')) saved_position[Y_AXIS] = gcode->get_value('Y'); - return; - - } else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing - if(gcode->get_num_args() == 0) { - THEROBOT->reset_axis_position(0, 0, 0); - } else { - // do a manual homing based on given coordinates, no endstops required - if(gcode->has_letter('X')) THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS); - if(gcode->has_letter('Y')) THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS); - if(gcode->has_letter('Z')) THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); - } - return; - - } else if(gcode->subcode == 4) { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta) - // do a manual homing based on given coordinates, no endstops required - ActuatorCoordinates ac; - if(gcode->has_letter('X')) ac[0] = gcode->get_value('X'); - if(gcode->has_letter('Y')) ac[1] = gcode->get_value('Y'); - if(gcode->has_letter('Z')) ac[2] = gcode->get_value('Z'); - THEROBOT->reset_actuator_position(ac); - return; - - } else if(THEKERNEL->is_grbl_mode()) { - gcode->stream->printf("error:Unsupported command\n"); - return; - } - - // G28 is received, we have homing to do - // First wait for the queue to be empty THECONVEYOR->wait_for_idle(); + // turn off any compensation transform so Z does not move as XY home + auto savect= THEROBOT->compensationTransform; + THEROBOT->compensationTransform= nullptr; + // deltas always home Z axis only, which moves all three actuators - bool home_in_z = this->is_delta || this->is_rdelta; + bool home_in_z_only = this->is_delta || this->is_rdelta; // figure out which axis to home - bitset<3> haxis; + axis_bitmap_t haxis; haxis.reset(); - if(!home_in_z) { // ie not a delta - bool axis_speced = ( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ); - // only enable homing if the endstop is defined, - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if (this->pins[c + (this->home_direction[c] ? 0 : 3)].connected() && (!axis_speced || gcode->has_letter(c + 'X')) ) { - haxis.set(c); + bool axis_speced = (gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') || + gcode->has_letter('A') || gcode->has_letter('B') || gcode->has_letter('C')); + + if(!home_in_z_only) { // ie not a delta + for (auto &p : homing_axis) { + // only enable homing if the endstop is defined, + if(p.pin_info == nullptr) continue; + if(!axis_speced || gcode->has_letter(p.axis)) { + haxis.set(p.axis_index); // now reset axis to 0 as we do not know what state we are in - THEROBOT->reset_axis_position(0, c); + if (!is_scara) { + THEROBOT->reset_axis_position(0, p.axis_index); + } else { + // SCARA resets arms to plausable minimum angles + THEROBOT->reset_axis_position(-30,30,0); // angles set into axis space for homing. + } } } } else { - // Only Z axis homes (even though all actuators move this is handled by arm solution) - haxis.set(Z_AXIS); - // we also set the kinematics to a known good position, this is necessary for a rotary delta, but doesn't hurt for linear delta - THEROBOT->reset_axis_position(0, 0, 0); + bool home_z= !axis_speced || gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z'); + + // if we specified an axis we check ABC + for (size_t i = A_AXIS; i < homing_axis.size(); ++i) { + auto &p= homing_axis[i]; + if(p.pin_info == nullptr) continue; + if(!axis_speced || gcode->has_letter(p.axis)) haxis.set(p.axis_index); + } + + if(home_z){ + // Only Z axis homes (even though all actuators move this is handled by arm solution) + haxis.set(Z_AXIS); + // we also set the kinematics to a known good position, this is necessary for a rotary delta, but doesn't hurt for linear delta + THEROBOT->reset_axis_position(0, 0, 0); + } + } + + if(haxis.none()) { + THEKERNEL->streams->printf("WARNING: Nothing to home\n"); + return; } // do the actual homing - if(homing_order != 0) { + if(homing_order != 0 && !is_scara) { // if an order has been specified do it in the specified order - // homing order is 0b00ccbbaa where aa is 0,1,2 to specify the first axis, bb is the second and cc is the third - // eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z - for (uint8_t m = homing_order; m != 0; m >>= 2) { - int a= (m & 0x03); // axis to home - if(haxis[a]) { // if axis is selected to home - std::bitset<3> bs; + // homing order is 0bfffeeedddcccbbbaaa where aaa is 1,2,3,4,5,6 to specify the first axis (XYZABC), bbb is the second and ccc is the third etc + // eg 0b0101011001010 would be Y X Z A, 011 010 001 100 101 would be B A X Y Z + for (uint32_t m = homing_order; m != 0; m >>= 3) { + uint32_t a= (m & 0x07)-1; // axis to home + if(a < homing_axis.size() && haxis[a]) { // if axis is selected to home + axis_bitmap_t bs; bs.set(a); home(bs); } @@ -655,12 +813,14 @@ void Endstops::process_home_command(Gcode* gcode) } else if(is_corexy) { // corexy must home each axis individually - for (int a = X_AXIS; a <= Z_AXIS; ++a) { - if(haxis[a]) { - std::bitset<3> bs; - bs.set(a); + for (auto &p : homing_axis) { + if(haxis[p.axis_index]) { + axis_bitmap_t bs; + bs.set(p.axis_index); home(bs); } + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()) break; } } else { @@ -668,26 +828,33 @@ void Endstops::process_home_command(Gcode* gcode) home(haxis); } - // check if on_halt (eg kill) + // restore compensationTransform + THEROBOT->compensationTransform= savect; + + // check if on_halt (eg kill or fail) if(THEKERNEL->is_halted()) { if(!THEKERNEL->is_grbl_mode()) { - THEKERNEL->streams->printf("Homing cycle aborted by kill\n"); + THEKERNEL->streams->printf("ERROR: Homing cycle failed - check the max_travel settings\n"); + }else{ + THEKERNEL->streams->printf("ALARM: Homing fail\n"); } + // clear all the homed flags + for (auto &p : homing_axis) p.homed= false; return; } - if(home_in_z) { // deltas only + if(home_in_z_only || is_scara) { // deltas and scaras only // Here's where we would have been if the endstops were perfectly trimmed // NOTE on a rotary delta home_offset is actuator position in degrees when homed and // home_offset is the theta offset for each actuator, so M206 is used to set theta offset for each actuator in degrees // FIXME not sure this will work with compensation transforms on. float ideal_position[3] = { - this->homing_position[X_AXIS] + this->home_offset[X_AXIS], - this->homing_position[Y_AXIS] + this->home_offset[Y_AXIS], - this->homing_position[Z_AXIS] + this->home_offset[Z_AXIS] + homing_axis[X_AXIS].homing_position + homing_axis[X_AXIS].home_offset, + homing_axis[Y_AXIS].homing_position + homing_axis[Y_AXIS].home_offset, + homing_axis[Z_AXIS].homing_position + homing_axis[Z_AXIS].home_offset }; - bool has_endstop_trim = this->is_delta; + bool has_endstop_trim = this->is_delta || is_scara; if (has_endstop_trim) { ActuatorCoordinates ideal_actuator_position; THEROBOT->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position); @@ -701,7 +868,7 @@ void Endstops::process_home_command(Gcode* gcode) float real_position[3]; THEROBOT->arm_solution->actuator_to_cartesian(real_actuator_position, real_position); - // Reset the actuator positions to correspond our real position + // Reset the actuator positions to correspond to our real position THEROBOT->reset_axis_position(real_position[0], real_position[1], real_position[2]); } else { @@ -712,16 +879,35 @@ void Endstops::process_home_command(Gcode* gcode) THEROBOT->reset_actuator_position(real_actuator_position); } else { - // Reset the actuator positions to correspond our real position + // Reset the actuator positions to correspond to our real position THEROBOT->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]); } } + // for deltas we say all 3 axis are homed even though it was only Z + homing_axis[X_AXIS].homed= true; + homing_axis[Y_AXIS].homed= true; + homing_axis[Z_AXIS].homed= true; + + // if we also homed ABC then we need to reset them + for (size_t i = A_AXIS; i < homing_axis.size(); ++i) { + auto &p= homing_axis[i]; + if (haxis[p.axis_index]) { // if we requested this axis to home + THEROBOT->reset_axis_position(p.homing_position + p.home_offset, p.axis_index); + // set flag indicating axis was homed, it stays set once set until H/W reset or unhomed + p.homed= true; + } + } + } else { // Zero the ax(i/e)s position, add in the home offset - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if (haxis[c]) { // if we requested this axis to home - THEROBOT->reset_axis_position(this->homing_position[c] + this->home_offset[c], c); + // NOTE that if compensation is active the Z will be set based on where XY are, so make sure XY are homed first then Z + // so XY are at a known consistent position. (especially true if using a proximity probe) + for (auto &p : homing_axis) { + if (haxis[p.axis_index]) { // if we requested this axis to home + THEROBOT->reset_axis_position(p.homing_position + p.home_offset, p.axis_index); + // set flag indicating axis was homed, it stays set once set until H/W reset or unhomed + p.homed= true; } } } @@ -734,7 +920,7 @@ void Endstops::process_home_command(Gcode* gcode) // if limit switches are enabled we must back off endstop after setting home back_off_home(haxis); - } else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) { + } else if(haxis[Z_AXIS] && (this->move_to_origin_after_home || homing_axis[X_AXIS].pin_info->limit_enable)) { // deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first // also need to back off endstops if limits are enabled back_off_home(haxis); @@ -744,52 +930,172 @@ void Endstops::process_home_command(Gcode* gcode) void Endstops::set_homing_offset(Gcode *gcode) { - // Similar to M206 and G92 but sets Homing offsets based on current position - float cartesian[3]; - THEROBOT->get_axis_position(cartesian); // get actual position from robot + // M306 Similar to M206 but sets Homing offsets based on current MCS position + // Basically it finds the delta between the current MCS position and the requested position and adds it to the homing offset + // then will not let it be set again until that axis is homed. + float pos[3]; + THEROBOT->get_axis_position(pos); + if (gcode->has_letter('X')) { - home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X')); - THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS); + if(!homing_axis[X_AXIS].homed) { + gcode->stream->printf("error: Axis X must be homed before setting Homing offset\n"); + return; + } + homing_axis[X_AXIS].home_offset += (THEROBOT->to_millimeters(gcode->get_value('X')) - pos[X_AXIS]); + homing_axis[X_AXIS].homed= false; // force it to be homed } if (gcode->has_letter('Y')) { - home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y')); - THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS); + if(!homing_axis[Y_AXIS].homed) { + gcode->stream->printf("error: Axis Y must be homed before setting Homing offset\n"); + return; + } + homing_axis[Y_AXIS].home_offset += (THEROBOT->to_millimeters(gcode->get_value('Y')) - pos[Y_AXIS]); + homing_axis[Y_AXIS].homed= false; // force it to be homed } if (gcode->has_letter('Z')) { - home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z')); - THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); + if(!homing_axis[Z_AXIS].homed) { + gcode->stream->printf("error: Axis Z must be homed before setting Homing offset\n"); + return; + } + homing_axis[Z_AXIS].home_offset += (THEROBOT->to_millimeters(gcode->get_value('Z')) - pos[Z_AXIS]); + homing_axis[Z_AXIS].homed= false; // force it to be homed } - gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); + gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f will take effect next home\n", homing_axis[X_AXIS].home_offset, homing_axis[Y_AXIS].home_offset, homing_axis[Z_AXIS].home_offset); } -// Start homing sequences by response to GCode commands +void Endstops::handle_park(Gcode * gcode) +{ + // TODO: spec says if XYZ specified move to them first then move to MCS of specifed axis + THEROBOT->push_state(); + THEROBOT->absolute_mode = true; + char buf[32]; + snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", THEROBOT->from_millimeters(saved_position[X_AXIS]), THEROBOT->from_millimeters(saved_position[Y_AXIS])); // must use machine coordinates in case G92 or WCS is in effect + struct SerialMessage message; + message.message = buf; + message.stream = &(StreamOutput::NullStream); + THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command + // Wait for above to finish + THECONVEYOR->wait_for_idle(); + THEROBOT->pop_state(); +} + +// parse gcodes void Endstops::on_gcode_received(void *argument) { Gcode *gcode = static_cast(argument); + if ( gcode->has_g && gcode->g == 28) { - process_home_command(gcode); + switch(gcode->subcode) { + case 0: // G28 in grbl mode will do a rapid to the predefined position otherwise it is home command + if(THEKERNEL->is_grbl_mode()){ + handle_park(gcode); + }else{ + process_home_command(gcode); + } + break; + + case 1: // G28.1 set pre defined park position + // saves current position in absolute machine coordinates + THEROBOT->get_axis_position(saved_position); // Only XY are used + // Note the following is only meant to be used for recovering a saved position from config-override + // Not a standard Gcode and not to be relied on + if (gcode->has_letter('X')) saved_position[X_AXIS] = gcode->get_value('X'); + if (gcode->has_letter('Y')) saved_position[Y_AXIS] = gcode->get_value('Y'); + break; + + case 2: // G28.2 in grbl mode does homing (triggered by $H), otherwise it moves to the park position + if(THEKERNEL->is_grbl_mode()) { + process_home_command(gcode); + }else{ + handle_park(gcode); + } + break; + + case 3: // G28.3 is a smoothie special it sets manual homing + if(gcode->get_num_args() == 0) { + for (auto &p : homing_axis) { + p.homed= true; + THEROBOT->reset_axis_position(0, p.axis_index); + } + } else { + // do a manual homing based on given coordinates, no endstops required + if(gcode->has_letter('X')){ THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS); homing_axis[X_AXIS].homed= true; } + if(gcode->has_letter('Y')){ THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS); homing_axis[Y_AXIS].homed= true; } + if(gcode->has_letter('Z')){ THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); homing_axis[Z_AXIS].homed= true; } + if(homing_axis.size() > A_AXIS && gcode->has_letter('A')){ THEROBOT->reset_axis_position(gcode->get_value('A'), A_AXIS); homing_axis[A_AXIS].homed= true; } + if(homing_axis.size() > B_AXIS && gcode->has_letter('B')){ THEROBOT->reset_axis_position(gcode->get_value('B'), B_AXIS); homing_axis[B_AXIS].homed= true; } + if(homing_axis.size() > C_AXIS && gcode->has_letter('C')){ THEROBOT->reset_axis_position(gcode->get_value('C'), C_AXIS); homing_axis[C_AXIS].homed= true; } + } + break; + + case 4: { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta) + // do a manual homing based on given coordinates, no endstops required + ActuatorCoordinates ac{NAN, NAN, NAN}; + if(gcode->has_letter('X')){ ac[0] = gcode->get_value('X'); homing_axis[X_AXIS].homed= true; } + if(gcode->has_letter('Y')){ ac[1] = gcode->get_value('Y'); homing_axis[Y_AXIS].homed= true; } + if(gcode->has_letter('Z')){ ac[2] = gcode->get_value('Z'); homing_axis[Z_AXIS].homed= true; } + THEROBOT->reset_actuator_position(ac); + } + break; + + case 5: // G28.5 is a smoothie special it clears the homed flag for the specified axis, or all if not specifed + if(gcode->get_num_args() == 0) { + for (auto &p : homing_axis) p.homed= false; + } else { + if(gcode->has_letter('X')) homing_axis[X_AXIS].homed= false; + if(gcode->has_letter('Y')) homing_axis[Y_AXIS].homed= false; + if(gcode->has_letter('Z')) homing_axis[Z_AXIS].homed= false; + if(homing_axis.size() > A_AXIS && gcode->has_letter('A')) homing_axis[A_AXIS].homed= false; + if(homing_axis.size() > B_AXIS && gcode->has_letter('B')) homing_axis[B_AXIS].homed= false; + if(homing_axis.size() > C_AXIS && gcode->has_letter('C')) homing_axis[C_AXIS].homed= false; + } + break; + + case 6: // G28.6 is a smoothie special it shows the homing status of each axis + for (auto &p : homing_axis) { + gcode->stream->printf("%c:%d ", p.axis, p.homed); + } + gcode->add_nl= true; + break; + + default: + if(THEKERNEL->is_grbl_mode()) { + gcode->stream->printf("error:Unsupported command\n"); + } + break; + } } else if (gcode->has_m) { switch (gcode->m) { case 119: { - for (int i = 0; i < 6; ++i) { - if(this->pins[i].connected()) - gcode->stream->printf("%s:%d ", endstop_names[i], this->pins[i].get()); + for(auto& h : homing_axis) { + string name; + name.append(1, h.axis).append(h.home_direction ? "_min" : "_max"); + gcode->stream->printf("%s:%d ", name.c_str(), h.pin_info->pin.get()); + } + gcode->stream->printf("pins- "); + for(auto& p : endstops) { + string str(1, p->axis); + if(p->limit_enable) str.append("L"); + gcode->stream->printf("(%s)P%d.%d:%d ", str.c_str(), p->pin.port_number, p->pin.pin, p->pin.get()); } gcode->add_nl = true; - } break; case 206: // M206 - set homing offset if(is_rdelta) return; // RotaryDeltaCalibration module will handle this + for (auto &p : homing_axis) { + if (gcode->has_letter(p.axis)) p.home_offset= gcode->get_value(p.axis); + } + + for (auto &p : homing_axis) { + gcode->stream->printf("%c: %5.3f ", p.axis, p.home_offset); + } - if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X'); - if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y'); - if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z'); - gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); + gcode->stream->printf(" will take effect next home\n"); break; case 306: // set homing offset based on current position @@ -800,14 +1106,21 @@ void Endstops::on_gcode_received(void *argument) case 500: // save settings case 503: // print settings - if(!is_rdelta) - 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]); - else - gcode->stream->printf(";Theta offset (degrees):\nM206 A%1.5f B%1.5f C%1.5f\n", home_offset[0], home_offset[1], home_offset[2]); + if(!is_rdelta) { + gcode->stream->printf(";Home offset (mm):\nM206 "); + for (auto &p : homing_axis) { + gcode->stream->printf("%c%1.2f ", p.axis, p.home_offset); + } + gcode->stream->printf("\n"); + + }else{ + gcode->stream->printf(";Theta offset (degrees):\nM206 A%1.5f B%1.5f C%1.5f\n", + homing_axis[X_AXIS].home_offset, homing_axis[Y_AXIS].home_offset, homing_axis[Z_AXIS].home_offset); + } if (this->is_delta || this->is_scara) { gcode->stream->printf(";Trim (mm):\nM666 X%1.3f Y%1.3f Z%1.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); - gcode->stream->printf(";Max Z\nM665 Z%1.3f\n", this->homing_position[2]); + gcode->stream->printf(";Max Z\nM665 Z%1.3f\n", homing_axis[Z_AXIS].homing_position); } if(saved_position[X_AXIS] != 0 || saved_position[Y_AXIS] != 0) { gcode->stream->printf(";predefined position:\nG28.1 X%1.4f Y%1.4f\n", saved_position[X_AXIS], saved_position[Y_AXIS]); @@ -816,9 +1129,9 @@ void Endstops::on_gcode_received(void *argument) case 665: if (this->is_delta || this->is_scara) { // M665 - set max gamma/z height - float gamma_max = this->homing_position[2]; + float gamma_max = homing_axis[Z_AXIS].homing_position; if (gcode->has_letter('Z')) { - this->homing_position[2] = gamma_max = gcode->get_value('Z'); + homing_axis[Z_AXIS].homing_position= gamma_max = gcode->get_value('Z'); } gcode->stream->printf("Max Z %8.3f ", gamma_max); gcode->add_nl = true; @@ -852,7 +1165,11 @@ void Endstops::on_get_public_data(void* argument) pdr->set_taken(); } else if(pdr->second_element_is(home_offset_checksum)) { - pdr->set_data_ptr(&this->home_offset); + // provided by caller + float *data = static_cast(pdr->get_data_ptr()); + for (int i = 0; i < 3; ++i) { + data[i]= homing_axis[i].home_offset; + } pdr->set_taken(); } else if(pdr->second_element_is(saved_position_checksum)) { @@ -863,6 +1180,13 @@ void Endstops::on_get_public_data(void* argument) bool *homing = static_cast(pdr->get_data_ptr()); *homing = this->status != NOT_HOMING; pdr->set_taken(); + + } else if(pdr->second_element_is(get_homed_status_checksum)) { + bool *homed = static_cast(pdr->get_data_ptr()); + for (int i = 0; i < 3; ++i) { + homed[i]= homing_axis[i].homed; + } + pdr->set_taken(); } } @@ -881,8 +1205,8 @@ void Endstops::on_set_public_data(void* argument) } else if(pdr->second_element_is(home_offset_checksum)) { float *t = static_cast(pdr->get_data_ptr()); - if(!isnan(t[0])) this->home_offset[0] = t[0]; - if(!isnan(t[1])) this->home_offset[1] = t[1]; - if(!isnan(t[2])) this->home_offset[2] = t[2]; + if(!isnan(t[0])) homing_axis[0].home_offset= t[0]; + if(!isnan(t[1])) homing_axis[1].home_offset= t[1]; + if(!isnan(t[2])) homing_axis[2].home_offset= t[2]; } }