bool Endstops::load_config()
{
bool limit_enabled= false;
+ size_t max_index= 0;
std::array<homing_info_t, k_max_actuators> temp_axis_array; // needs to be at least XYZ, but allow for ABC
{
continue;
}
+ // keep track of the maximum index that has been defined
+ if(i > max_index) max_index= i;
+
// init pin struct
pin_info->debounce= 0;
pin_info->axis= toupper(axis[0]);
// if no pins defined then disable the module
if(endstops.empty()) return false;
- // copy to the homing_axis array
+ // copy to the homing_axis array, make sure that undefined entries are filled in as well
+ // as the order is important and all slots must be filled upto the max_index
for (size_t i = 0; i < temp_axis_array.size(); ++i) {
if(temp_axis_array[i].axis == 0) {
// was not configured above, if it is XYZ then we need to force a dummy entry
t.axis_index= i;
t.pin_info= nullptr; // this tells it that it cannot be used for homing
homing_axis.push_back(t);
+
+ }else if(i <= max_index) {
+ // for instance case where we defined C without A or B
+ homing_info_t t;
+ t.axis= 'A' + i;
+ t.axis_index= i;
+ t.pin_info= nullptr; // this tells it that it cannot be used for homing
+ homing_axis.push_back(t);
}
}else{
this->trim_mm[1] = THEKERNEL->config->value(beta_trim_checksum)->by_default(0)->as_number();
this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum)->by_default(0)->as_number();
- // see if an order has been specified, must be three characters, XYZ or YXZ etc
+ // see if an order has been specified, must be three or more characters, XYZABC or ABYXZ etc
string order = THEKERNEL->config->value(homing_order_checksum)->by_default("")->as_string();
this->homing_order = 0;
- if(order.size() == 3 && !(this->is_delta || this->is_rdelta)) {
+ if(order.size() >= 3 && order.size() <= homing_axis.size() && !(this->is_delta || this->is_rdelta)) {
int shift = 0;
for(auto c : order) {
- uint8_t i = toupper(c) - 'X';
- if(i > 2) { // bad value
+ char n= toupper(c);
+ uint32_t i = n >= 'X' ? n - 'X' : n - 'A' + 3;
+ i += 1; // So X is 1
+ if(i > 6) { // bad value
this->homing_order = 0;
break;
}
homing_order |= (i << shift);
- shift += 2;
+ shift += 3;
}
}
// check min and max endstops
if(debounced_get(&i->pin)) {
// endstop triggered
- THEKERNEL->streams->printf("Limit switch %c was hit - reset or M999 required\n", i->axis);
+ if(!THEKERNEL->is_grbl_mode()) {
+ THEKERNEL->streams->printf("Limit switch %c%c was hit - reset or M999 required\n", STEPPER[i->axis_index]->which_direction() ? '-' : '+', i->axis);
+ }else{
+ THEKERNEL->streams->printf("ALARM: Hard limit %c%c\n", STEPPER[i->axis_index]->which_direction() ? '-' : '+', i->axis);
+ }
this->status = LIMIT_TRIGGERED;
i->debounce= 0;
// disables heaters and motors, ignores incoming Gcode and flushes block queue
// these are handled differently
if(is_delta) {
// Move off of the endstop using a regular relative move in Z only
- params.push_back({'Z', homing_axis[Z_AXIS].retract * (homing_axis[Z_AXIS].home_direction ? 1 : -1)});
+ params.push_back({'Z', THEROBOT->from_millimeters(homing_axis[Z_AXIS].retract * (homing_axis[Z_AXIS].home_direction ? 1 : -1))});
slow_rate= homing_axis[Z_AXIS].slow_rate;
} else {
- // cartesians, concatenate all the moves we need to do into one gcode
+ // cartesians concatenate all the moves we need to do into one gcode
for( auto& e : homing_axis) {
if(!axis[e.axis_index]) continue; // only for axes we asked to move
// if not triggered no need to move off
if(e.pin_info != nullptr && e.pin_info->limit_enable && debounced_get(&e.pin_info->pin)) {
char ax= e.axis;
- params.push_back({ax, e.retract * (e.home_direction ? 1 : -1)});
+ params.push_back({ax, THEROBOT->from_millimeters(e.retract * (e.home_direction ? 1 : -1))});
// select slowest of them all
slow_rate= isnan(slow_rate) ? e.slow_rate : std::min(slow_rate, e.slow_rate);
}
// 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', slow_rate * 60.0F});
+ params.push_back({'F', THEROBOT->from_millimeters(slow_rate * 60.0F)});
char gcode_buf[64];
append_parameters(gcode_buf, params, sizeof(gcode_buf));
Gcode gc(gcode_buf, &(StreamOutput::NullStream));
THEROBOT->push_state();
- THEROBOT->inch_mode = false; // needs to be in mm
THEROBOT->absolute_mode = false; // needs to be relative mode
THEROBOT->on_gcode_received(&gc); // send to robot directly
// Wait for above to finish
// float pos[3]; THEROBOT->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return;
this->status = MOVE_TO_ORIGIN;
- // Move to center using a regular move, use slower of X and Y fast rate
- //float rate = std::min(this->fast_rates[0], this->fast_rates[1]) * 60.0F;
+ // Move to center using a regular move, use slower of X and Y fast rate in mm/sec
+ float rate = std::min(homing_axis[X_AXIS].fast_rate, homing_axis[Y_AXIS].fast_rate) * 60.0F;
char buf[32];
THEROBOT->push_state();
- THEROBOT->inch_mode = false; // needs to be in mm
THEROBOT->absolute_mode = true;
- //snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", rate); // must use machine coordinates in case G92 or WCS is in effect
- snprintf(buf, sizeof(buf), "G53 G0 X0 Y0"); // must use machine coordinates in case G92 or WCS is in effect
+ snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", THEROBOT->from_millimeters(rate)); // must use machine coordinates in case G92 or WCS is in effect
struct SerialMessage message;
message.message = buf;
message.stream = &(StreamOutput::NullStream);
// we signal the motor to stop, which will preempt any moves on that axis
STEPPER[m]->stop_moving();
}
+ e.pin_info->triggered= true;
}
} else {
// reset debounce counts for all endstops
for(auto& e : endstops) {
e->debounce= 0;
+ e->triggered= false;
+ }
+
+ if (is_scara) {
+ THEROBOT->disable_arm_solution = true; // Polar bots has to home in the actuator space. Arm solution disabled.
}
this->axis_to_home= a;
}
}
+ // check that the endstops were hit and it did not stop short for some reason
+ // if the endstop is not triggered then enter ALARM state
+ // with deltas we check all three axis were triggered, but at least one of XYZ must be set to home
+ if(axis_to_home[X_AXIS] || axis_to_home[Y_AXIS] || axis_to_home[Z_AXIS]) {
+ for (size_t i = X_AXIS; i <= Z_AXIS; ++i) {
+ if((axis_to_home[i] || this->is_delta || this->is_rdelta) && !homing_axis[i].pin_info->triggered) {
+ this->status = NOT_HOMING;
+ THEKERNEL->call_event(ON_HALT, nullptr);
+ return;
+ }
+ }
+ }
+
+ // also check ABC
+ if(homing_axis.size() > 3){
+ for (size_t i = A_AXIS; i < homing_axis.size(); ++i) {
+ if(axis_to_home[i] && !homing_axis[i].pin_info->triggered) {
+ this->status = NOT_HOMING;
+ THEKERNEL->call_event(ON_HALT, nullptr);
+ return;
+ }
+ }
+ }
- // TODO: should check that the endstops were hit and it did not stop short for some reason
- // we did not complete movement the full distance if we hit the endstops
- THEROBOT->reset_position_from_current_actuator_position();
+ if (!is_scara) {
+ // Only for non polar bots
+ // we did not complete movement the full distance if we hit the endstops
+ // TODO Maybe only reset axis involved in the homing cycle
+ THEROBOT->reset_position_from_current_actuator_position();
+ }
// Move back a small distance for all homing axis
this->status = MOVING_BACK;
// wait until finished
THECONVEYOR->wait_for_idle();
- // TODO: should check that the endstops were hit and it did not stop short for some reason
// we did not complete movement the full distance if we hit the endstops
+ // TODO Maybe only reset axis involved in the homing cycle
THEROBOT->reset_position_from_current_actuator_position();
THEROBOT->disable_segmentation= false;
+ if (is_scara) {
+ THEROBOT->disable_arm_solution = false; // Arm solution enabled again.
+ }
this->status = NOT_HOMING;
}
THEROBOT->compensationTransform= nullptr;
// deltas always home Z axis only, which moves all three actuators
- bool home_in_z = this->is_delta || this->is_rdelta;
+ bool home_in_z_only = this->is_delta || this->is_rdelta;
// figure out which axis to home
axis_bitmap_t haxis;
haxis.reset();
- if(!home_in_z) { // ie not a delta
- bool axis_speced = (gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ||
- gcode->has_letter('A') || gcode->has_letter('B') || gcode->has_letter('C'));
+ bool axis_speced = (gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') ||
+ gcode->has_letter('A') || gcode->has_letter('B') || gcode->has_letter('C'));
+ if(!home_in_z_only) { // ie not a delta
for (auto &p : homing_axis) {
// only enable homing if the endstop is defined,
if(p.pin_info == nullptr) continue;
if(!axis_speced || gcode->has_letter(p.axis)) {
haxis.set(p.axis_index);
// now reset axis to 0 as we do not know what state we are in
- THEROBOT->reset_axis_position(0, p.axis_index);
+ if (!is_scara) {
+ THEROBOT->reset_axis_position(0, p.axis_index);
+ } else {
+ // SCARA resets arms to plausable minimum angles
+ THEROBOT->reset_axis_position(-30,30,0); // angles set into axis space for homing.
+ }
}
}
} else {
- // Only Z axis homes (even though all actuators move this is handled by arm solution)
- haxis.set(Z_AXIS);
- // we also set the kinematics to a known good position, this is necessary for a rotary delta, but doesn't hurt for linear delta
- THEROBOT->reset_axis_position(0, 0, 0);
+ bool home_z= !axis_speced || gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z');
+
+ // if we specified an axis we check ABC
+ for (size_t i = A_AXIS; i < homing_axis.size(); ++i) {
+ auto &p= homing_axis[i];
+ if(p.pin_info == nullptr) continue;
+ if(!axis_speced || gcode->has_letter(p.axis)) haxis.set(p.axis_index);
+ }
+
+ if(home_z){
+ // Only Z axis homes (even though all actuators move this is handled by arm solution)
+ haxis.set(Z_AXIS);
+ // we also set the kinematics to a known good position, this is necessary for a rotary delta, but doesn't hurt for linear delta
+ THEROBOT->reset_axis_position(0, 0, 0);
+ }
+ }
+
+ if(haxis.none()) {
+ THEKERNEL->streams->printf("WARNING: Nothing to home\n");
+ return;
}
// do the actual homing
- if(homing_order != 0) {
+ if(homing_order != 0 && !is_scara) {
// if an order has been specified do it in the specified order
- // homing order is 0b00ccbbaa where aa is 0,1,2 to specify the first axis, bb is the second and cc is the third
- // eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z
- for (uint8_t m = homing_order; m != 0; m >>= 2) {
- int a= (m & 0x03); // axis to home
- if(haxis[a]) { // if axis is selected to home
+ // homing order is 0bfffeeedddcccbbbaaa where aaa is 1,2,3,4,5,6 to specify the first axis (XYZABC), bbb is the second and ccc is the third etc
+ // eg 0b0101011001010 would be Y X Z A, 011 010 001 100 101 would be B A X Y Z
+ for (uint32_t m = homing_order; m != 0; m >>= 3) {
+ uint32_t a= (m & 0x07)-1; // axis to home
+ if(a < homing_axis.size() && haxis[a]) { // if axis is selected to home
axis_bitmap_t bs;
bs.set(a);
home(bs);
// restore compensationTransform
THEROBOT->compensationTransform= savect;
- // check if on_halt (eg kill)
+ // check if on_halt (eg kill or fail)
if(THEKERNEL->is_halted()) {
if(!THEKERNEL->is_grbl_mode()) {
- THEKERNEL->streams->printf("Homing cycle aborted by kill\n");
+ THEKERNEL->streams->printf("ERROR: Homing cycle failed - check the max_travel settings\n");
+ }else{
+ THEKERNEL->streams->printf("ALARM: Homing fail\n");
}
// clear all the homed flags
for (auto &p : homing_axis) p.homed= false;
return;
}
- if(home_in_z) { // deltas only
+ if(home_in_z_only || is_scara) { // deltas and scaras only
// Here's where we would have been if the endstops were perfectly trimmed
// NOTE on a rotary delta home_offset is actuator position in degrees when homed and
// home_offset is the theta offset for each actuator, so M206 is used to set theta offset for each actuator in degrees
homing_axis[Z_AXIS].homing_position + homing_axis[Z_AXIS].home_offset
};
- bool has_endstop_trim = this->is_delta;
+ bool has_endstop_trim = this->is_delta || is_scara;
if (has_endstop_trim) {
ActuatorCoordinates ideal_actuator_position;
THEROBOT->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
float real_position[3];
THEROBOT->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
- // Reset the actuator positions to correspond our real position
+ // Reset the actuator positions to correspond to our real position
THEROBOT->reset_axis_position(real_position[0], real_position[1], real_position[2]);
} else {
THEROBOT->reset_actuator_position(real_actuator_position);
} else {
- // Reset the actuator positions to correspond our real position
+ // Reset the actuator positions to correspond to our real position
THEROBOT->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
}
}
- // for deltas we say all axis are homed even though it was only Z
- for (auto &p : homing_axis) p.homed= true;
+ // for deltas we say all 3 axis are homed even though it was only Z
+ homing_axis[X_AXIS].homed= true;
+ homing_axis[Y_AXIS].homed= true;
+ homing_axis[Z_AXIS].homed= true;
+
+ // if we also homed ABC then we need to reset them
+ for (size_t i = A_AXIS; i < homing_axis.size(); ++i) {
+ auto &p= homing_axis[i];
+ if (haxis[p.axis_index]) { // if we requested this axis to home
+ THEROBOT->reset_axis_position(p.homing_position + p.home_offset, p.axis_index);
+ // set flag indicating axis was homed, it stays set once set until H/W reset or unhomed
+ p.homed= true;
+ }
+ }
} else {
// Zero the ax(i/e)s position, add in the home offset
// 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 || homing_axis[X_AXIS].pin_info->limit_enable) {
+ } else if(haxis[Z_AXIS] && (this->move_to_origin_after_home || homing_axis[X_AXIS].pin_info->limit_enable)) {
// deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first
// also need to back off endstops if limits are enabled
back_off_home(haxis);
{
// TODO: spec says if XYZ specified move to them first then move to MCS of specifed axis
THEROBOT->push_state();
- THEROBOT->inch_mode = false; // needs to be in mm
THEROBOT->absolute_mode = true;
char buf[32];
- snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", saved_position[X_AXIS], saved_position[Y_AXIS]); // must use machine coordinates in case G92 or WCS is in effect
+ snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", THEROBOT->from_millimeters(saved_position[X_AXIS]), THEROBOT->from_millimeters(saved_position[Y_AXIS])); // must use machine coordinates in case G92 or WCS is in effect
struct SerialMessage message;
message.message = buf;
message.stream = &(StreamOutput::NullStream);
bool *homing = static_cast<bool *>(pdr->get_data_ptr());
*homing = this->status != NOT_HOMING;
pdr->set_taken();
+
+ } else if(pdr->second_element_is(get_homed_status_checksum)) {
+ bool *homed = static_cast<bool *>(pdr->get_data_ptr());
+ for (int i = 0; i < 3; ++i) {
+ homed[i]= homing_axis[i].homed;
+ }
+ pdr->set_taken();
}
}
HCoop / clinton / Smoothieware.git / blobdiff