// Homing States
-enum{
+enum {
MOVING_TO_ENDSTOP_FAST, // homing move
MOVING_BACK, // homing move
MOVING_TO_ENDSTOP_SLOW, // homing move
register_for_event(ON_GET_PUBLIC_DATA);
register_for_event(ON_SET_PUBLIC_DATA);
- THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){acceleration_tick(); });
+ THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {acceleration_tick(); });
// Settings
- this->on_config_reload(this);
+ this->load_config();
}
// 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->is_scara = THEKERNEL->config->value(scara_homing_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;
+ string order = THEKERNEL->config->value(homing_order_checksum)->by_default("")->as_string();
+ this->homing_order = 0;
if(order.size() == 3 && !this->is_delta) {
- int shift= 0;
+ int shift = 0;
for(auto c : order) {
- uint8_t i= toupper(c) - 'X';
+ uint8_t i = toupper(c) - 'X';
if(i > 2) { // bad value
- this->homing_order= 0;
+ this->homing_order = 0;
break;
}
homing_order |= (i << shift);
this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum )->by_default(0 )->as_number();
// 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();
+ 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();
- //s et to true by default for deltas duwe to trim, false on cartesians
- this->move_to_origin_after_home= THEKERNEL->config->value(move_to_origin_checksum)->by_default(is_delta)->as_bool();
+ // set to true by default for deltas duwe 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]){
+ 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) {
// 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;
+ this->limit_enable[X_AXIS] = true;
+ this->limit_enable[Y_AXIS] = true;
+ this->limit_enable[Z_AXIS] = true;
}
}
//
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;
+ 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];
+ // 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;
}
}
bool Endstops::debounced_get(int pin)
{
- uint8_t debounce= 0;
+ uint8_t debounce = 0;
while(this->pins[pin].get()) {
if ( ++debounce >= this->debounce_count ) {
// pin triggered
return false;
}
-static const char *endstop_names[]= {"min_x", "min_y", "min_z", "max_x", "max_y", "max_z"};
+static const char *endstop_names[] = {"min_x", "min_y", "min_z", "max_x", "max_y", "max_z"};
void Endstops::on_idle(void *argument)
{
std::array<int, 2> minmax{{0, 3}};
// check min and max endstops
for (int i : minmax) {
- int n= c+i;
+ int n = c + i;
if(this->pins[n].get()) {
// still triggered, so exit
- bounce_cnt= 0;
+ bounce_cnt = 0;
return;
}
}
}
if(++bounce_cnt > 10) { // can use less as it calls on_idle in between
// clear the state
- this->status= NOT_HOMING;
+ this->status = NOT_HOMING;
}
return;
- }else if(this->status != NOT_HOMING) {
+ } else if(this->status != NOT_HOMING) {
// don't check while homing
return;
}
std::array<int, 2> minmax{{0, 3}};
// check min and max endstops
for (int i : minmax) {
- int n= c+i;
+ 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;
+ this->status = LIMIT_TRIGGERED;
// disables heaters and motors, ignores incoming Gcode and flushes block queue
THEKERNEL->call_event(ON_HALT, nullptr);
return;
// checks if triggered and only backs off if triggered
void Endstops::back_off_home(char axes_to_move)
{
- std::vector<std::pair<char,float>> params;
+ std::vector<std::pair<char, float>> params;
this->status = BACK_OFF_HOME;
// 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', this->retract_mm[Z_AXIS] * (this->home_direction[Z_AXIS] ? 1 : -1)});
- }else{
+ } 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 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)});
+ params.push_back({c + 'X', this->retract_mm[c] * (this->home_direction[c] ? 1 : -1)});
}
}
}
// 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', this->slow_rates[X_AXIS] * 60.0F});
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->push_state();
+ 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
// Wait for above to finish
THEKERNEL->conveyor->wait_for_empty_queue();
+ THEKERNEL->robot->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)
{
- if( (axes_to_move&0x03) != 3 ) return; // ignore if X and Y not homing
+ if( (axes_to_move & 0x03) != 3 ) 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;
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;
+ 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
+ THEKERNEL->robot->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();
+ THEKERNEL->robot->pop_state();
this->status = NOT_HOMING;
}
running = true;
} else if ( STEPPER[c]->is_moving() ) {
STEPPER[c]->move(0, 0);
- axes_to_move &= ~(1<<c); // no need to check it again
+ axes_to_move &= ~(1 << c); // no need to check it again
}
} else {
// The endstop was not hit yet
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];
+ this->feed_rate[c] = this->fast_rates[c];
STEPPER[c]->move(this->home_direction[c], 10000000, 0);
}
}
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];
+ this->feed_rate[c] = this->slow_rates[c];
STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c), 0);
}
}
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];
+ this->feed_rate[c] = this->slow_rates[c];
STEPPER[c]->move(this->home_direction[c], 10000000, 0);
}
}
if(THEKERNEL->is_halted()) return;
this->status = MOVING_TO_ENDSTOP_FAST;
- this->feed_rate[X_AXIS]= fast_rate;
+ this->feed_rate[X_AXIS] = fast_rate;
STEPPER[X_AXIS]->move(dirx, 10000000, 0);
- this->feed_rate[Y_AXIS]= fast_rate;
+ this->feed_rate[Y_AXIS] = fast_rate;
STEPPER[Y_AXIS]->move(diry, 10000000, 0);
// wait for primary axis
// Move back a small distance
this->status = MOVING_BACK;
- this->feed_rate[X_AXIS]= slow_rate;
+ this->feed_rate[X_AXIS] = slow_rate;
STEPPER[X_AXIS]->move(!dirx, retract_steps, 0);
- this->feed_rate[Y_AXIS]= slow_rate;
+ this->feed_rate[Y_AXIS] = slow_rate;
STEPPER[Y_AXIS]->move(!diry, retract_steps, 0);
// wait until done
// Start moving the axes to the origin slowly
this->status = MOVING_TO_ENDSTOP_SLOW;
- this->feed_rate[X_AXIS]= slow_rate;
+ this->feed_rate[X_AXIS] = slow_rate;
STEPPER[X_AXIS]->move(dirx, 10000000, 0);
- this->feed_rate[Y_AXIS]= slow_rate;
+ this->feed_rate[Y_AXIS] = slow_rate;
STEPPER[Y_AXIS]->move(diry, 10000000, 0);
// wait for primary axis
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];
+ 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;
+ 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;
}
// 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;
+ // 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;
+ bool running = true;
while (running) {
THEKERNEL->call_event(ON_IDLE);
if(THEKERNEL->is_halted()) return;
- for(int m=X_AXIS;m<=Y_AXIS;m++) {
+ 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;
+ running = false;
break;
}
}
// 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];
+ 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));
}
if (axes_to_move & 0x02) { // Home Y, which means both X and Y in different directions
- bool dir= this->home_direction[Y_AXIS];
+ 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));
}
STEPPER[c]->set_moved_last_block(false);
}
- if (is_corexy){
+ if (is_corexy) {
// corexy/HBot homing
do_homing_corexy(axes_to_move);
- }else{
+ } else {
// cartesian/delta homing
do_homing_cartesian(axes_to_move);
}
this->status = NOT_HOMING;
}
-// Start homing sequences by response to GCode commands
-void Endstops::on_gcode_received(void *argument)
+void Endstops::process_home_command(Gcode* gcode)
{
- Gcode *gcode = static_cast<Gcode *>(argument);
- if ( gcode->has_g) {
- if ( gcode->g == 28 ) {
- if(gcode->subcode == 1) { // G28.1
- 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);
- }
- return;
- }
+ 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;
- // G28 is received, we have homing to do
+ } 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
- // First wait for the queue to be empty
- THEKERNEL->conveyor->wait_for_empty_queue();
+ } 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;
- // 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_rdelta || this->is_scara || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+ } 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 given coordinates, 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);
+ }
+ return;
- 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 );
- }
- }
+ } 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');
+ THEKERNEL->robot->reset_actuator_position(ac);
+ return;
- // 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(THEKERNEL->is_grbl_mode()) {
+ gcode->stream->printf("error:Unsupported command\n");
+ return;
+ }
- }else {
- // they all home at the same time
- home(axes_to_move);
- }
+ // G28 is received, we have homing to do
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()){
- THEKERNEL->streams->printf("Homing cycle aborted by kill\n");
- return;
+ // 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_rdelta || this->is_scara || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+
+ 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{
+ THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+ };
+
+ // 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 {
+ // they all home at the same time
+ home(axes_to_move);
+ }
+
+ // 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;
+ }
+
+ // set the last probe position to the actuator units moved during this home
+ THEKERNEL->robot->set_last_probe_position(
+ std::make_tuple(
+ start_pos[0] - THEKERNEL->robot->actuators[0]->get_current_position(),
+ start_pos[1] - THEKERNEL->robot->actuators[1]->get_current_position(),
+ start_pos[2] - THEKERNEL->robot->actuators[2]->get_current_position(),
+ 0));
+
+ if(home_all) {
+ // 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;
+ THEKERNEL->robot->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]
+ };
+
+ 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
+ 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]};
+ THEKERNEL->robot->reset_actuator_position(real_actuator_position);
- 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]
- };
-
- 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);
-
- // 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]
- };
-
- 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);
- }
- }
+ // Reset the actuator positions to correspond our real position
+ THEKERNEL->robot->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 ( (axes_to_move >> c) & 1 ) {
+ THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
}
}
+ }
+
+ // 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(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);
+ }
+}
+
+void Endstops::set_homing_offset(Gcode *gcode)
+{
+ // 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]);
+}
+
+// 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) {
if(this->pins[i].connected())
gcode->stream->printf("%s:%d ", endstop_names[i], this->pins[i].get());
}
- gcode->add_nl= true;
+ gcode->add_nl = true;
}
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);
- }
+ case 306: // set homing offset based on current position
+ if(is_rdelta) return; // RotaryDeltaCalibration module will handle this
- gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
-
- }
+ 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) {
- if(!this->is_rdelta) 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(";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]);
}
+ if(saved_position[X_AXIS] != 0 || saved_position[Y_AXIS] != 0) {
+ 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]);
+ }
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
gcode->stream->printf("X: %5.3f Y: %5.3f Z: %5.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]);
}
- break;
+ break;
// NOTE this is to test accuracy of lead screws etc.
- case 1910: { // M1910 - move specific number of raw steps
- if(gcode->subcode == 0) {
+ case 1910: {
+ // M1910.0 - move specific number of raw steps
+ // M1910.1 - stop any moves
+ // M1910.2 - move specific number of actuator units (usually mm but is degrees for a rotary delta)
+ if(gcode->subcode == 0 || gcode->subcode == 2) {
// Enable the motors
THEKERNEL->stepper->turn_enable_pins_on();
- int x= 0, y=0 , z= 0, f= 200*16;
+ int32_t 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);
+ float v = gcode->get_value('X');
+ if(gcode->subcode == 2) x = lroundf(v * STEPS_PER_MM(X_AXIS));
+ else x = roundf(v);
+ 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);
+ float v = gcode->get_value('Y');
+ if(gcode->subcode == 2) y = lroundf(v * STEPS_PER_MM(Y_AXIS));
+ else y = roundf(v);
+ 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);
+ float v = gcode->get_value('Z');
+ if(gcode->subcode == 2) z = lroundf(v * STEPS_PER_MM(Z_AXIS));
+ else z = roundf(v);
+ 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);
+ gcode->stream->printf("Moving X %ld Y %ld Z %ld steps at F %ld steps/sec\n", x, y, z, f);
- }else if(gcode->subcode == 1) {
+ } 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);
- }
+ if(STEPPER[i]->is_moving()) STEPPER[i]->move(0, 0);
+ }
}
break;
}
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));
+ 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; }
+ 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){
+void Endstops::on_get_public_data(void* argument)
+{
PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
if(!pdr->starts_with(endstops_checksum)) return;
pdr->set_data_ptr(&this->trim_mm);
pdr->set_taken();
- }else if(pdr->second_element_is(home_offset_checksum)) {
+ } else if(pdr->second_element_is(home_offset_checksum)) {
pdr->set_data_ptr(&this->home_offset);
pdr->set_taken();
+
+ } 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();
}
}
-void Endstops::on_set_public_data(void* argument){
+void Endstops::on_set_public_data(void* argument)
+{
PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
if(!pdr->starts_with(endstops_checksum)) return;
if(pdr->second_element_is(trim_checksum)) {
- float *t= static_cast<float*>(pdr->get_data_ptr());
- this->trim_mm[0]= t[0];
- this->trim_mm[1]= t[1];
- this->trim_mm[2]= t[2];
+ float *t = static_cast<float*>(pdr->get_data_ptr());
+ this->trim_mm[0] = t[0];
+ this->trim_mm[1] = t[1];
+ this->trim_mm[2] = t[2];
pdr->set_taken();
- }else if(pdr->second_element_is(home_offset_checksum)) {
- float *t= static_cast<float*>(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];
+ } else if(pdr->second_element_is(home_offset_checksum)) {
+ float *t = static_cast<float*>(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];
}
}
HCoop / clinton / Smoothieware.git / blobdiff