#include "libs/StepperMotor.h"
#include "wait_api.h" // mbed.h lib
#include "Robot.h"
-#include "Stepper.h"
#include "Config.h"
#include "SlowTicker.h"
#include "Planner.h"
#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 gamma_homing_retract_mm_checksum CHECKSUM("gamma_homing_retract_mm")
#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 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 STEPPER THEKERNEL->robot->actuators
+#define STEPPER THEROBOT->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
// Homing States
enum {
MOVING_TO_ENDSTOP_FAST, // homing move
- MOVING_BACK, // homing move
MOVING_TO_ENDSTOP_SLOW, // homing move
+ MOVING_BACK, // homing move
NOT_HOMING,
BACK_OFF_HOME,
MOVE_TO_ORIGIN,
{
this->status = NOT_HOMING;
home_offset[0] = home_offset[1] = home_offset[2] = 0.0F;
+ debounce.fill(0);
}
void Endstops::on_module_loaded()
register_for_event(ON_GET_PUBLIC_DATA);
register_for_event(ON_SET_PUBLIC_DATA);
- THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {acceleration_tick(); });
-
// Settings
- this->on_config_reload(this);
+ this->load_config();
+
+ THEKERNEL->slow_ticker->attach(1000, this, &Endstops::read_endstops);
}
// Get config
-void Endstops::on_config_reload(void *argument)
+void Endstops::load_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->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->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
+ 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->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) {
+ if(order.size() == 3 && !(this->is_delta || this->is_rdelta)) {
int shift = 0;
for(auto c : order) {
uint8_t i = toupper(c) - 'X';
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();
- //s et to true by default for deltas duwe to trim, false on cartesians
+ // 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();
if(this->limit_enable[X_AXIS] || this->limit_enable[Y_AXIS] || this->limit_enable[Z_AXIS]) {
register_for_event(ON_IDLE);
- if(this->is_delta) {
+ 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;
}
}
- // NOTE this may also be true of scara. TBD
- if(this->is_delta) {
+ //
+ 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];
this->slow_rates[1] = this->slow_rates[2] = this->slow_rates[0];
this->retract_mm[1] = this->retract_mm[2] = this->retract_mm[0];
this->home_direction[1] = this->home_direction[2] = this->home_direction[0];
- this->homing_position[0] = this->homing_position[1] = 0;
+ // 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;
}
}
// 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(char axes_to_move)
+void Endstops::back_off_home(std::bitset<3> axis)
{
std::vector<std::pair<char, float>> params;
this->status = BACK_OFF_HOME;
// these are handled differently
- if(is_delta || is_scara) {
+ 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)});
} else {
// cartesians, concatenate all the moves we need to do into one gcode
for( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if( ((axes_to_move >> c ) & 1) == 0) continue; // only for axes we asked to move
+ if(!axis[c]) 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)) ) {
char gcode_buf[64];
append_parameters(gcode_buf, params, sizeof(gcode_buf));
Gcode gc(gcode_buf, &(StreamOutput::NullStream));
- bool oldmode = THEKERNEL->robot->absolute_mode;
- THEKERNEL->robot->absolute_mode = false; // needs to be relative mode
- THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
- THEKERNEL->robot->absolute_mode = oldmode; // restore mode
+ THEROBOT->push_state();
+ THEROBOT->absolute_mode = false; // needs to be relative mode
+ THEROBOT->on_gcode_received(&gc); // send to robot directly
// Wait for above to finish
THEKERNEL->conveyor->wait_for_empty_queue();
+ THEROBOT->pop_state();
}
this->status = NOT_HOMING;
}
// 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(char axes_to_move)
+void Endstops::move_to_origin()
{
- if( (axes_to_move & 0x03) != 3 ) return; // ignore if X and Y not homing
+ if(!(axis_to_home[X_AXIS] && axis_to_home[Y_AXIS])) return; // ignore if X and Y not homing
// Do we need to check if we are already at 0,0? probably not as the G0 will not do anything if we are
- // float pos[3]; THEKERNEL->robot->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return;
+ // 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;
char buf[32];
snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", rate); // must use machine coordinates in case G92 or WCS is in effect
+ THEROBOT->push_state();
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
THEKERNEL->conveyor->wait_for_empty_queue();
+ THEROBOT->pop_state();
this->status = NOT_HOMING;
}
-bool Endstops::wait_for_homed(char axes_to_move)
+// Called every millisecond in an ISR
+uint32_t Endstops::read_endstops(uint32_t dummy)
{
- bool running = true;
- unsigned int debounce[3] = {0, 0, 0};
- while (running) {
- running = false;
- THEKERNEL->call_event(ON_IDLE);
+ if(this->status != MOVING_TO_ENDSTOP_SLOW && this->status != MOVING_TO_ENDSTOP_FAST) return 0; // not doing anything we need to monitor for
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return false;
+ 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();
+ }
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- if ( this->pins[c + (this->home_direction[c] ? 0 : 3)].get() ) {
- if ( debounce[c] < debounce_count ) {
- debounce[c]++;
- running = true;
- } else if ( STEPPER[c]->is_moving() ) {
- STEPPER[c]->move(0, 0);
- axes_to_move &= ~(1 << c); // no need to check it again
+ } else {
+ // The endstop was not hit yet
+ debounce[m] = 0;
+ }
+ }
+ }
+
+ } 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
+ STEPPER[X_AXIS]->stop_moving();
+ STEPPER[Y_AXIS]->stop_moving();
+ STEPPER[Z_AXIS]->stop_moving();
}
+
} else {
// The endstop was not hit yet
- running = true;
- debounce[c] = 0;
+ debounce[m] = 0;
}
}
}
}
- return true;
+
+ return 0;
}
-void Endstops::do_homing_cartesian(char axes_to_move)
+void Endstops::home_xy()
{
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return;
+ 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*2, beta_max*2, 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]);
+ THEROBOT->delta_move(delta, feed_rate, 3);
+
+ // Wait for XY to have homed
+ THECONVEYOR->wait_for_empty_queue();
+
+ } else if(axis_to_home[X_AXIS]) {
+ // now home X only
+ float delta[3] {alpha_max*2, 0, 0};
+ if(this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS];
+ THEROBOT->delta_move(delta, fast_rates[X_AXIS], 3);
+ // wait for X
+ THECONVEYOR->wait_for_empty_queue();
+
+ } else if(axis_to_home[Y_AXIS]) {
+ // now home Y only
+ float delta[3] {0, beta_max*2, 0};
+ if(this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS];
+ THEROBOT->delta_move(delta, fast_rates[Y_AXIS], 3);
+ // wait for Y
+ THECONVEYOR->wait_for_empty_queue();
+ }
+}
+
+void Endstops::home(std::bitset<3> a)
+{
+ // reset debounce counts
+ debounce.fill(0);
+
+ this->axis_to_home= a;
- // this homing works for cartesian and delta printers
// Start moving the axes to the origin
this->status = MOVING_TO_ENDSTOP_FAST;
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c) & 1 ) {
- this->feed_rate[c] = this->fast_rates[c];
- STEPPER[c]->move(this->home_direction[c], 10000000, 0);
- }
+
+ THEROBOT->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled
+
+ if(!home_z_first) home_xy();
+
+ if(axis_to_home[Z_AXIS]) {
+ // now home z
+ float delta[3] {0, 0, gamma_max*2}; // we go twice the maxz just in case it was set incorrectly
+ if(this->home_direction[Z_AXIS]) delta[Z_AXIS]= -delta[Z_AXIS];
+ THEROBOT->delta_move(delta, fast_rates[Z_AXIS], 3);
+ // wait for Z
+ THECONVEYOR->wait_for_empty_queue();
}
- // Wait for all axes to have homed
- if(!this->wait_for_homed(axes_to_move)) return;
+ if(home_z_first) home_xy();
- // Move back a small distance
+ // TODO should check that the endstops were hit and it did not stop short for some reason
+
+ // Move back a small distance for all homing axis
this->status = MOVING_BACK;
- bool inverted_dir;
+ 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++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- inverted_dir = !this->home_direction[c];
- this->feed_rate[c] = this->slow_rates[c];
- STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c), 0);
+ 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);
}
}
- // Wait for moves to be done
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- while ( STEPPER[c]->is_moving() ) {
- THEKERNEL->call_event(ON_IDLE);
- if(THEKERNEL->is_halted()) return;
- }
- }
- }
+ THEROBOT->delta_move(delta, feed_rate, 3);
+ // wait until finished
+ THECONVEYOR->wait_for_empty_queue();
// Start moving the axes to the origin slowly
this->status = MOVING_TO_ENDSTOP_SLOW;
for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- this->feed_rate[c] = this->slow_rates[c];
- STEPPER[c]->move(this->home_direction[c], 10000000, 0);
+ if(axis_to_home[c]) {
+ delta[c]= this->retract_mm[c];
+ if(this->home_direction[c]) delta[c]= -delta[c];
+ }else{
+ delta[c]= 0;
}
}
+ THEROBOT->delta_move(delta, feed_rate, 3);
+ // wait until finished
+ THECONVEYOR->wait_for_empty_queue();
- // Wait for all axes to have homed
- if(!this->wait_for_homed(axes_to_move)) return;
-}
+ THEROBOT->disable_segmentation= false; // we must disable segmentation as this won't work with it enabled
-bool Endstops::wait_for_homed_corexy(int axis)
-{
- bool running = true;
- unsigned int debounce[3] = {0, 0, 0};
- while (running) {
- running = false;
- THEKERNEL->call_event(ON_IDLE);
-
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return false;
-
- if ( this->pins[axis + (this->home_direction[axis] ? 0 : 3)].get() ) {
- if ( debounce[axis] < debounce_count ) {
- debounce[axis] ++;
- running = true;
- } else {
- // turn both off if running
- if (STEPPER[X_AXIS]->is_moving()) STEPPER[X_AXIS]->move(0, 0);
- if (STEPPER[Y_AXIS]->is_moving()) STEPPER[Y_AXIS]->move(0, 0);
- }
- } else {
- // The endstop was not hit yet
- running = true;
- debounce[axis] = 0;
- }
- }
- return true;
+ this->status = NOT_HOMING;
}
-void Endstops::corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps)
+void Endstops::process_home_command(Gcode* gcode)
{
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return;
-
- this->status = MOVING_TO_ENDSTOP_FAST;
- this->feed_rate[X_AXIS] = fast_rate;
- STEPPER[X_AXIS]->move(dirx, 10000000, 0);
- this->feed_rate[Y_AXIS] = fast_rate;
- STEPPER[Y_AXIS]->move(diry, 10000000, 0);
-
- // wait for primary axis
- if(!this->wait_for_homed_corexy(home_axis)) return;
-
- // Move back a small distance
- this->status = MOVING_BACK;
- this->feed_rate[X_AXIS] = slow_rate;
- STEPPER[X_AXIS]->move(!dirx, retract_steps, 0);
- this->feed_rate[Y_AXIS] = slow_rate;
- STEPPER[Y_AXIS]->move(!diry, retract_steps, 0);
-
- // wait until done
- while ( STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving()) {
- THEKERNEL->call_event(ON_IDLE);
- if(THEKERNEL->is_halted()) return;
- }
-
- // Start moving the axes to the origin slowly
- this->status = MOVING_TO_ENDSTOP_SLOW;
- this->feed_rate[X_AXIS] = slow_rate;
- STEPPER[X_AXIS]->move(dirx, 10000000, 0);
- this->feed_rate[Y_AXIS] = slow_rate;
- STEPPER[Y_AXIS]->move(diry, 10000000, 0);
+ 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
+ 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
+ return;
- // wait for primary axis
- if(!this->wait_for_homed_corexy(home_axis)) 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
-// this homing works for HBots/CoreXY
-void Endstops::do_homing_corexy(char axes_to_move)
-{
- // TODO should really make order configurable, and select whether to allow XY to home at the same time, diagonally
- // 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
- // allow to move until an endstop triggers, then stop that motor. Speed up when moving diagonally to match X or Y speed
- // continue moving in the direction not yet triggered (which means two motors turning) until endstop hit
-
- if((axes_to_move & 0x03) == 0x03) { // both X and Y need Homing
- // determine which motor to turn and which way
- bool dirx = this->home_direction[X_AXIS];
- bool diry = this->home_direction[Y_AXIS];
- int motor;
- bool dir;
- if(dirx && diry) { // min/min
- motor = X_AXIS;
- dir = true;
- } else if(dirx && !diry) { // min/max
- motor = Y_AXIS;
- dir = true;
- } else if(!dirx && diry) { // max/min
- motor = Y_AXIS;
- dir = false;
- } else if(!dirx && !diry) { // max/max
- motor = X_AXIS;
- dir = false;
- }
+ } else if(gcode->subcode == 1) { // G28.1 set pre defined position
+ // saves current position in absolute machine coordinates
+ THEROBOT->get_axis_position(saved_position);
+ return;
- // then move both X and Y until one hits the endstop
- this->status = MOVING_TO_ENDSTOP_FAST;
- // need to allow for more ground covered when moving diagonally
- this->feed_rate[motor] = this->fast_rates[motor] * 1.4142;
- STEPPER[motor]->move(dir, 10000000, 0);
- // wait until either X or Y hits the endstop
- bool running = true;
- while (running) {
- THEKERNEL->call_event(ON_IDLE);
- if(THEKERNEL->is_halted()) return;
- for(int m = X_AXIS; m <= Y_AXIS; m++) {
- if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) {
- // turn off motor
- if(STEPPER[motor]->is_moving()) STEPPER[motor]->move(0, 0);
- running = false;
- break;
- }
- }
+ } 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;
- // move individual axis
- if (axes_to_move & 0x01) { // Home X, which means both X and Y in same direction
- bool dir = this->home_direction[X_AXIS];
- 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));
- }
+ } 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, NOTE does not support the multi actuator hack
+ ActuatorCoordinates ac;
+ if(gcode->has_letter('A')) ac[0] = gcode->get_value('A');
+ if(gcode->has_letter('B')) ac[1] = gcode->get_value('B');
+ if(gcode->has_letter('C')) ac[2] = gcode->get_value('C');
+ THEROBOT->reset_actuator_position(ac);
+ return;
- if (axes_to_move & 0x02) { // Home Y, which means both X and Y in different directions
- bool dir = this->home_direction[Y_AXIS];
- 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));
+ } else if(THEKERNEL->is_grbl_mode()) {
+ gcode->stream->printf("error:Unsupported command\n");
+ return;
}
- if (axes_to_move & 0x04) { // move Z
- do_homing_cartesian(0x04); // just home normally for Z
- }
-}
+ // G28 is received, we have homing to do
-void Endstops::home(char axes_to_move)
-{
- // not a block move so disable the last tick setting
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- STEPPER[c]->set_moved_last_block(false);
- }
+ // First wait for the queue to be empty
+ THEKERNEL->conveyor->wait_for_empty_queue();
- if (is_corexy) {
- // corexy/HBot homing
- do_homing_corexy(axes_to_move);
- } else {
- // cartesian/delta homing
- do_homing_cartesian(axes_to_move);
- }
+ // deltas, scaras always home Z axis only
+ bool home_in_z = this->is_delta || this->is_rdelta || this->is_scara;
- // make sure all steppers are off (especially if aborted)
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- STEPPER[c]->move(0, 0);
- }
- this->status = NOT_HOMING;
-}
+ // figure out which axis to home
+ bitset<3> haxis;
+ haxis.reset();
-// Start homing sequences by response to GCode commands
-void Endstops::on_gcode_received(void *argument)
-{
- Gcode *gcode = static_cast<Gcode *>(argument);
- if ( gcode->has_g && gcode->g == 28) {
- 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
- 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
- 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
- THEKERNEL->robot->get_axis_position(saved_position);
- return;
-
- } else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing
- if(gcode->get_num_args() == 0) {
- THEKERNEL->robot->reset_axis_position(0, 0, 0);
- } else {
- // do a manual homing based on current position, no endstops required
- if(gcode->has_letter('X')) THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
- if(gcode->has_letter('Y')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
- if(gcode->has_letter('Z')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+ 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);
}
- 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
- THEKERNEL->conveyor->wait_for_empty_queue();
-
- // Do we move select axes or all of them
- char axes_to_move = 0;
- // only enable homing if the endstop is defined, deltas, scaras always home all axis
- bool home_all = this->is_delta || this->is_scara || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+ } else {
+ // Only Z axis homes (even though all actuators move this is handled by arm solution)
+ haxis.set(Z_AXIS);
+ }
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( (home_all || gcode->has_letter(c + 'X')) && this->pins[c + (this->home_direction[c] ? 0 : 3)].connected() ) {
- axes_to_move += ( 1 << c );
+ // save current actuator position so we can report how far we moved
+ ActuatorCoordinates start_pos{
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position()
+ };
+
+ // do the actual homing
+ if(homing_order != 0) {
+ // 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;
+ bs.set(a);
+ home(bs);
}
+ // check if on_halt (eg kill)
+ if(THEKERNEL->is_halted()) break;
}
- // Enable the motors
- THEKERNEL->stepper->turn_enable_pins_on();
-
- // do the actual homing
- if(homing_order != 0) {
- // 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 = (1 << (m & 0x03)); // axis to move
- if((a & axes_to_move) != 0) {
- home(a);
- }
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) break;
+ } 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);
+ home(bs);
}
-
- } else {
- // they all home at the same time
- home(axes_to_move);
}
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) {
+ } else {
+ // they could all home at the same time
+ home(haxis);
+ }
+
+ // check if on_halt (eg kill)
+ if(THEKERNEL->is_halted()) {
+ if(!THEKERNEL->is_grbl_mode()) {
THEKERNEL->streams->printf("Homing cycle aborted by kill\n");
- return;
}
+ return;
+ }
- if(home_all) {
- // Here's where we would have been if the endstops were perfectly trimmed
- 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]
+ // set the last probe position to the actuator units moved during this home
+ THEROBOT->set_last_probe_position(
+ std::make_tuple(
+ start_pos[0] - THEROBOT->actuators[0]->get_current_position(),
+ start_pos[1] - THEROBOT->actuators[1]->get_current_position(),
+ start_pos[2] - THEROBOT->actuators[2]->get_current_position(),
+ 0));
+
+ if(home_in_z) { // deltas 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
+ 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]
+ };
+
+ bool has_endstop_trim = this->is_delta || this->is_scara;
+ if (has_endstop_trim) {
+ ActuatorCoordinates ideal_actuator_position;
+ THEROBOT->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
+
+ // We are actually not at the ideal position, but a trim away
+ ActuatorCoordinates real_actuator_position = {
+ ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS],
+ ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS],
+ ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS]
};
- bool has_endstop_trim = this->is_delta || this->is_scara;
- if (has_endstop_trim) {
- ActuatorCoordinates ideal_actuator_position;
- THEKERNEL->robot->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
+ float real_position[3];
+ THEROBOT->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
+ // Reset the actuator positions to correspond our real position
+ THEROBOT->reset_axis_position(real_position[0], real_position[1], real_position[2]);
- // We are actually not at the ideal position, but a trim away
- ActuatorCoordinates real_actuator_position = {
- ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS],
- ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS],
- ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS]
- };
+ } else {
+ // without endstop trim, real_position == ideal_position
+ if(is_rdelta) {
+ // with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates
+ ActuatorCoordinates real_actuator_position = {ideal_position[0], ideal_position[1], ideal_position[2]};
+ THEROBOT->reset_actuator_position(real_actuator_position);
- float real_position[3];
- THEKERNEL->robot->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
- // Reset the actuator positions to correspond our real position
- THEKERNEL->robot->reset_axis_position(real_position[0], real_position[1], real_position[2]);
} else {
- // without endstop trim, real_position == ideal_position
// Reset the actuator positions to correspond our real position
- THEKERNEL->robot->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
- }
- } else {
- // Zero the ax(i/e)s position, add in the home offset
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( (axes_to_move >> c) & 1 ) {
- THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
- }
+ THEROBOT->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
}
}
- // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing
- // default is off for cartesian on for deltas
- if(!is_delta) {
- if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
- // if limit switches are enabled we must back off endstop after setting home
- back_off_home(axes_to_move);
-
- } else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) {
- // 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(axes_to_move);
- if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+ } else {
+ // Zero the ax(i/e)s position, add in the home offset
+ for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+ if (axis_to_home[c]) {
+ THEROBOT->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
+ }
}
}
- if (gcode->has_m) {
+ // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing
+ // default is off for cartesian on for deltas
+ if(!is_delta) {
+ // NOTE a rotary delta usually has optical or hall-effect endstops so it is safe to go past them a little bit
+ if(this->move_to_origin_after_home) move_to_origin();
+ // 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]) {
+ // 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);
+ if(this->move_to_origin_after_home) move_to_origin();
+ }
+}
+
+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
+ 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 (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 (gcode->has_letter('Z')) {
+ home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
+ THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+ }
+
+ gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
+}
+
+// Start homing sequences by response to GCode commands
+void Endstops::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+ if ( gcode->has_g && gcode->g == 28) {
+ process_home_command(gcode);
+
+ } else if (gcode->has_m) {
+
switch (gcode->m) {
case 119: {
for (int i = 0; i < 6; ++i) {
break;
case 206: // M206 - set homing offset
+ if(is_rdelta) return; // RotaryDeltaCalibration module will handle this
+
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]);
-
break;
- case 306: { // Similar to M206 and G92 but sets Homing offsets based on current position
- float cartesian[3];
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
- if (gcode->has_letter('X')) {
- home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
- }
- if (gcode->has_letter('Y')) {
- home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
- }
- if (gcode->has_letter('Z')) {
- home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
- }
-
- gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
+ case 306: // set homing offset based on current position
+ if(is_rdelta) return; // RotaryDeltaCalibration module will handle this
- }
- break;
+ set_homing_offset(gcode);
+ break;
case 500: // save settings
case 503: // print settings
- 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]);
+ 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 (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]);
}
break;
- case 665: { // M665 - set max gamma/z height
-
- float gamma_max = this->homing_position[2];
- if (gcode->has_letter('Z')) {
- this->homing_position[2] = gamma_max = gcode->get_value('Z');
+ case 665:
+ if (this->is_delta || this->is_scara) { // M665 - set max gamma/z height
+ float gamma_max = this->homing_position[2];
+ if (gcode->has_letter('Z')) {
+ this->homing_position[2] = gamma_max = gcode->get_value('Z');
+ }
+ gcode->stream->printf("Max Z %8.3f ", gamma_max);
+ gcode->add_nl = true;
}
- gcode->stream->printf("Max Z %8.3f ", gamma_max);
- gcode->add_nl = true;
- }
- break;
-
+ break;
case 666:
if(this->is_delta || this->is_scara) { // M666 - set trim for each axis in mm, NB negative mm trim is down
}
break;
- // NOTE this is to test accuracy of lead screws etc.
- case 1910: { // M1910 - move specific number of raw steps
- if(gcode->subcode == 0) {
- // Enable the motors
- THEKERNEL->stepper->turn_enable_pins_on();
-
- int x = 0, y = 0 , z = 0, f = 200 * 16;
- if (gcode->has_letter('F')) f = gcode->get_value('F');
- if (gcode->has_letter('X')) {
- x = gcode->get_value('X');
- STEPPER[X_AXIS]->move(x < 0, abs(x), f);
- }
- if (gcode->has_letter('Y')) {
- y = gcode->get_value('Y');
- STEPPER[Y_AXIS]->move(y < 0, abs(y), f);
- }
- if (gcode->has_letter('Z')) {
- z = gcode->get_value('Z');
- STEPPER[Z_AXIS]->move(z < 0, abs(z), f);
- }
- gcode->stream->printf("Moving X %d Y %d Z %d steps at F %d steps/sec\n", x, y, z, f);
-
- } else if(gcode->subcode == 1) {
- // stop any that are moving
- for (int i = 0; i < 3; ++i) {
- if(STEPPER[i]->is_moving()) STEPPER[i]->move(0, 0);
- }
- }
- break;
- }
- }
- }
-}
-
-// Called periodically to change the speed to match acceleration
-void Endstops::acceleration_tick(void)
-{
- if(this->status >= NOT_HOMING) return; // nothing to do, only do this when moving for homing sequence
-
- // foreach stepper that is moving
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if( !STEPPER[c]->is_moving() ) continue;
-
- uint32_t current_rate = STEPPER[c]->get_steps_per_second();
- uint32_t target_rate = floorf(this->feed_rate[c] * STEPS_PER_MM(c));
- float acc = (c == Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration();
- if( current_rate < target_rate ) {
- uint32_t rate_increase = floorf((acc / THEKERNEL->acceleration_ticks_per_second) * STEPS_PER_MM(c));
- current_rate = min( target_rate, current_rate + rate_increase );
}
- if( current_rate > target_rate ) { current_rate = target_rate; }
-
- // steps per second
- STEPPER[c]->set_speed(current_rate);
}
-
- return;
}
void Endstops::on_get_public_data(void* argument)
} else if(pdr->second_element_is(saved_position_checksum)) {
pdr->set_data_ptr(&this->saved_position);
pdr->set_taken();
+
+ } else if(pdr->second_element_is(get_homing_status_checksum)) {
+ bool *homing = static_cast<bool *>(pdr->get_data_ptr());
+ *homing = this->status != NOT_HOMING;
+ pdr->set_taken();
}
}
HCoop / clinton / Smoothieware.git / blobdiff