static stat_t _homing_axis_start(int8_t axis)
{
// get the first or next axis
if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
return (_set_homing_func(_homing_finalize_exit));
} else if (axis == -2) { // -2 is error
cm_set_units_mode(hm.saved_units_mode);
cm_set_distance_mode(hm.saved_distance_mode);
cm.cycle_state = CYCLE_OFF;
cm_cycle_end();
return (_homing_error_exit(-2));
}
}
// trap gross mis-configurations
if ((fp_ZERO(cm.a[axis].search_velocity)) || (fp_ZERO(cm.a[axis].latch_velocity))) {
return (_homing_error_exit(axis));
}
if ((cm.a[axis].travel_max <= 0) || (cm.a[axis].latch_backoff <= 0)) {
return (_homing_error_exit(axis));
}
// determine the switch setup and that config is OK
hm.min_mode = get_switch_mode(MIN_SWITCH(axis));
hm.max_mode = get_switch_mode(MAX_SWITCH(axis));
if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) {// one or the other must be homing
return (_homing_error_exit(axis)); // axis cannot be homed
}
hm.axis = axis; // persist the axis
hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive
hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive
// setup parameters homing to the minimum switch
if (hm.min_mode & SW_HOMING_BIT) {
hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch
hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch
hm.search_travel = -cm.a[axis].travel_max; // search travels in negative direction
hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction
hm.zero_backoff = cm.a[axis].zero_backoff;
// setup parameters for positive travel (homing to the maximum switch)
} else {
hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch
hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch
hm.search_travel = cm.a[axis].travel_max; // search travels in positive direction
hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction
hm.zero_backoff = -cm.a[axis].zero_backoff;
}
// if homing is disabled for the axis then skip to the next axis
uint8_t sw_mode = get_switch_mode(hm.homing_switch);
if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) {
return (_set_homing_func(_homing_axis_start));
}
// disable the limit switch parameter if there is no limit switch
if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) { hm.limit_switch = -1;}
hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value
return (_set_homing_func(_homing_axis_clear)); // start the clear
}
static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[])
// gms = Gcode model state
{
float inv_time=0; // inverse time if doing a feed in G93 mode
float xyz_time=0; // coordinated move linear part at requested feed rate
float abc_time=0; // coordinated move rotary part at requested feed rate
float max_time=0; // time required for the rate-limiting axis
float tmp_time=0; // used in computation
gms->minimum_time = 8675309; // arbitrarily large number
// compute times for feed motion
if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) {
if (gms->feed_rate_mode == INVERSE_TIME_MODE) {
inv_time = gms->feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_feed_rate()
gms->feed_rate_mode = UNITS_PER_MINUTE_MODE;
} else {
// compute length of linear move in millimeters. Feed rate is provided as mm/min
xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / gms->feed_rate;
// if no linear axes, compute length of multi-axis rotary move in degrees. Feed rate is provided as degrees/min
if (fp_ZERO(xyz_time)) {
abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / gms->feed_rate;
}
}
}
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (gms->motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE) {
tmp_time = fabs(axis_length[axis]) / cm.a[axis].velocity_max;
} else { // MOTION_MODE_STRAIGHT_FEED
tmp_time = fabs(axis_length[axis]) / cm.a[axis].feedrate_max;
}
max_time = max(max_time, tmp_time);
if (tmp_time > 0) { // collect minimum time if this axis is not zero
gms->minimum_time = min(gms->minimum_time, tmp_time);
}
}
gms->move_time = max4(inv_time, max_time, xyz_time, abc_time);
}
uint8_t cm_straight_probe(float target[], bool flags[])
{
// trap zero feed rate condition
if (fp_ZERO(cm.gm.feed_rate)) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// error if no axes specified
if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z]) {
return (STAT_GCODE_AXIS_IS_MISSING);
}
// set probe move endpoint
copy_vector(pb.target, target); // set probe move endpoint
copy_vector(pb.flags, flags); // set axes involved on the move
clear_vector(cm.probe_results); // clear the old probe position.
// NOTE: relying on probe_result will not detect a probe to 0,0,0.
cm.probe_state = PROBE_WAITING; // wait until planner queue empties before completing initialization
pb.func = _probing_init; // bind probing initialization function
return (STAT_OK);
}
/*
#define axis_length bf->body_length
#define axis_velocity bf->cruise_velocity
#define axis_tail bf->tail_length
#define longest_tail bf->head_length
*/
stat_t mp_aline(GCodeState_t *gm_in)
{
mpBuf_t *bf; // current move pointer
float exact_stop = 0; // preset this value OFF
float junction_velocity;
uint8_t mr_flag = false;
// compute some reusable terms
float axis_length[AXES];
float axis_square[AXES];
float length_square = 0;
for (uint8_t axis=0; axis<AXES; axis++) {
axis_length[axis] = gm_in->target[axis] - mm.position[axis];
axis_square[axis] = square(axis_length[axis]);
length_square += axis_square[axis];
}
float length = sqrt(length_square);
if (fp_ZERO(length)) {
// sr_request_status_report();
return (STAT_OK);
}
// If _calc_move_times() says the move will take less than the minimum move time
// get a more accurate time estimate based on starting velocity and acceleration.
// The time of the move is determined by its initial velocity (Vi) and how much
// acceleration will be occur. For this we need to look at the exit velocity of
// the previous block. There are 3 possible cases:
// (1) There is no previous block. Vi = 0
// (2) Previous block is optimally planned. Vi = previous block's exit_velocity
// (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity
_calc_move_times(gm_in, axis_length, axis_square); // set move time and minimum time in the state
if (gm_in->move_time < MIN_BLOCK_TIME) {
float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move
float entry_velocity = 0; // pre-set as if no previous block
if ((bf = mp_get_run_buffer()) != NULL) {
if (bf->replannable == true) { // not optimally planned
entry_velocity = bf->entry_velocity + bf->delta_vmax;
} else { // optimally planned
entry_velocity = bf->exit_velocity;
}
}
float move_time = (2 * length) / (2*entry_velocity + delta_velocity);// compute execution time for this move
if (move_time < MIN_BLOCK_TIME)
return (STAT_MINIMUM_TIME_MOVE);
}
// get a cleared buffer and setup move variables
if ((bf = mp_get_write_buffer()) == NULL)
return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // never supposed to fail
bf->bf_func = mp_exec_aline; // register the callback to the exec function
bf->length = length;
memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer
// Compute the unit vector and find the right jerk to use (combined operations)
// To determine the jerk value to use for the block we want to find the axis for which
// the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which
// the time to decelerate from the target velocity to zero would be the longest.
//
// We can determine the "longest" deceleration in terms of time or distance.
//
// The math for time-to-decelerate T from speed S to speed E with constant
// jerk J is:
//
// T = 2*sqrt((S-E)/J[n])
//
// Since E is always zero in this calculation, we can simplify:
// T = 2*sqrt(S/J[n])
//
// The math for distance-to-decelerate l from speed S to speed E with constant
// jerk J is:
//
// l = (S+E)*sqrt((S-E)/J)
//
// Since E is always zero in this calculation, we can simplify:
// l = S*sqrt(S/J)
//
// The final value we want is to know which one is *longest*, compared to the others,
// so we don't need the actual value. This means that the scale of the value doesn't
// matter, so for T we can remove the "2 *" and For L we can remove the "S*".
// This leaves both to be simply "sqrt(S/J)". Since we don't need the scale,
// it doesn't matter what speed we're coming from, so S can be replaced with 1.
//
// However, we *do* need to compensate for the fact that each axis contributes
// differently to the move, so we will scale each contribution C[n] by the
// proportion of the axis movement length D[n] to the total length of the move L.
// Using that, we construct the following, with these definitions:
//
// J[n] = max_jerk for axis n
// D[n] = distance traveled for this move for axis n
// L = total length of the move in all axes
// C[n] = "axis contribution" of axis n
//
// For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L)
//
// Keeping in mind that we only need a rank compared to the other axes, we can further
// optimize the calculations::
//
// Square the expression to remove the square root:
// C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.)
//
// Re-arranged to optimize divides for pre-calculated values, we create a value
// M that we compute once:
// M = (1/(L^2))
// Then we use it in the calculations for every axis:
// C[n] = (1/J[n]) * D[n]^2 * M
//
// Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further.
//
// Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value
// of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into
// its constituent axes for execution the jerk for that axis will be at it's maximum value.
float C; // contribution term. C = T * a
float maxC = 0;
float recip_L2 = 1/length_square;
for (uint8_t axis=0; axis<AXES; axis++) {
if (fabs(axis_length[axis]) > 0) { // You cannot use the fp_XXX comparisons here!
bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero)
C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's positive
if (C > maxC) {
maxC = C;
bf->jerk_axis = axis; // also needed for junction vmax calculation
}
}
}
// set up and pre-compute the jerk terms needed for this round of planning
bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale the jerk
if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta
mm.jerk = bf->jerk; // used before this point next time around
mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning
mm.cbrt_jerk = cbrt(bf->jerk);
}
bf->recip_jerk = mm.recip_jerk;
bf->cbrt_jerk = mm.cbrt_jerk;
// finish up the current block variables
if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed
bf->replannable = true;
exact_stop = 8675309; // an arbitrarily large floating point number
}
bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested
junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit);
bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop);
bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop);
bf->braking_velocity = bf->delta_vmax;
// Note: these next lines must remain in exact order. Position must update before committing the buffer.
_plan_block_list(bf, &mr_flag); // replan block list
copy_vector(mm.position, bf->gm.target); // set the planner position
mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update)
return (STAT_OK);
}
stat_t mp_plan_hold_callback()
{
if (cm.hold_state != FEEDHOLD_PLAN)
return (STAT_NOOP); // not planning a feedhold
mpBuf_t *bp; // working buffer pointer
if ((bp = mp_get_run_buffer()) == NULL)
return (STAT_NOOP); // Oops! nothing's running
uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx
float mr_available_length; // available length left in mr buffer for deceleration
float braking_velocity; // velocity left to shed to brake to zero
float braking_length; // distance required to brake to zero from braking_velocity
// examine and process mr buffer
mr_available_length = get_axis_vector_length(mr.target, mr.position);
/* mr_available_length =
(sqrt(square(mr.endpoint[AXIS_X] - mr.position[AXIS_X]) +
square(mr.endpoint[AXIS_Y] - mr.position[AXIS_Y]) +
square(mr.endpoint[AXIS_Z] - mr.position[AXIS_Z]) +
square(mr.endpoint[AXIS_A] - mr.position[AXIS_A]) +
square(mr.endpoint[AXIS_B] - mr.position[AXIS_B]) +
square(mr.endpoint[AXIS_C] - mr.position[AXIS_C])));
*/
// compute next_segment velocity
// braking_velocity = mr.segment_velocity;
// if (mr.section != SECTION_BODY) { braking_velocity += mr.forward_diff_1;}
braking_velocity = _compute_next_segment_velocity();
braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here
// Hack to prevent Case 2 moves for perfect-fit decels. Happens in homing situations
// The real fix: The braking velocity cannot simply be the mr.segment_velocity as this
// is the velocity of the last segment, not the one that's going to be executed next.
// The braking_velocity needs to be the velocity of the next segment that has not yet
// been computed. In the mean time, this hack will work.
if ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) {
braking_length = mr_available_length;
}
// Case 1: deceleration fits entirely into the length remaining in mr buffer
if (braking_length <= mr_available_length) {
// set mr to a tail to perform the deceleration
mr.exit_velocity = 0;
mr.tail_length = braking_length;
mr.cruise_velocity = braking_velocity;
mr.section = SECTION_TAIL;
mr.section_state = SECTION_NEW;
// re-use bp+0 to be the hold point and to run the remaining block length
bp->length = mr_available_length - braking_length;
bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
bp->entry_vmax = 0; // set bp+0 as hold point
bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer
_reset_replannable_list(); // make it replan all the blocks
_plan_block_list(mp_get_last_buffer(), &mr_flag);
cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
return (STAT_OK);
}
// Case 2: deceleration exceeds length remaining in mr buffer
// First, replan mr to minimum (but non-zero) exit velocity
mr.section = SECTION_TAIL;
mr.section_state = SECTION_NEW;
mr.tail_length = mr_available_length;
mr.cruise_velocity = braking_velocity;
mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp);
// Find the point where deceleration reaches zero. This could span multiple buffers.
braking_velocity = mr.exit_velocity; // adjust braking velocity downward
bp->move_state = MOVE_NEW; // tell _exec to re-use buffer
for (uint8_t i=0; i<PLANNER_BUFFER_POOL_SIZE; i++) {// a safety to avoid wraparound
mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0 (and onward...)
if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers
bp = mp_get_next_buffer(bp); // point to next buffer
continue;
}
bp->entry_vmax = braking_velocity; // velocity we need to shed
braking_length = mp_get_target_length(braking_velocity, 0, bp);
if (braking_length > bp->length) { // decel does not fit in bp buffer
bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp);
braking_velocity = bp->exit_vmax; // braking velocity for next buffer
bp = mp_get_next_buffer(bp); // point to next buffer
continue;
}
break;
}
// Deceleration now fits in the current bp buffer
// Plan the first buffer of the pair as the decel, the second as the accel
bp->length = braking_length;
bp->exit_vmax = 0;
bp = mp_get_next_buffer(bp); // point to the acceleration buffer
bp->entry_vmax = 0;
bp->length -= braking_length; // the buffers were identical (and hence their lengths)
bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
bp->exit_vmax = bp->delta_vmax;
_reset_replannable_list(); // make it replan all the blocks
_plan_block_list(mp_get_last_buffer(), &mr_flag);
cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
return (STAT_OK);
}
void json_print_response(uint8_t status)
{
#ifdef __SILENCE_JSON_RESPONSES
return;
#endif
if (js.json_verbosity == JV_SILENT) { // silent means no responses
return;
}
if (js.json_verbosity == JV_EXCEPTIONS) { // cutout for JV_EXCEPTIONS mode
if (status == STAT_OK) {
if (cm.machine_state != MACHINE_INITIALIZING) { // always do full echo during startup
return;
}
}
}
// Body processing
nvObj_t *nv = nv_body;
if (status == STAT_JSON_SYNTAX_ERROR) {
nv_reset_nv_list();
nv_add_string((const char *)"err", escape_string(cs.bufp, cs.saved_buf));
} else if (cm.machine_state != MACHINE_INITIALIZING) { // always do full echo during startup
uint8_t nv_type;
do {
if ((nv_type = nv_get_type(nv)) == NV_TYPE_NULL) break;
if (nv_type == NV_TYPE_GCODE) {
if (js.echo_json_gcode_block == false) { // kill command echo if not enabled
nv->valuetype = TYPE_EMPTY;
}
//++++ } else if (nv_type == NV_TYPE_CONFIG) { // kill config echo if not enabled
//fix me if (js.echo_json_configs == false) {
// nv->valuetype = TYPE_EMPTY;
// }
} else if (nv_type == NV_TYPE_MESSAGE) { // kill message echo if not enabled
if (js.echo_json_messages == false) {
nv->valuetype = TYPE_EMPTY;
}
} else if (nv_type == NV_TYPE_LINENUM) { // kill line number echo if not enabled
if ((js.echo_json_linenum == false) || (fp_ZERO(nv->value))) { // do not report line# 0
nv->valuetype = TYPE_EMPTY;
}
}
} while ((nv = nv->nx) != NULL);
}
// Footer processing
while(nv->valuetype != TYPE_EMPTY) { // find a free nvObj at end of the list...
if ((nv = nv->nx) == NULL) { // oops! No free nvObj!
rpt_exception(STAT_JSON_TOO_LONG, "json_print"); // report this as an exception
return;
}
}
char footer_string[NV_FOOTER_LEN];
// in xio.cpp:xio.readline the CR||LF read from the host is not appended to the string.
// to ensure that the correct number of bytes are reported back to the host we add a +1 to
// cs.linelen so that the number of bytes received matches the number of bytes reported
sprintf((char *)footer_string, "%d,%d,%d", 1, status, cs.linelen + 1);
cs.linelen = 0; // reset linelen so it's only reported once
// if (xio.enable_window_mode) { // 2 footer styles are supported...
// sprintf((char *)footer_string, "%d,%d,%d", 2, status, xio_get_window_slots()); //...windowing
// } else {
// sprintf((char *)footer_string, "%d,%d,%d", 1, status, cs.linelen); //...streaming
// cs.linelen = 0; // reset linelen so it's only reported once
// }
nv_copy_string(nv, footer_string); // link string to nv object
nv->depth = 0; // footer 'f' is a peer to response 'r' (hard wired to 0)
nv->valuetype = TYPE_ARRAY; // declare it as an array
strcpy(nv->token, "f"); // set it to Footer
nv->nx = NULL; // terminate the list
// serialize the JSON response and print it if there were no errors
if (json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf)) >= 0) {
fprintf(stderr, "%s", cs.out_buf);
}
}
/*
* cm_arc_feed() - canonical machine entry point for arc
*
* Generates an arc by queueing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear segments.
*/
stat_t cm_arc_feed(float target[], float flags[],// arc endpoints
float i, float j, float k, // raw arc offsets
float radius, // non-zero radius implies radius mode
uint8_t motion_mode) // defined motion mode
{
// trap zero feed rate condition
if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// Trap conditions where no arc movement will occur, but the system is still in
// arc motion mode - this is not an error. This can happen when a F word or M
// word is by itself.(The tests below are organized for execution efficiency)
if ( fp_ZERO(i) && fp_ZERO(j) && fp_ZERO(k) && fp_ZERO(radius) ) {
if ( fp_ZERO((flags[AXIS_X] + flags[AXIS_Y] + flags[AXIS_Z] +
flags[AXIS_A] + flags[AXIS_B] + flags[AXIS_C]))) {
return (STAT_OK);
}
}
// set values in the Gcode model state & copy it (linenum was already captured)
cm_set_model_target(target, flags);
cm.gm.motion_mode = motion_mode;
cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm
memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments
// populate the arc control singleton
copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model
arc.radius = _to_millimeters(radius); // set arc radius or zero
arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm)
arc.offset[1] = _to_millimeters(j);
arc.offset[2] = _to_millimeters(k);
// Set the arc plane for the current G17/G18/G19 setting
// Plane axis 0 and 1 are the arc plane, 2 is the linear axis normal to the arc plane
if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
} else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_Y;
} else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19
arc.plane_axis_0 = AXIS_Y;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_X;
}
// compute arc runtime values and prep for execution by the callback
ritorno(_compute_arc());
// test arc soft limits
stat_t status = _test_arc_soft_limits();
if (status != STAT_OK) {
cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
copy_vector(cm.gm.target, cm.gmx.position); // reset model position
return (cm_soft_alarm(status));
}
cm_cycle_start(); // if not already started
arc.run_state = MOVE_RUN; // enable arc to be run from the callback
cm_finalize_move();
return (STAT_OK);
}
/*
* _compute_arc() - compute arc from I and J (arc center point)
*
* The theta calculation sets up an clockwise or counterclockwise arc from the current
* position to the target position around the center designated by the offset vector.
* All theta-values measured in radians of deviance from the positive y-axis.
*
* | <- theta == 0
* * * *
* * *
* * *
* * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* * /
* C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*
* Parts of this routine were originally sourced from the grbl project.
*/
static stat_t _compute_arc()
{
// A non-zero radius value indicates a radius arc
// Compute IJK offset coordinates. These override any current IJK offsets
if (fp_NOT_ZERO(arc.radius)) ritorno(_compute_arc_offsets_from_radius()); // returns if error
// Calculate the theta (angle) of the current point (see header notes)
// Arc.theta is starting point for theta (theta_start)
arc.theta = _get_theta(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
if(isnan(arc.theta) == true) return(STAT_ARC_SPECIFICATION_ERROR);
// calculate the theta (angle) of the target point
float theta_end = _get_theta(
arc.gm.target[arc.plane_axis_0] - arc.offset[arc.plane_axis_0] - arc.position[arc.plane_axis_0],
arc.gm.target[arc.plane_axis_1] - arc.offset[arc.plane_axis_1] - arc.position[arc.plane_axis_1]);
if(isnan(theta_end) == true) return (STAT_ARC_SPECIFICATION_ERROR);
// ensure that the difference is positive so we have clockwise travel
if (theta_end < arc.theta) { theta_end += 2*M_PI; }
// compute angular travel and invert if gcode wants a counterclockwise arc
// if angular travel is zero interpret it as a full circle
arc.angular_travel = theta_end - arc.theta;
if (fp_ZERO(arc.angular_travel)) {
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) {
arc.angular_travel -= 2*M_PI;
} else {
arc.angular_travel = 2*M_PI;
}
} else {
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) {
arc.angular_travel -= 2*M_PI;
}
}
// Find the radius, calculate travel in the depth axis of the helix,
// and compute the time it should take to perform the move
arc.radius = hypot(arc.offset[arc.plane_axis_0], arc.offset[arc.plane_axis_1]);
arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
// length is the total mm of travel of the helix (or just a planar arc)
arc.length = hypot(arc.angular_travel * arc.radius, fabs(arc.linear_travel));
if (arc.length < cm.arc_segment_len) return (STAT_MINIMUM_LENGTH_MOVE); // arc is too short to draw
arc.time = _get_arc_time(arc.linear_travel, arc.angular_travel, arc.radius);
// Find the minimum number of segments that meets these constraints...
float segments_required_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
float segments_required_for_minimum_distance = arc.length / cm.arc_segment_len;
float segments_required_for_minimum_time = arc.time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
arc.segments = floor(min3(segments_required_for_chordal_accuracy,
segments_required_for_minimum_distance,
segments_required_for_minimum_time));
arc.segments = max(arc.segments, 1); //...but is at least 1 segment
arc.gm.move_time = arc.time / arc.segments; // gcode state struct gets segment_time, not arc time
arc.segment_count = (int32_t)arc.segments;
arc.segment_theta = arc.angular_travel / arc.segments;
arc.segment_linear_travel = arc.linear_travel / arc.segments;
arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target
return (STAT_OK);
}
void mp_calculate_trapezoid(mpBuf_t *bf)
{
//********************************************
//********************************************
//** RULE #1 of mp_calculate_trapezoid() **
//** DON'T CHANGE bf->length **
//********************************************
//********************************************
// F case: Block is too short - run time < minimum segment time
// Force block into a single segment body with limited velocities
// Accept the entry velocity, limit the cruise, and go for the best exit velocity
// you can get given the delta_vmax (maximum velocity slew) supportable.
bf->naiive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// We are violating the jerk value but since it's a single segment move we don't use it.
return;
}
// B" case: Block is short, but fits into a single body segment
if (bf->naiive_move_time <= NOM_SEGMENT_TIME) {
bf->entry_velocity = bf->pv->exit_velocity;
if (fp_NOT_ZERO(bf->entry_velocity)) {
bf->cruise_velocity = bf->entry_velocity;
bf->exit_velocity = bf->entry_velocity;
} else {
bf->cruise_velocity = bf->delta_vmax / 2;
bf->exit_velocity = bf->delta_vmax;
}
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// We are violating the jerk value but since it's a single segment move we don't use it.
return;
}
// B case: Velocities all match (or close enough)
// This occurs frequently in normal gcode files with lots of short lines
// This case is not really necessary, but saves lots of processing time
if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) &&
((bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) {
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
return;
}
// Head-only and tail-only short-line cases
// H" and T" degraded-fit cases
// H' and T' requested-fit cases where the body residual is less than MIN_BODY_LENGTH
bf->body_length = 0;
float minimum_length = mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf);
if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { // head-only & tail-only cases
if (bf->entry_velocity > bf->exit_velocity) { // tail-only cases (short decelerations)
if (bf->length < minimum_length) { // T" (degraded case)
bf->entry_velocity = mp_get_target_velocity(bf->exit_velocity, bf->length, bf);
}
bf->cruise_velocity = bf->entry_velocity;
bf->tail_length = bf->length;
bf->head_length = 0;
return;
}
if (bf->entry_velocity < bf->exit_velocity) { // head-only cases (short accelerations)
if (bf->length < minimum_length) { // H" (degraded case)
bf->exit_velocity = mp_get_target_velocity(bf->entry_velocity, bf->length, bf);
}
bf->cruise_velocity = bf->exit_velocity;
bf->head_length = bf->length;
bf->tail_length = 0;
return;
}
}
// Set head and tail lengths for evaluating the next cases
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
// Rate-limited HT and HT' cases
if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited
// Symmetric rate-limited case (HT)
if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) {
bf->head_length = bf->length/2;
bf->tail_length = bf->head_length;
bf->cruise_velocity = min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf));
if (bf->head_length < MIN_HEAD_LENGTH) {
// Convert this to a body-only move
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// Average the entry speed and computed best cruise-speed
bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2;
bf->entry_velocity = bf->cruise_velocity;
bf->exit_velocity = bf->cruise_velocity;
}
return;
}
// Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often
// iteration trap: uint8_t i=0;
// iteration trap: if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));}
float computed_velocity = bf->cruise_vmax;
do {
bf->cruise_velocity = computed_velocity; // initialize from previous iteration
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
if (bf->head_length > bf->tail_length) {
bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity = mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf);
} else {
bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity = mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf);
}
// insert iteration trap here if needed
} while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT);
// set velocity and clean up any parts that are too short
bf->cruise_velocity = computed_velocity;
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = bf->length - bf->head_length;
if (bf->head_length < MIN_HEAD_LENGTH) {
bf->tail_length = bf->length; // adjust the move to be all tail...
bf->head_length = 0;
}
if (bf->tail_length < MIN_TAIL_LENGTH) {
bf->head_length = bf->length; //...or all head
bf->tail_length = 0;
}
return;
}
// Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases
bf->body_length = bf->length - bf->head_length - bf->tail_length;
// If a non-zero body is < minimum length distribute it to the head and/or tail
// This will generate small (acceptable) velocity errors in runtime execution
// but preserve correct distance, which is more important.
if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) {
if (fp_NOT_ZERO(bf->head_length)) {
if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT
bf->head_length += bf->body_length/2;
bf->tail_length += bf->body_length/2;
} else { // HB reduces to H
bf->head_length += bf->body_length;
}
} else { // BT reduces to T
bf->tail_length += bf->body_length;
}
bf->body_length = 0;
// If the body is a standalone make the cruise velocity match the entry velocity
// This removes a potential velocity discontinuity at the expense of top speed
} else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) {
bf->cruise_velocity = bf->entry_velocity;
}
}
/*
* cm_arc_feed() - canonical machine entry point for arc
*
* Generates an arc by queuing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear arc_segments.
*/
stat_t cm_arc_feed(float target[], float flags[], // arc endpoints
float i, float j, float k, // raw arc offsets
float radius, // non-zero radius implies radius mode
uint8_t motion_mode) // defined motion mode
{
////////////////////////////////////////////////////
// Set axis plane and trap arc specification errors
// trap missing feed rate
if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// set radius mode flag and do simple test(s)
bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc
if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) { // radius value must be + and > minimum radius
return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE);
}
// setup some flags
bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been specified
bool target_y = fp_NOT_ZERO(flags[AXIS_Y]);
bool target_z = fp_NOT_ZERO(flags[AXIS_Z]);
bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // set true if offset I has been specified
bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J
bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K
// Set the arc plane for the current G17/G18/G19 setting and test arc specification
// Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane.
if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
if (radius_f) {
if (!(target_x || target_y)) { // must have at least one endpoint specified
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
}
} else { // center format arc tests
if (offset_k) { // it's OK to be missing either or both i and j, but error if k is present
return (STAT_ARC_SPECIFICATION_ERROR);
}
}
} else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_Y;
if (radius_f) {
if (!(target_x || target_z))
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
} else {
if (offset_j)
return (STAT_ARC_SPECIFICATION_ERROR);
}
} else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19
arc.plane_axis_0 = AXIS_Y;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_X;
if (radius_f) {
if (!(target_y || target_z))
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
} else {
if (offset_i)
return (STAT_ARC_SPECIFICATION_ERROR);
}
}
// set values in the Gcode model state & copy it (linenum was already captured)
cm_set_model_target(target, flags);
// in radius mode it's an error for start == end
if(radius_f) {
if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) &&
(fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) &&
(fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) {
return (STAT_ARC_ENDPOINT_IS_STARTING_POINT);
}
}
// now get down to the rest of the work setting up the arc for execution
cm.gm.motion_mode = motion_mode;
cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm
memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments
copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model
arc.radius = _to_millimeters(radius); // set arc radius or zero
arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm)
arc.offset[1] = _to_millimeters(j);
arc.offset[2] = _to_millimeters(k);
arc.rotations = floor(fabs(cm.gn.parameter)); // P must be a positive integer - force it if not
// determine if this is a full circle arc. Evaluates true if no target is set
arc.full_circle = (fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]));
// compute arc runtime values
ritorno(_compute_arc());
if (fp_ZERO(arc.length)) {
return (STAT_MINIMUM_LENGTH_MOVE); // trap zero length arcs that _compute_arc can throw
}
/* // test arc soft limits
stat_t status = _test_arc_soft_limits();
if (status != STAT_OK) {
cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
copy_vector(cm.gm.target, cm.gmx.position); // reset model position
return (cm_soft_alarm(status));
}
*/
cm_cycle_start(); // if not already started
arc.run_state = MOVE_RUN; // enable arc to be run from the callback
cm_finalize_move();
return (STAT_OK);
}
static stat_t _compute_arc()
{
// Compute radius. A non-zero radius value indicates a radius arc
if (fp_NOT_ZERO(arc.radius)) { // indicates a radius arc
_compute_arc_offsets_from_radius();
} else { // compute start radius
arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
}
// Test arc specification for correctness according to:
// http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc
// "It is an error if: when the arc is projected on the selected plane, the distance from
// the current point to the center differs from the distance from the end point to the
// center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)."
// Compute end radius from the center of circle (offsets) to target endpoint
float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0];
float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1];
float err = fabs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius
if ( (err > ARC_RADIUS_ERROR_MAX) ||
((err > ARC_RADIUS_ERROR_MIN) && (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) {
// return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT);
return (STAT_ARC_SPECIFICATION_ERROR);
}
// Calculate the theta (angle) of the current point (position)
// arc.theta is angular starting point for the arc (also needed later for calculating center point)
arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
// g18_correction is used to invert G18 XZ plane arcs for proper CW orientation
float g18_correction = (cm.gm.select_plane == CANON_PLANE_XZ) ? -1 : 1;
if (arc.full_circle) { // if full circle you can skip the stuff in the else clause
arc.angular_travel = 0; // angular travel always starts as zero for full circles
if (fp_ZERO(arc.rotations)) { // handle the valid case of a full circle arc w/P=0
arc.rotations = 1.0;
}
} else { // ... it's not a full circle
arc.theta_end = atan2(end_0, end_1);
// Compute the angular travel
if (fp_EQ(arc.theta_end, arc.theta)) {
arc.angular_travel = 0; // very large radii arcs can have zero angular travel (thanks PartKam)
} else {
if (arc.theta_end < arc.theta) { // make the difference positive so we have clockwise travel
arc.theta_end += (2*M_PI * g18_correction);
}
arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc
arc.angular_travel -= (2*M_PI * g18_correction);
}
}
}
// Add in travel for rotations
if (cm.gm.motion_mode == MOTION_MODE_CW_ARC) {
arc.angular_travel += (2*M_PI * arc.rotations * g18_correction);
} else {
arc.angular_travel -= (2*M_PI * arc.rotations * g18_correction);
}
// Calculate travel in the depth axis of the helix and compute the time it should take to perform the move
// arc.length is the total mm of travel of the helix (or just a planar arc)
arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
arc.planar_travel = arc.angular_travel * arc.radius;
arc.length = hypotf(arc.planar_travel, arc.linear_travel); // NB: hypot is insensitive to +/- signs
_estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation
// Find the minimum number of arc_segments that meets these constraints...
float arc_segments_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len;
float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy,
arc_segments_for_minimum_distance,
arc_segments_for_minimum_time));
arc.arc_segments = max(arc.arc_segments, 1); //...but is at least 1 arc_segment
arc.gm.move_time = arc.arc_time / arc.arc_segments; // gcode state struct gets arc_segment_time, not arc time
arc.arc_segment_count = (int32_t)arc.arc_segments;
arc.arc_segment_theta = arc.angular_travel / arc.arc_segments;
arc.arc_segment_linear_travel = arc.linear_travel / arc.arc_segments;
arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target
return (STAT_OK);
}
static stat_t _homing_axis_start(int8_t axis)
{
// get the first or next axis
if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
cm.homing_state = HOMING_HOMED;
return (_set_homing_func(_homing_finalize_exit));
} else if (axis == -2) { // -2 is error
return (_homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS));
}
}
// clear the homed flag for axis so we'll be able to move w/o triggering soft limits
cm.homed[axis] = false;
// trap axis mis-configurations
if (fp_ZERO(cm.a[axis].search_velocity)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY));
if (fp_ZERO(cm.a[axis].latch_velocity)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY));
if (cm.a[axis].latch_backoff < 0) return (_homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF));
// calculate and test travel distance
float travel_distance = fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) + cm.a[axis].latch_backoff;
if (fp_ZERO(travel_distance)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL));
// determine the switch setup and that config is OK
#ifndef __NEW_SWITCHES
hm.min_mode = get_switch_mode(MIN_SWITCH(axis));
hm.max_mode = get_switch_mode(MAX_SWITCH(axis));
#else
hm.min_mode = get_switch_mode(axis, SW_MIN);
hm.max_mode = get_switch_mode(axis, SW_MAX);
#endif
if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) { // one or the other must be homing
return (_homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION)); // axis cannot be homed
}
hm.axis = axis; // persist the axis
hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive
hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive
// setup parameters homing to the minimum switch
if (hm.min_mode & SW_HOMING_BIT) {
#ifndef __NEW_SWITCHES
hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch
hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch
#else
hm.homing_switch_axis = axis;
hm.homing_switch_position = SW_MIN; // the min is the homing switch
hm.limit_switch_axis = axis;
hm.limit_switch_position = SW_MAX; // the max would be the limit switch
#endif
hm.search_travel = -travel_distance; // search travels in negative direction
hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction
hm.zero_backoff = cm.a[axis].zero_backoff;
// setup parameters for positive travel (homing to the maximum switch)
} else {
#ifndef __NEW_SWITCHES
hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch
hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch
#else
hm.homing_switch_axis = axis;
hm.homing_switch_position = SW_MAX; // the max is the homing switch
hm.limit_switch_axis = axis;
hm.limit_switch_position = SW_MIN; // the min would be the limit switch
#endif
hm.search_travel = travel_distance; // search travels in positive direction
hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction
hm.zero_backoff = -cm.a[axis].zero_backoff;
}
// if homing is disabled for the axis then skip to the next axis
#ifndef __NEW_SWITCHES
uint8_t sw_mode = get_switch_mode(hm.homing_switch);
if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) {
return (_set_homing_func(_homing_axis_start));
}
// disable the limit switch parameter if there is no limit switch
if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) hm.limit_switch = -1;
#else
// switch_t *s = &sw.s[hm.homing_switch_axis][hm.homing_switch_position];
// _bind_switch_settings(s);
_bind_switch_settings(&sw.s[hm.homing_switch_axis][hm.homing_switch_position]);
uint8_t sw_mode = get_switch_mode(hm.homing_switch_axis, hm.homing_switch_position);
if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) {
return (_set_homing_func(_homing_axis_start));
}
// disable the limit switch parameter if there is no limit switch
if (get_switch_mode(hm.limit_switch_axis, hm.limit_switch_position) == SW_MODE_DISABLED) {
hm.limit_switch_axis = -1;
}
#endif
hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value
return (_set_homing_func(_homing_axis_clear)); // start the clear
}
