| 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 "modules/robot/ActuatorCoordinates.h" |
| 13 | #include "Endstops.h" |
| 14 | #include "libs/nuts_bolts.h" |
| 15 | #include "libs/Pin.h" |
| 16 | #include "libs/StepperMotor.h" |
| 17 | #include "wait_api.h" // mbed.h lib |
| 18 | #include "Robot.h" |
| 19 | #include "Stepper.h" |
| 20 | #include "Config.h" |
| 21 | #include "SlowTicker.h" |
| 22 | #include "Planner.h" |
| 23 | #include "checksumm.h" |
| 24 | #include "utils.h" |
| 25 | #include "ConfigValue.h" |
| 26 | #include "libs/StreamOutput.h" |
| 27 | #include "PublicDataRequest.h" |
| 28 | #include "EndstopsPublicAccess.h" |
| 29 | #include "StreamOutputPool.h" |
| 30 | #include "StepTicker.h" |
| 31 | #include "BaseSolution.h" |
| 32 | #include "SerialMessage.h" |
| 33 | |
| 34 | #include <ctype.h> |
| 35 | |
| 36 | #define ALPHA_AXIS 0 |
| 37 | #define BETA_AXIS 1 |
| 38 | #define GAMMA_AXIS 2 |
| 39 | #define X_AXIS 0 |
| 40 | #define Y_AXIS 1 |
| 41 | #define Z_AXIS 2 |
| 42 | |
| 43 | #define endstops_module_enable_checksum CHECKSUM("endstops_enable") |
| 44 | #define corexy_homing_checksum CHECKSUM("corexy_homing") |
| 45 | #define delta_homing_checksum CHECKSUM("delta_homing") |
| 46 | #define rdelta_homing_checksum CHECKSUM("rdelta_homing") |
| 47 | #define scara_homing_checksum CHECKSUM("scara_homing") |
| 48 | |
| 49 | #define alpha_min_endstop_checksum CHECKSUM("alpha_min_endstop") |
| 50 | #define beta_min_endstop_checksum CHECKSUM("beta_min_endstop") |
| 51 | #define gamma_min_endstop_checksum CHECKSUM("gamma_min_endstop") |
| 52 | |
| 53 | #define alpha_max_endstop_checksum CHECKSUM("alpha_max_endstop") |
| 54 | #define beta_max_endstop_checksum CHECKSUM("beta_max_endstop") |
| 55 | #define gamma_max_endstop_checksum CHECKSUM("gamma_max_endstop") |
| 56 | |
| 57 | #define alpha_trim_checksum CHECKSUM("alpha_trim") |
| 58 | #define beta_trim_checksum CHECKSUM("beta_trim") |
| 59 | #define gamma_trim_checksum CHECKSUM("gamma_trim") |
| 60 | |
| 61 | // these values are in steps and should be deprecated |
| 62 | #define alpha_fast_homing_rate_checksum CHECKSUM("alpha_fast_homing_rate") |
| 63 | #define beta_fast_homing_rate_checksum CHECKSUM("beta_fast_homing_rate") |
| 64 | #define gamma_fast_homing_rate_checksum CHECKSUM("gamma_fast_homing_rate") |
| 65 | |
| 66 | #define alpha_slow_homing_rate_checksum CHECKSUM("alpha_slow_homing_rate") |
| 67 | #define beta_slow_homing_rate_checksum CHECKSUM("beta_slow_homing_rate") |
| 68 | #define gamma_slow_homing_rate_checksum CHECKSUM("gamma_slow_homing_rate") |
| 69 | |
| 70 | #define alpha_homing_retract_checksum CHECKSUM("alpha_homing_retract") |
| 71 | #define beta_homing_retract_checksum CHECKSUM("beta_homing_retract") |
| 72 | #define gamma_homing_retract_checksum CHECKSUM("gamma_homing_retract") |
| 73 | |
| 74 | // same as above but in user friendly mm/s and mm |
| 75 | #define alpha_fast_homing_rate_mm_checksum CHECKSUM("alpha_fast_homing_rate_mm_s") |
| 76 | #define beta_fast_homing_rate_mm_checksum CHECKSUM("beta_fast_homing_rate_mm_s") |
| 77 | #define gamma_fast_homing_rate_mm_checksum CHECKSUM("gamma_fast_homing_rate_mm_s") |
| 78 | |
| 79 | #define alpha_slow_homing_rate_mm_checksum CHECKSUM("alpha_slow_homing_rate_mm_s") |
| 80 | #define beta_slow_homing_rate_mm_checksum CHECKSUM("beta_slow_homing_rate_mm_s") |
| 81 | #define gamma_slow_homing_rate_mm_checksum CHECKSUM("gamma_slow_homing_rate_mm_s") |
| 82 | |
| 83 | #define alpha_homing_retract_mm_checksum CHECKSUM("alpha_homing_retract_mm") |
| 84 | #define beta_homing_retract_mm_checksum CHECKSUM("beta_homing_retract_mm") |
| 85 | #define gamma_homing_retract_mm_checksum CHECKSUM("gamma_homing_retract_mm") |
| 86 | |
| 87 | #define endstop_debounce_count_checksum CHECKSUM("endstop_debounce_count") |
| 88 | |
| 89 | #define alpha_homing_direction_checksum CHECKSUM("alpha_homing_direction") |
| 90 | #define beta_homing_direction_checksum CHECKSUM("beta_homing_direction") |
| 91 | #define gamma_homing_direction_checksum CHECKSUM("gamma_homing_direction") |
| 92 | #define home_to_max_checksum CHECKSUM("home_to_max") |
| 93 | #define home_to_min_checksum CHECKSUM("home_to_min") |
| 94 | #define alpha_min_checksum CHECKSUM("alpha_min") |
| 95 | #define beta_min_checksum CHECKSUM("beta_min") |
| 96 | #define gamma_min_checksum CHECKSUM("gamma_min") |
| 97 | |
| 98 | #define alpha_max_checksum CHECKSUM("alpha_max") |
| 99 | #define beta_max_checksum CHECKSUM("beta_max") |
| 100 | #define gamma_max_checksum CHECKSUM("gamma_max") |
| 101 | |
| 102 | #define alpha_limit_enable_checksum CHECKSUM("alpha_limit_enable") |
| 103 | #define beta_limit_enable_checksum CHECKSUM("beta_limit_enable") |
| 104 | #define gamma_limit_enable_checksum CHECKSUM("gamma_limit_enable") |
| 105 | |
| 106 | #define homing_order_checksum CHECKSUM("homing_order") |
| 107 | #define move_to_origin_checksum CHECKSUM("move_to_origin_after_home") |
| 108 | |
| 109 | #define STEPPER THEKERNEL->robot->actuators |
| 110 | #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) |
| 111 | |
| 112 | |
| 113 | // Homing States |
| 114 | enum { |
| 115 | MOVING_TO_ENDSTOP_FAST, // homing move |
| 116 | MOVING_BACK, // homing move |
| 117 | MOVING_TO_ENDSTOP_SLOW, // homing move |
| 118 | NOT_HOMING, |
| 119 | BACK_OFF_HOME, |
| 120 | MOVE_TO_ORIGIN, |
| 121 | LIMIT_TRIGGERED |
| 122 | }; |
| 123 | |
| 124 | Endstops::Endstops() |
| 125 | { |
| 126 | this->status = NOT_HOMING; |
| 127 | home_offset[0] = home_offset[1] = home_offset[2] = 0.0F; |
| 128 | } |
| 129 | |
| 130 | void Endstops::on_module_loaded() |
| 131 | { |
| 132 | // Do not do anything if not enabled |
| 133 | if ( THEKERNEL->config->value( endstops_module_enable_checksum )->by_default(true)->as_bool() == false ) { |
| 134 | delete this; |
| 135 | return; |
| 136 | } |
| 137 | |
| 138 | register_for_event(ON_GCODE_RECEIVED); |
| 139 | register_for_event(ON_GET_PUBLIC_DATA); |
| 140 | register_for_event(ON_SET_PUBLIC_DATA); |
| 141 | |
| 142 | THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {acceleration_tick(); }); |
| 143 | |
| 144 | // Settings |
| 145 | this->on_config_reload(this); |
| 146 | } |
| 147 | |
| 148 | // Get config |
| 149 | void Endstops::on_config_reload(void *argument) |
| 150 | { |
| 151 | this->pins[0].from_string( THEKERNEL->config->value(alpha_min_endstop_checksum )->by_default("nc" )->as_string())->as_input(); |
| 152 | this->pins[1].from_string( THEKERNEL->config->value(beta_min_endstop_checksum )->by_default("nc" )->as_string())->as_input(); |
| 153 | this->pins[2].from_string( THEKERNEL->config->value(gamma_min_endstop_checksum )->by_default("nc" )->as_string())->as_input(); |
| 154 | this->pins[3].from_string( THEKERNEL->config->value(alpha_max_endstop_checksum )->by_default("nc" )->as_string())->as_input(); |
| 155 | this->pins[4].from_string( THEKERNEL->config->value(beta_max_endstop_checksum )->by_default("nc" )->as_string())->as_input(); |
| 156 | this->pins[5].from_string( THEKERNEL->config->value(gamma_max_endstop_checksum )->by_default("nc" )->as_string())->as_input(); |
| 157 | |
| 158 | // These are the old ones in steps still here for backwards compatibility |
| 159 | this->fast_rates[0] = THEKERNEL->config->value(alpha_fast_homing_rate_checksum )->by_default(4000 )->as_number() / STEPS_PER_MM(0); |
| 160 | this->fast_rates[1] = THEKERNEL->config->value(beta_fast_homing_rate_checksum )->by_default(4000 )->as_number() / STEPS_PER_MM(1); |
| 161 | this->fast_rates[2] = THEKERNEL->config->value(gamma_fast_homing_rate_checksum )->by_default(6400 )->as_number() / STEPS_PER_MM(2); |
| 162 | this->slow_rates[0] = THEKERNEL->config->value(alpha_slow_homing_rate_checksum )->by_default(2000 )->as_number() / STEPS_PER_MM(0); |
| 163 | this->slow_rates[1] = THEKERNEL->config->value(beta_slow_homing_rate_checksum )->by_default(2000 )->as_number() / STEPS_PER_MM(1); |
| 164 | this->slow_rates[2] = THEKERNEL->config->value(gamma_slow_homing_rate_checksum )->by_default(3200 )->as_number() / STEPS_PER_MM(2); |
| 165 | this->retract_mm[0] = THEKERNEL->config->value(alpha_homing_retract_checksum )->by_default(400 )->as_number() / STEPS_PER_MM(0); |
| 166 | this->retract_mm[1] = THEKERNEL->config->value(beta_homing_retract_checksum )->by_default(400 )->as_number() / STEPS_PER_MM(1); |
| 167 | this->retract_mm[2] = THEKERNEL->config->value(gamma_homing_retract_checksum )->by_default(1600 )->as_number() / STEPS_PER_MM(2); |
| 168 | |
| 169 | // 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 |
| 170 | this->fast_rates[0] = THEKERNEL->config->value(alpha_fast_homing_rate_mm_checksum )->by_default(this->fast_rates[0])->as_number(); |
| 171 | this->fast_rates[1] = THEKERNEL->config->value(beta_fast_homing_rate_mm_checksum )->by_default(this->fast_rates[1])->as_number(); |
| 172 | this->fast_rates[2] = THEKERNEL->config->value(gamma_fast_homing_rate_mm_checksum )->by_default(this->fast_rates[2])->as_number(); |
| 173 | this->slow_rates[0] = THEKERNEL->config->value(alpha_slow_homing_rate_mm_checksum )->by_default(this->slow_rates[0])->as_number(); |
| 174 | this->slow_rates[1] = THEKERNEL->config->value(beta_slow_homing_rate_mm_checksum )->by_default(this->slow_rates[1])->as_number(); |
| 175 | this->slow_rates[2] = THEKERNEL->config->value(gamma_slow_homing_rate_mm_checksum )->by_default(this->slow_rates[2])->as_number(); |
| 176 | this->retract_mm[0] = THEKERNEL->config->value(alpha_homing_retract_mm_checksum )->by_default(this->retract_mm[0])->as_number(); |
| 177 | this->retract_mm[1] = THEKERNEL->config->value(beta_homing_retract_mm_checksum )->by_default(this->retract_mm[1])->as_number(); |
| 178 | this->retract_mm[2] = THEKERNEL->config->value(gamma_homing_retract_mm_checksum )->by_default(this->retract_mm[2])->as_number(); |
| 179 | |
| 180 | this->debounce_count = THEKERNEL->config->value(endstop_debounce_count_checksum )->by_default(100)->as_number(); |
| 181 | |
| 182 | // get homing direction and convert to boolean where true is home to min, and false is home to max |
| 183 | int home_dir = get_checksum(THEKERNEL->config->value(alpha_homing_direction_checksum)->by_default("home_to_min")->as_string()); |
| 184 | this->home_direction[0] = home_dir != home_to_max_checksum; |
| 185 | |
| 186 | home_dir = get_checksum(THEKERNEL->config->value(beta_homing_direction_checksum)->by_default("home_to_min")->as_string()); |
| 187 | this->home_direction[1] = home_dir != home_to_max_checksum; |
| 188 | |
| 189 | home_dir = get_checksum(THEKERNEL->config->value(gamma_homing_direction_checksum)->by_default("home_to_min")->as_string()); |
| 190 | this->home_direction[2] = home_dir != home_to_max_checksum; |
| 191 | |
| 192 | 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(); |
| 193 | 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(); |
| 194 | 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(); |
| 195 | |
| 196 | this->is_corexy = THEKERNEL->config->value(corexy_homing_checksum)->by_default(false)->as_bool(); |
| 197 | this->is_delta = THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool(); |
| 198 | this->is_rdelta = THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool(); |
| 199 | this->is_scara = THEKERNEL->config->value(scara_homing_checksum)->by_default(false)->as_bool(); |
| 200 | |
| 201 | // see if an order has been specified, must be three characters, XYZ or YXZ etc |
| 202 | string order = THEKERNEL->config->value(homing_order_checksum)->by_default("")->as_string(); |
| 203 | this->homing_order = 0; |
| 204 | if(order.size() == 3 && !this->is_delta) { |
| 205 | int shift = 0; |
| 206 | for(auto c : order) { |
| 207 | uint8_t i = toupper(c) - 'X'; |
| 208 | if(i > 2) { // bad value |
| 209 | this->homing_order = 0; |
| 210 | break; |
| 211 | } |
| 212 | homing_order |= (i << shift); |
| 213 | shift += 2; |
| 214 | } |
| 215 | } |
| 216 | |
| 217 | // endstop trim used by deltas to do soft adjusting |
| 218 | // on a delta homing to max, a negative trim value will move the carriage down, and a positive will move it up |
| 219 | this->trim_mm[0] = THEKERNEL->config->value(alpha_trim_checksum )->by_default(0 )->as_number(); |
| 220 | this->trim_mm[1] = THEKERNEL->config->value(beta_trim_checksum )->by_default(0 )->as_number(); |
| 221 | this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum )->by_default(0 )->as_number(); |
| 222 | |
| 223 | // limits enabled |
| 224 | this->limit_enable[X_AXIS] = THEKERNEL->config->value(alpha_limit_enable_checksum)->by_default(false)->as_bool(); |
| 225 | this->limit_enable[Y_AXIS] = THEKERNEL->config->value(beta_limit_enable_checksum)->by_default(false)->as_bool(); |
| 226 | this->limit_enable[Z_AXIS] = THEKERNEL->config->value(gamma_limit_enable_checksum)->by_default(false)->as_bool(); |
| 227 | |
| 228 | // set to true by default for deltas duwe to trim, false on cartesians |
| 229 | this->move_to_origin_after_home = THEKERNEL->config->value(move_to_origin_checksum)->by_default(is_delta)->as_bool(); |
| 230 | |
| 231 | if(this->limit_enable[X_AXIS] || this->limit_enable[Y_AXIS] || this->limit_enable[Z_AXIS]) { |
| 232 | register_for_event(ON_IDLE); |
| 233 | if(this->is_delta) { |
| 234 | // we must enable all the limits not just one |
| 235 | this->limit_enable[X_AXIS] = true; |
| 236 | this->limit_enable[Y_AXIS] = true; |
| 237 | this->limit_enable[Z_AXIS] = true; |
| 238 | } |
| 239 | } |
| 240 | |
| 241 | // |
| 242 | if(this->is_delta || this->is_rdelta) { |
| 243 | // some things must be the same or they will die, so force it here to avoid config errors |
| 244 | this->fast_rates[1] = this->fast_rates[2] = this->fast_rates[0]; |
| 245 | this->slow_rates[1] = this->slow_rates[2] = this->slow_rates[0]; |
| 246 | this->retract_mm[1] = this->retract_mm[2] = this->retract_mm[0]; |
| 247 | this->home_direction[1] = this->home_direction[2] = this->home_direction[0]; |
| 248 | // NOTE homing_position for rdelta is the angle of the actuator not the cartesian position |
| 249 | if(!this->is_rdelta) this->homing_position[0] = this->homing_position[1] = 0; |
| 250 | } |
| 251 | } |
| 252 | |
| 253 | bool Endstops::debounced_get(int pin) |
| 254 | { |
| 255 | uint8_t debounce = 0; |
| 256 | while(this->pins[pin].get()) { |
| 257 | if ( ++debounce >= this->debounce_count ) { |
| 258 | // pin triggered |
| 259 | return true; |
| 260 | } |
| 261 | } |
| 262 | return false; |
| 263 | } |
| 264 | |
| 265 | static const char *endstop_names[] = {"min_x", "min_y", "min_z", "max_x", "max_y", "max_z"}; |
| 266 | |
| 267 | void Endstops::on_idle(void *argument) |
| 268 | { |
| 269 | if(this->status == LIMIT_TRIGGERED) { |
| 270 | // if we were in limit triggered see if it has been cleared |
| 271 | for( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 272 | if(this->limit_enable[c]) { |
| 273 | std::array<int, 2> minmax{{0, 3}}; |
| 274 | // check min and max endstops |
| 275 | for (int i : minmax) { |
| 276 | int n = c + i; |
| 277 | if(this->pins[n].get()) { |
| 278 | // still triggered, so exit |
| 279 | bounce_cnt = 0; |
| 280 | return; |
| 281 | } |
| 282 | } |
| 283 | } |
| 284 | } |
| 285 | if(++bounce_cnt > 10) { // can use less as it calls on_idle in between |
| 286 | // clear the state |
| 287 | this->status = NOT_HOMING; |
| 288 | } |
| 289 | return; |
| 290 | |
| 291 | } else if(this->status != NOT_HOMING) { |
| 292 | // don't check while homing |
| 293 | return; |
| 294 | } |
| 295 | |
| 296 | for( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 297 | if(this->limit_enable[c] && STEPPER[c]->is_moving()) { |
| 298 | std::array<int, 2> minmax{{0, 3}}; |
| 299 | // check min and max endstops |
| 300 | for (int i : minmax) { |
| 301 | int n = c + i; |
| 302 | if(debounced_get(n)) { |
| 303 | // endstop triggered |
| 304 | THEKERNEL->streams->printf("Limit switch %s was hit - reset or M999 required\n", endstop_names[n]); |
| 305 | this->status = LIMIT_TRIGGERED; |
| 306 | // disables heaters and motors, ignores incoming Gcode and flushes block queue |
| 307 | THEKERNEL->call_event(ON_HALT, nullptr); |
| 308 | return; |
| 309 | } |
| 310 | } |
| 311 | } |
| 312 | } |
| 313 | } |
| 314 | |
| 315 | // if limit switches are enabled, then we must move off of the endstop otherwise we won't be able to move |
| 316 | // checks if triggered and only backs off if triggered |
| 317 | void Endstops::back_off_home(char axes_to_move) |
| 318 | { |
| 319 | std::vector<std::pair<char, float>> params; |
| 320 | this->status = BACK_OFF_HOME; |
| 321 | |
| 322 | // these are handled differently |
| 323 | if(is_delta) { |
| 324 | // Move off of the endstop using a regular relative move in Z only |
| 325 | params.push_back({'Z', this->retract_mm[Z_AXIS] * (this->home_direction[Z_AXIS] ? 1 : -1)}); |
| 326 | |
| 327 | } else { |
| 328 | // cartesians, concatenate all the moves we need to do into one gcode |
| 329 | for( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 330 | if( ((axes_to_move >> c ) & 1) == 0) continue; // only for axes we asked to move |
| 331 | |
| 332 | // if not triggered no need to move off |
| 333 | if(this->limit_enable[c] && debounced_get(c + (this->home_direction[c] ? 0 : 3)) ) { |
| 334 | params.push_back({c + 'X', this->retract_mm[c] * (this->home_direction[c] ? 1 : -1)}); |
| 335 | } |
| 336 | } |
| 337 | } |
| 338 | |
| 339 | if(!params.empty()) { |
| 340 | // Move off of the endstop using a regular relative move |
| 341 | params.insert(params.begin(), {'G', 0}); |
| 342 | // use X slow rate to move, Z should have a max speed set anyway |
| 343 | params.push_back({'F', this->slow_rates[X_AXIS] * 60.0F}); |
| 344 | char gcode_buf[64]; |
| 345 | append_parameters(gcode_buf, params, sizeof(gcode_buf)); |
| 346 | Gcode gc(gcode_buf, &(StreamOutput::NullStream)); |
| 347 | bool oldmode = THEKERNEL->robot->absolute_mode; |
| 348 | THEKERNEL->robot->absolute_mode = false; // needs to be relative mode |
| 349 | THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly |
| 350 | THEKERNEL->robot->absolute_mode = oldmode; // restore mode |
| 351 | // Wait for above to finish |
| 352 | THEKERNEL->conveyor->wait_for_empty_queue(); |
| 353 | } |
| 354 | |
| 355 | this->status = NOT_HOMING; |
| 356 | } |
| 357 | |
| 358 | // If enabled will move the head to 0,0 after homing, but only if X and Y were set to home |
| 359 | void Endstops::move_to_origin(char axes_to_move) |
| 360 | { |
| 361 | if( (axes_to_move & 0x03) != 3 ) return; // ignore if X and Y not homing |
| 362 | |
| 363 | // Do we need to check if we are already at 0,0? probably not as the G0 will not do anything if we are |
| 364 | // float pos[3]; THEKERNEL->robot->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return; |
| 365 | |
| 366 | this->status = MOVE_TO_ORIGIN; |
| 367 | // Move to center using a regular move, use slower of X and Y fast rate |
| 368 | float rate = std::min(this->fast_rates[0], this->fast_rates[1]) * 60.0F; |
| 369 | char buf[32]; |
| 370 | snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", rate); // must use machine coordinates in case G92 or WCS is in effect |
| 371 | struct SerialMessage message; |
| 372 | message.message = buf; |
| 373 | message.stream = &(StreamOutput::NullStream); |
| 374 | THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command |
| 375 | // Wait for above to finish |
| 376 | THEKERNEL->conveyor->wait_for_empty_queue(); |
| 377 | this->status = NOT_HOMING; |
| 378 | } |
| 379 | |
| 380 | bool Endstops::wait_for_homed(char axes_to_move) |
| 381 | { |
| 382 | bool running = true; |
| 383 | unsigned int debounce[3] = {0, 0, 0}; |
| 384 | while (running) { |
| 385 | running = false; |
| 386 | THEKERNEL->call_event(ON_IDLE); |
| 387 | |
| 388 | // check if on_halt (eg kill) |
| 389 | if(THEKERNEL->is_halted()) return false; |
| 390 | |
| 391 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 392 | if ( ( axes_to_move >> c ) & 1 ) { |
| 393 | if ( this->pins[c + (this->home_direction[c] ? 0 : 3)].get() ) { |
| 394 | if ( debounce[c] < debounce_count ) { |
| 395 | debounce[c]++; |
| 396 | running = true; |
| 397 | } else if ( STEPPER[c]->is_moving() ) { |
| 398 | STEPPER[c]->move(0, 0); |
| 399 | axes_to_move &= ~(1 << c); // no need to check it again |
| 400 | } |
| 401 | } else { |
| 402 | // The endstop was not hit yet |
| 403 | running = true; |
| 404 | debounce[c] = 0; |
| 405 | } |
| 406 | } |
| 407 | } |
| 408 | } |
| 409 | return true; |
| 410 | } |
| 411 | |
| 412 | void Endstops::do_homing_cartesian(char axes_to_move) |
| 413 | { |
| 414 | // check if on_halt (eg kill) |
| 415 | if(THEKERNEL->is_halted()) return; |
| 416 | |
| 417 | // this homing works for cartesian and delta printers |
| 418 | // Start moving the axes to the origin |
| 419 | this->status = MOVING_TO_ENDSTOP_FAST; |
| 420 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 421 | if ( ( axes_to_move >> c) & 1 ) { |
| 422 | this->feed_rate[c] = this->fast_rates[c]; |
| 423 | STEPPER[c]->move(this->home_direction[c], 10000000, 0); |
| 424 | } |
| 425 | } |
| 426 | |
| 427 | // Wait for all axes to have homed |
| 428 | if(!this->wait_for_homed(axes_to_move)) return; |
| 429 | |
| 430 | // Move back a small distance |
| 431 | this->status = MOVING_BACK; |
| 432 | bool inverted_dir; |
| 433 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 434 | if ( ( axes_to_move >> c ) & 1 ) { |
| 435 | inverted_dir = !this->home_direction[c]; |
| 436 | this->feed_rate[c] = this->slow_rates[c]; |
| 437 | STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c), 0); |
| 438 | } |
| 439 | } |
| 440 | |
| 441 | // Wait for moves to be done |
| 442 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 443 | if ( ( axes_to_move >> c ) & 1 ) { |
| 444 | while ( STEPPER[c]->is_moving() ) { |
| 445 | THEKERNEL->call_event(ON_IDLE); |
| 446 | if(THEKERNEL->is_halted()) return; |
| 447 | } |
| 448 | } |
| 449 | } |
| 450 | |
| 451 | // Start moving the axes to the origin slowly |
| 452 | this->status = MOVING_TO_ENDSTOP_SLOW; |
| 453 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 454 | if ( ( axes_to_move >> c ) & 1 ) { |
| 455 | this->feed_rate[c] = this->slow_rates[c]; |
| 456 | STEPPER[c]->move(this->home_direction[c], 10000000, 0); |
| 457 | } |
| 458 | } |
| 459 | |
| 460 | // Wait for all axes to have homed |
| 461 | if(!this->wait_for_homed(axes_to_move)) return; |
| 462 | } |
| 463 | |
| 464 | bool Endstops::wait_for_homed_corexy(int axis) |
| 465 | { |
| 466 | bool running = true; |
| 467 | unsigned int debounce[3] = {0, 0, 0}; |
| 468 | while (running) { |
| 469 | running = false; |
| 470 | THEKERNEL->call_event(ON_IDLE); |
| 471 | |
| 472 | // check if on_halt (eg kill) |
| 473 | if(THEKERNEL->is_halted()) return false; |
| 474 | |
| 475 | if ( this->pins[axis + (this->home_direction[axis] ? 0 : 3)].get() ) { |
| 476 | if ( debounce[axis] < debounce_count ) { |
| 477 | debounce[axis] ++; |
| 478 | running = true; |
| 479 | } else { |
| 480 | // turn both off if running |
| 481 | if (STEPPER[X_AXIS]->is_moving()) STEPPER[X_AXIS]->move(0, 0); |
| 482 | if (STEPPER[Y_AXIS]->is_moving()) STEPPER[Y_AXIS]->move(0, 0); |
| 483 | } |
| 484 | } else { |
| 485 | // The endstop was not hit yet |
| 486 | running = true; |
| 487 | debounce[axis] = 0; |
| 488 | } |
| 489 | } |
| 490 | return true; |
| 491 | } |
| 492 | |
| 493 | void Endstops::corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps) |
| 494 | { |
| 495 | // check if on_halt (eg kill) |
| 496 | if(THEKERNEL->is_halted()) return; |
| 497 | |
| 498 | this->status = MOVING_TO_ENDSTOP_FAST; |
| 499 | this->feed_rate[X_AXIS] = fast_rate; |
| 500 | STEPPER[X_AXIS]->move(dirx, 10000000, 0); |
| 501 | this->feed_rate[Y_AXIS] = fast_rate; |
| 502 | STEPPER[Y_AXIS]->move(diry, 10000000, 0); |
| 503 | |
| 504 | // wait for primary axis |
| 505 | if(!this->wait_for_homed_corexy(home_axis)) return; |
| 506 | |
| 507 | // Move back a small distance |
| 508 | this->status = MOVING_BACK; |
| 509 | this->feed_rate[X_AXIS] = slow_rate; |
| 510 | STEPPER[X_AXIS]->move(!dirx, retract_steps, 0); |
| 511 | this->feed_rate[Y_AXIS] = slow_rate; |
| 512 | STEPPER[Y_AXIS]->move(!diry, retract_steps, 0); |
| 513 | |
| 514 | // wait until done |
| 515 | while ( STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving()) { |
| 516 | THEKERNEL->call_event(ON_IDLE); |
| 517 | if(THEKERNEL->is_halted()) return; |
| 518 | } |
| 519 | |
| 520 | // Start moving the axes to the origin slowly |
| 521 | this->status = MOVING_TO_ENDSTOP_SLOW; |
| 522 | this->feed_rate[X_AXIS] = slow_rate; |
| 523 | STEPPER[X_AXIS]->move(dirx, 10000000, 0); |
| 524 | this->feed_rate[Y_AXIS] = slow_rate; |
| 525 | STEPPER[Y_AXIS]->move(diry, 10000000, 0); |
| 526 | |
| 527 | // wait for primary axis |
| 528 | if(!this->wait_for_homed_corexy(home_axis)) return; |
| 529 | } |
| 530 | |
| 531 | // this homing works for HBots/CoreXY |
| 532 | void Endstops::do_homing_corexy(char axes_to_move) |
| 533 | { |
| 534 | // TODO should really make order configurable, and select whether to allow XY to home at the same time, diagonally |
| 535 | // To move XY at the same time only one motor needs to turn, determine which motor and which direction based on min or max directions |
| 536 | // allow to move until an endstop triggers, then stop that motor. Speed up when moving diagonally to match X or Y speed |
| 537 | // continue moving in the direction not yet triggered (which means two motors turning) until endstop hit |
| 538 | |
| 539 | if((axes_to_move & 0x03) == 0x03) { // both X and Y need Homing |
| 540 | // determine which motor to turn and which way |
| 541 | bool dirx = this->home_direction[X_AXIS]; |
| 542 | bool diry = this->home_direction[Y_AXIS]; |
| 543 | int motor; |
| 544 | bool dir; |
| 545 | if(dirx && diry) { // min/min |
| 546 | motor = X_AXIS; |
| 547 | dir = true; |
| 548 | } else if(dirx && !diry) { // min/max |
| 549 | motor = Y_AXIS; |
| 550 | dir = true; |
| 551 | } else if(!dirx && diry) { // max/min |
| 552 | motor = Y_AXIS; |
| 553 | dir = false; |
| 554 | } else if(!dirx && !diry) { // max/max |
| 555 | motor = X_AXIS; |
| 556 | dir = false; |
| 557 | } |
| 558 | |
| 559 | // then move both X and Y until one hits the endstop |
| 560 | this->status = MOVING_TO_ENDSTOP_FAST; |
| 561 | // need to allow for more ground covered when moving diagonally |
| 562 | this->feed_rate[motor] = this->fast_rates[motor] * 1.4142; |
| 563 | STEPPER[motor]->move(dir, 10000000, 0); |
| 564 | // wait until either X or Y hits the endstop |
| 565 | bool running = true; |
| 566 | while (running) { |
| 567 | THEKERNEL->call_event(ON_IDLE); |
| 568 | if(THEKERNEL->is_halted()) return; |
| 569 | for(int m = X_AXIS; m <= Y_AXIS; m++) { |
| 570 | if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) { |
| 571 | // turn off motor |
| 572 | if(STEPPER[motor]->is_moving()) STEPPER[motor]->move(0, 0); |
| 573 | running = false; |
| 574 | break; |
| 575 | } |
| 576 | } |
| 577 | } |
| 578 | } |
| 579 | |
| 580 | // move individual axis |
| 581 | if (axes_to_move & 0x01) { // Home X, which means both X and Y in same direction |
| 582 | bool dir = this->home_direction[X_AXIS]; |
| 583 | corexy_home(X_AXIS, dir, dir, this->fast_rates[X_AXIS], this->slow_rates[X_AXIS], this->retract_mm[X_AXIS]*STEPS_PER_MM(X_AXIS)); |
| 584 | } |
| 585 | |
| 586 | if (axes_to_move & 0x02) { // Home Y, which means both X and Y in different directions |
| 587 | bool dir = this->home_direction[Y_AXIS]; |
| 588 | corexy_home(Y_AXIS, dir, !dir, this->fast_rates[Y_AXIS], this->slow_rates[Y_AXIS], this->retract_mm[Y_AXIS]*STEPS_PER_MM(Y_AXIS)); |
| 589 | } |
| 590 | |
| 591 | if (axes_to_move & 0x04) { // move Z |
| 592 | do_homing_cartesian(0x04); // just home normally for Z |
| 593 | } |
| 594 | } |
| 595 | |
| 596 | void Endstops::home(char axes_to_move) |
| 597 | { |
| 598 | // not a block move so disable the last tick setting |
| 599 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 600 | STEPPER[c]->set_moved_last_block(false); |
| 601 | } |
| 602 | |
| 603 | if (is_corexy) { |
| 604 | // corexy/HBot homing |
| 605 | do_homing_corexy(axes_to_move); |
| 606 | } else { |
| 607 | // cartesian/delta homing |
| 608 | do_homing_cartesian(axes_to_move); |
| 609 | } |
| 610 | |
| 611 | // make sure all steppers are off (especially if aborted) |
| 612 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 613 | STEPPER[c]->move(0, 0); |
| 614 | } |
| 615 | this->status = NOT_HOMING; |
| 616 | } |
| 617 | |
| 618 | // Start homing sequences by response to GCode commands |
| 619 | void Endstops::on_gcode_received(void *argument) |
| 620 | { |
| 621 | Gcode *gcode = static_cast<Gcode *>(argument); |
| 622 | if ( gcode->has_g && gcode->g == 28) { |
| 623 | if( (gcode->subcode == 0 && THEKERNEL->is_grbl_mode()) || (gcode->subcode == 2 && !THEKERNEL->is_grbl_mode()) ) { |
| 624 | // G28 in grbl mode or G28.2 in normal mode will do a rapid to the predefined position |
| 625 | // TODO spec says if XYZ specified move to them first then move to MCS of specifed axis |
| 626 | char buf[32]; |
| 627 | 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 |
| 628 | struct SerialMessage message; |
| 629 | message.message = buf; |
| 630 | message.stream = &(StreamOutput::NullStream); |
| 631 | THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command |
| 632 | return; |
| 633 | |
| 634 | } else if(THEKERNEL->is_grbl_mode() && gcode->subcode == 2) { // G28.2 in grbl mode forces homing (triggered by $H) |
| 635 | // fall through so it does homing cycle |
| 636 | |
| 637 | } else if(gcode->subcode == 1) { // G28.1 set pre defined position |
| 638 | // saves current position in absolute machine coordinates |
| 639 | THEKERNEL->robot->get_axis_position(saved_position); |
| 640 | return; |
| 641 | |
| 642 | } else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing |
| 643 | if(gcode->get_num_args() == 0) { |
| 644 | THEKERNEL->robot->reset_axis_position(0, 0, 0); |
| 645 | } else { |
| 646 | // do a manual homing based on given coordinates, no endstops required |
| 647 | if(gcode->has_letter('X')) THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS); |
| 648 | if(gcode->has_letter('Y')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS); |
| 649 | if(gcode->has_letter('Z')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS); |
| 650 | } |
| 651 | return; |
| 652 | |
| 653 | } else if(gcode->subcode == 4) { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta) |
| 654 | // do a manual homing based on given coordinates, no endstops required |
| 655 | float a=NAN, b=NAN, c=NAN; |
| 656 | if(gcode->has_letter('A')) a= gcode->get_value('A'); |
| 657 | if(gcode->has_letter('B')) b= gcode->get_value('B'); |
| 658 | if(gcode->has_letter('C')) c= gcode->get_value('C'); |
| 659 | THEKERNEL->robot->reset_actuator_position(a, b, c); |
| 660 | return; |
| 661 | |
| 662 | } else if(THEKERNEL->is_grbl_mode()) { |
| 663 | gcode->stream->printf("error:Unsupported command\n"); |
| 664 | return; |
| 665 | } |
| 666 | |
| 667 | // G28 is received, we have homing to do |
| 668 | |
| 669 | // First wait for the queue to be empty |
| 670 | THEKERNEL->conveyor->wait_for_empty_queue(); |
| 671 | |
| 672 | // Do we move select axes or all of them |
| 673 | char axes_to_move = 0; |
| 674 | // only enable homing if the endstop is defined, deltas, scaras always home all axis |
| 675 | bool home_all = this->is_delta || this->is_rdelta || this->is_scara || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ); |
| 676 | |
| 677 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 678 | if ( (home_all || gcode->has_letter(c + 'X')) && this->pins[c + (this->home_direction[c] ? 0 : 3)].connected() ) { |
| 679 | axes_to_move += ( 1 << c ); |
| 680 | } |
| 681 | } |
| 682 | |
| 683 | // Enable the motors |
| 684 | THEKERNEL->stepper->turn_enable_pins_on(); |
| 685 | |
| 686 | // do the actual homing |
| 687 | if(homing_order != 0) { |
| 688 | // if an order has been specified do it in the specified order |
| 689 | // homing order is 0b00ccbbaa where aa is 0,1,2 to specify the first axis, bb is the second and cc is the third |
| 690 | // eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z |
| 691 | for (uint8_t m = homing_order; m != 0; m >>= 2) { |
| 692 | int a = (1 << (m & 0x03)); // axis to move |
| 693 | if((a & axes_to_move) != 0) { |
| 694 | home(a); |
| 695 | } |
| 696 | // check if on_halt (eg kill) |
| 697 | if(THEKERNEL->is_halted()) break; |
| 698 | } |
| 699 | |
| 700 | } else { |
| 701 | // they all home at the same time |
| 702 | home(axes_to_move); |
| 703 | } |
| 704 | |
| 705 | // check if on_halt (eg kill) |
| 706 | if(THEKERNEL->is_halted()) { |
| 707 | if(!THEKERNEL->is_grbl_mode()) { |
| 708 | THEKERNEL->streams->printf("Homing cycle aborted by kill\n"); |
| 709 | } |
| 710 | return; |
| 711 | } |
| 712 | |
| 713 | // set the last probe position to the actuator units moved during this home |
| 714 | THEKERNEL->robot->set_last_probe_position(std::make_tuple(STEPPER[0]->get_stepped()/STEPS_PER_MM(0), STEPPER[1]->get_stepped()/STEPS_PER_MM(1), STEPPER[2]->get_stepped()/STEPS_PER_MM(2), 0)); |
| 715 | |
| 716 | if(home_all) { |
| 717 | // Here's where we would have been if the endstops were perfectly trimmed |
| 718 | // NOTE on a rotary delta home_offset is actuator position in degrees when homed and |
| 719 | // home_offset is the theta offset for each actuator, so M206 is used to set theta offset for each actuator in degrees |
| 720 | float ideal_position[3] = { |
| 721 | this->homing_position[X_AXIS] + this->home_offset[X_AXIS], |
| 722 | this->homing_position[Y_AXIS] + this->home_offset[Y_AXIS], |
| 723 | this->homing_position[Z_AXIS] + this->home_offset[Z_AXIS] |
| 724 | }; |
| 725 | |
| 726 | bool has_endstop_trim = this->is_delta || this->is_scara; |
| 727 | if (has_endstop_trim) { |
| 728 | ActuatorCoordinates ideal_actuator_position; |
| 729 | THEKERNEL->robot->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position); |
| 730 | |
| 731 | // We are actually not at the ideal position, but a trim away |
| 732 | ActuatorCoordinates real_actuator_position = { |
| 733 | ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS], |
| 734 | ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS], |
| 735 | ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS] |
| 736 | }; |
| 737 | |
| 738 | float real_position[3]; |
| 739 | THEKERNEL->robot->arm_solution->actuator_to_cartesian(real_actuator_position, real_position); |
| 740 | // Reset the actuator positions to correspond our real position |
| 741 | THEKERNEL->robot->reset_axis_position(real_position[0], real_position[1], real_position[2]); |
| 742 | |
| 743 | } else { |
| 744 | // without endstop trim, real_position == ideal_position |
| 745 | if(is_rdelta) { |
| 746 | // with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates |
| 747 | THEKERNEL->robot->reset_actuator_position(ideal_position[0], ideal_position[1], ideal_position[2]); |
| 748 | |
| 749 | }else{ |
| 750 | // Reset the actuator positions to correspond our real position |
| 751 | THEKERNEL->robot->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]); |
| 752 | } |
| 753 | } |
| 754 | |
| 755 | } else { |
| 756 | // Zero the ax(i/e)s position, add in the home offset |
| 757 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 758 | if ( (axes_to_move >> c) & 1 ) { |
| 759 | THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c); |
| 760 | } |
| 761 | } |
| 762 | } |
| 763 | |
| 764 | // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing |
| 765 | // default is off for cartesian on for deltas |
| 766 | if(!is_delta) { |
| 767 | // NOTE a rotary delta usually has optical or hall-effect endstops so it is safe to go past them a little bit |
| 768 | if(this->move_to_origin_after_home) move_to_origin(axes_to_move); |
| 769 | // if limit switches are enabled we must back off endstop after setting home |
| 770 | back_off_home(axes_to_move); |
| 771 | |
| 772 | } else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) { |
| 773 | // 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 |
| 774 | // also need to back off endstops if limits are enabled |
| 775 | back_off_home(axes_to_move); |
| 776 | if(this->move_to_origin_after_home) move_to_origin(axes_to_move); |
| 777 | } |
| 778 | } |
| 779 | |
| 780 | if (gcode->has_m) { |
| 781 | switch (gcode->m) { |
| 782 | case 119: { |
| 783 | for (int i = 0; i < 6; ++i) { |
| 784 | if(this->pins[i].connected()) |
| 785 | gcode->stream->printf("%s:%d ", endstop_names[i], this->pins[i].get()); |
| 786 | } |
| 787 | gcode->add_nl = true; |
| 788 | |
| 789 | } |
| 790 | break; |
| 791 | |
| 792 | case 206: // M206 - set homing offset |
| 793 | if(!is_rdelta) { |
| 794 | if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X'); |
| 795 | if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y'); |
| 796 | if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z'); |
| 797 | gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); |
| 798 | |
| 799 | }else{ |
| 800 | // set theta offset |
| 801 | if (gcode->has_letter('A')) home_offset[0] = gcode->get_value('A'); |
| 802 | if (gcode->has_letter('B')) home_offset[1] = gcode->get_value('B'); |
| 803 | if (gcode->has_letter('C')) home_offset[2] = gcode->get_value('C'); |
| 804 | gcode->stream->printf("Theta offset A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]); |
| 805 | } |
| 806 | break; |
| 807 | |
| 808 | case 306: |
| 809 | if(!is_rdelta) { // Similar to M206 and G92 but sets Homing offsets based on current position |
| 810 | float cartesian[3]; |
| 811 | THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot |
| 812 | if (gcode->has_letter('X')) { |
| 813 | home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X')); |
| 814 | THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS); |
| 815 | } |
| 816 | if (gcode->has_letter('Y')) { |
| 817 | home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y')); |
| 818 | THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS); |
| 819 | } |
| 820 | if (gcode->has_letter('Z')) { |
| 821 | home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z')); |
| 822 | THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS); |
| 823 | } |
| 824 | |
| 825 | gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); |
| 826 | |
| 827 | }else{ |
| 828 | // for a rotary delta M306 calibrates the homing angle |
| 829 | // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference |
| 830 | // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C) |
| 831 | |
| 832 | // get the current angle for each actuator |
| 833 | ActuatorCoordinates current_angle{ |
| 834 | THEKERNEL->robot->actuators[X_AXIS]->get_current_position(), |
| 835 | THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(), |
| 836 | THEKERNEL->robot->actuators[Z_AXIS]->get_current_position() |
| 837 | }; |
| 838 | |
| 839 | //figure out what home_offset needs to be to correct the homing_position |
| 840 | if (gcode->has_letter('A')) { |
| 841 | float a= gcode->get_value('A'); // what actual angle is |
| 842 | home_offset[0]= (a - current_angle[0]); |
| 843 | THEKERNEL->robot->reset_actuator_position(a, NAN, NAN); |
| 844 | } |
| 845 | if (gcode->has_letter('B')) { |
| 846 | float b= gcode->get_value('B'); |
| 847 | home_offset[1]= (b - current_angle[1]); |
| 848 | THEKERNEL->robot->reset_actuator_position(NAN, b, NAN); |
| 849 | } |
| 850 | if (gcode->has_letter('C')) { |
| 851 | float c= gcode->get_value('C'); |
| 852 | home_offset[2]= (c - current_angle[2]); |
| 853 | THEKERNEL->robot->reset_actuator_position(NAN, NAN, c); |
| 854 | } |
| 855 | |
| 856 | gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]); |
| 857 | } |
| 858 | break; |
| 859 | |
| 860 | case 500: // save settings |
| 861 | case 503: // print settings |
| 862 | if(!is_rdelta) |
| 863 | 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]); |
| 864 | else |
| 865 | 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]); |
| 866 | |
| 867 | if (this->is_delta || this->is_scara) { |
| 868 | 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]); |
| 869 | gcode->stream->printf(";Max Z\nM665 Z%1.3f\n", this->homing_position[2]); |
| 870 | } |
| 871 | if(saved_position[X_AXIS] != 0 || saved_position[Y_AXIS] != 0) { |
| 872 | gcode->stream->printf(";predefined position:\nG28.1 X%1.4f Y%1.4f Z%1.4f\n", saved_position[X_AXIS], saved_position[Y_AXIS], saved_position[Z_AXIS]); |
| 873 | } |
| 874 | break; |
| 875 | |
| 876 | case 665: |
| 877 | if (this->is_delta || this->is_scara) { // M665 - set max gamma/z height |
| 878 | float gamma_max = this->homing_position[2]; |
| 879 | if (gcode->has_letter('Z')) { |
| 880 | this->homing_position[2] = gamma_max = gcode->get_value('Z'); |
| 881 | } |
| 882 | gcode->stream->printf("Max Z %8.3f ", gamma_max); |
| 883 | gcode->add_nl = true; |
| 884 | } |
| 885 | break; |
| 886 | |
| 887 | case 666: |
| 888 | if(this->is_delta || this->is_scara) { // M666 - set trim for each axis in mm, NB negative mm trim is down |
| 889 | if (gcode->has_letter('X')) trim_mm[0] = gcode->get_value('X'); |
| 890 | if (gcode->has_letter('Y')) trim_mm[1] = gcode->get_value('Y'); |
| 891 | if (gcode->has_letter('Z')) trim_mm[2] = gcode->get_value('Z'); |
| 892 | |
| 893 | // print the current trim values in mm |
| 894 | gcode->stream->printf("X: %5.3f Y: %5.3f Z: %5.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); |
| 895 | |
| 896 | } |
| 897 | break; |
| 898 | |
| 899 | // NOTE this is to test accuracy of lead screws etc. |
| 900 | case 1910: { |
| 901 | // M1910.0 - move specific number of raw steps |
| 902 | // M1910.1 - stop any moves |
| 903 | // M1910.2 - move specific number of actuator coordinates (usually mm but is degrees for a rotary delta) |
| 904 | if(gcode->subcode == 0 || gcode->subcode == 2) { |
| 905 | // Enable the motors |
| 906 | THEKERNEL->stepper->turn_enable_pins_on(); |
| 907 | |
| 908 | int32_t x = 0, y = 0, z = 0, f = 200 * 16; |
| 909 | if (gcode->has_letter('F')) f = gcode->get_value('F'); |
| 910 | |
| 911 | if (gcode->has_letter('X')) { |
| 912 | float v = gcode->get_value('X'); |
| 913 | if(gcode->subcode == 2) x= lroundf(v * STEPS_PER_MM(X_AXIS)); |
| 914 | else x= roundf(v); |
| 915 | STEPPER[X_AXIS]->move(x < 0, abs(x), f); |
| 916 | } |
| 917 | if (gcode->has_letter('Y')) { |
| 918 | float v = gcode->get_value('Y'); |
| 919 | if(gcode->subcode == 2) y= lroundf(v * STEPS_PER_MM(Y_AXIS)); |
| 920 | else y= roundf(v); |
| 921 | STEPPER[Y_AXIS]->move(y < 0, abs(y), f); |
| 922 | } |
| 923 | if (gcode->has_letter('Z')) { |
| 924 | float v = gcode->get_value('Z'); |
| 925 | if(gcode->subcode == 2) z= lroundf(v * STEPS_PER_MM(Z_AXIS)); |
| 926 | else z= roundf(v); |
| 927 | STEPPER[Z_AXIS]->move(z < 0, abs(z), f); |
| 928 | } |
| 929 | gcode->stream->printf("Moving X %ld Y %ld Z %ld steps at F %ld steps/sec\n", x, y, z, f); |
| 930 | |
| 931 | } else if(gcode->subcode == 1) { |
| 932 | // stop any that are moving |
| 933 | for (int i = 0; i < 3; ++i) { |
| 934 | if(STEPPER[i]->is_moving()) STEPPER[i]->move(0, 0); |
| 935 | } |
| 936 | } |
| 937 | break; |
| 938 | } |
| 939 | } |
| 940 | } |
| 941 | } |
| 942 | |
| 943 | // Called periodically to change the speed to match acceleration |
| 944 | void Endstops::acceleration_tick(void) |
| 945 | { |
| 946 | if(this->status >= NOT_HOMING) return; // nothing to do, only do this when moving for homing sequence |
| 947 | |
| 948 | // foreach stepper that is moving |
| 949 | for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { |
| 950 | if( !STEPPER[c]->is_moving() ) continue; |
| 951 | |
| 952 | uint32_t current_rate = STEPPER[c]->get_steps_per_second(); |
| 953 | uint32_t target_rate = floorf(this->feed_rate[c] * STEPS_PER_MM(c)); |
| 954 | float acc = (c == Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration(); |
| 955 | if( current_rate < target_rate ) { |
| 956 | uint32_t rate_increase = floorf((acc / THEKERNEL->acceleration_ticks_per_second) * STEPS_PER_MM(c)); |
| 957 | current_rate = min( target_rate, current_rate + rate_increase ); |
| 958 | } |
| 959 | if( current_rate > target_rate ) { current_rate = target_rate; } |
| 960 | |
| 961 | // steps per second |
| 962 | STEPPER[c]->set_speed(current_rate); |
| 963 | } |
| 964 | |
| 965 | return; |
| 966 | } |
| 967 | |
| 968 | void Endstops::on_get_public_data(void* argument) |
| 969 | { |
| 970 | PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument); |
| 971 | |
| 972 | if(!pdr->starts_with(endstops_checksum)) return; |
| 973 | |
| 974 | if(pdr->second_element_is(trim_checksum)) { |
| 975 | pdr->set_data_ptr(&this->trim_mm); |
| 976 | pdr->set_taken(); |
| 977 | |
| 978 | } else if(pdr->second_element_is(home_offset_checksum)) { |
| 979 | pdr->set_data_ptr(&this->home_offset); |
| 980 | pdr->set_taken(); |
| 981 | |
| 982 | } else if(pdr->second_element_is(saved_position_checksum)) { |
| 983 | pdr->set_data_ptr(&this->saved_position); |
| 984 | pdr->set_taken(); |
| 985 | |
| 986 | } else if(pdr->second_element_is(get_homing_status_checksum)) { |
| 987 | bool *homing= static_cast<bool *>(pdr->get_data_ptr()); |
| 988 | *homing= this->status != NOT_HOMING; |
| 989 | pdr->set_taken(); |
| 990 | } |
| 991 | } |
| 992 | |
| 993 | void Endstops::on_set_public_data(void* argument) |
| 994 | { |
| 995 | PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument); |
| 996 | |
| 997 | if(!pdr->starts_with(endstops_checksum)) return; |
| 998 | |
| 999 | if(pdr->second_element_is(trim_checksum)) { |
| 1000 | float *t = static_cast<float*>(pdr->get_data_ptr()); |
| 1001 | this->trim_mm[0] = t[0]; |
| 1002 | this->trim_mm[1] = t[1]; |
| 1003 | this->trim_mm[2] = t[2]; |
| 1004 | pdr->set_taken(); |
| 1005 | |
| 1006 | } else if(pdr->second_element_is(home_offset_checksum)) { |
| 1007 | float *t = static_cast<float*>(pdr->get_data_ptr()); |
| 1008 | if(!isnan(t[0])) this->home_offset[0] = t[0]; |
| 1009 | if(!isnan(t[1])) this->home_offset[1] = t[1]; |
| 1010 | if(!isnan(t[2])) this->home_offset[2] = t[2]; |
| 1011 | } |
| 1012 | } |