diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 63a78390..a5fbb671 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -2665,6 +2665,26 @@ stat_t cm_set_mto(nvObj_t *nv) return(STAT_OK); } +/* + * cm_get_so() - get spring factor offset + * + */ + +stat_t cm_get_so(nvObj_t *nv) +{ + if (cm_get_motion_state() == MOTION_STOP) { + nv->value = 0; + } else { + nv->value = mp_get_runtime_spring_value(_get_axis(nv->index)); + if (cm_get_units_mode(RUNTIME) == INCHES) { + nv->value *= INCHES_PER_MM; + } + } + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return (STAT_OK); +} + /* * Commands * @@ -2880,6 +2900,9 @@ static const char fmt_Xtn[] = "[%s%s] %s travel minimum%17.3f%s\n"; static const char fmt_Xjm[] = "[%s%s] %s jerk maximum%15.0f%s/min^3 * 1 million\n"; static const char fmt_Xjh[] = "[%s%s] %s jerk homing%16.0f%s/min^3 * 1 million\n"; static const char fmt_Xra[] = "[%s%s] %s radius value%20.4f%s\n"; +static const char fmt_Xsf[] = "[%s%s] %s spring offset factor%20.4f%s\n"; +static const char fmt_Xsm[] = "[%s%s] %s spring offset max%20.4f%s\n"; +static const char fmt_Xso[] = "[%s%s] %s spring offset%20.4f%s\n"; static const char fmt_Xhi[] = "[%s%s] %s homing input%15d [input 1-N or 0 to disable homing this axis]\n"; static const char fmt_Xhd[] = "[%s%s] %s homing direction%11d [0=search-to-negative, 1=search-to-positive]\n"; static const char fmt_Xsv[] = "[%s%s] %s search velocity%12.0f%s/min\n"; @@ -2957,6 +2980,10 @@ void cm_print_jm(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xjm);} void cm_print_jh(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xjh);} void cm_print_ra(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xra);} +void cm_print_sf(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xsf);} +void cm_print_sm(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xsm);} +void cm_print_so(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xso);} + void cm_print_hi(nvObj_t *nv) { _print_axis_ui8(nv, fmt_Xhi);} void cm_print_hd(nvObj_t *nv) { _print_axis_ui8(nv, fmt_Xhd);} void cm_print_sv(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xsv);} diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index ab825b4f..e4768cff 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -384,6 +384,10 @@ typedef struct cmAxis { float latch_velocity; // homing latch velocity float latch_backoff; // backoff sufficient to clear a switch float zero_backoff; // backoff from switches for machine zero + + float spring_offset_factor; // factor of feed offset (sof * velocity = spring_offset) + float spring_retraction_factor; // factor of the x/y velocity to reverse the axis during non-movement + float spring_offset_max; // max amount of spring offset compensation allowed } cfgAxis_t; typedef struct cmSingleton { // struct to manage cm globals and cycles @@ -704,6 +708,8 @@ stat_t cm_set_jh(nvObj_t *nv); // set jerk high with 1,000,000 correcti stat_t cm_set_mfo(nvObj_t *nv); // set manual feedrate override factor stat_t cm_set_mto(nvObj_t *nv); // set manual traverse override factor +stat_t cm_get_so(nvObj_t *nv); // get spring factor offset + stat_t cm_set_probe(nvObj_t *nv); // store current position as the latest probe @@ -774,6 +780,10 @@ stat_t cm_get_nxln(nvObj_t *nv); // return what value we expect the next line void cm_print_jh(nvObj_t *nv); void cm_print_ra(nvObj_t *nv); + void cm_print_sf(nvObj_t *nv); + void cm_print_sm(nvObj_t *nv); + void cm_print_so(nvObj_t *nv); + void cm_print_hi(nvObj_t *nv); void cm_print_hd(nvObj_t *nv); void cm_print_sv(nvObj_t *nv); diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index ed891e46..2768fdf3 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -1073,12 +1073,55 @@ static stat_t _exec_aline_tail(mpBuf_t *bf) static stat_t _exec_aline_segment() { float travel_steps[MOTORS]; + // we don't want to keep the adjusted target in the recorded position, so we'll adjust after + float adjusted_target[AXES]; + + for (uint8_t a=0; a -cm.a[a].spring_retraction_factor) { + // retract backward as fast as allowed, up to -cm.a[a].spring_retraction_factor + new_spring_offset = mr.spring_offset[a]-(mr.segment_time * cm.a[a].velocity_max); + if (new_spring_offset <= -cm.a[a].spring_retraction_factor) { + new_spring_offset = -cm.a[a].spring_retraction_factor; + mr.spring_retraction_backward[a] = false; + } + } else { + // undo retract at half velocity + new_spring_offset = std::max(0.0, mr.spring_offset[a] + (mr.segment_time * cm.a[a].velocity_max)/2.0); + mr.spring_retraction_backward[a] = false; + } + } else { + new_spring_offset = std::min( + (double)cm.a[a].spring_offset_factor * axis_velocity, // new actual offset + (double)mr.spring_offset[a]+((mr.segment_time * cm.a[a].velocity_max)-(mr.unit[a] * (mr.segment_velocity+mr.target_velocity) * 0.5 * mr.segment_time)) // offset at max speed + ); + mr.spring_retraction_backward[a] = true; // next zero-velocity move should be a retraction + } + new_spring_offset = std::max(-cm.a[a].spring_offset_max, std::min(cm.a[a].spring_offset_max, new_spring_offset)); + mr.spring_offset[a] = new_spring_offset; + } else +#endif + { + mr.spring_offset[a] = 0; + } + } + // Set target position for the segment // If the segment ends on a section waypoint synchronize to the head, body or tail end // Otherwise if not at a section waypoint compute target from segment time and velocity // Don't do waypoint correction if you are going into a hold. + // See https://en.wikipedia.org/wiki/Kahan_summation_algorithm + // for the description of the summation compensation used. + if ((--mr.segment_count == 0) && (cm.motion_state != MOTION_HOLD)) { copy_vector(mr.gm.target, mr.waypoint[mr.section]); } else { @@ -1099,6 +1142,11 @@ static stat_t _exec_aline_segment() mr.gm.target[a] = target; } } + copy_vector(adjusted_target, mr.gm.target); + { + uint8_t a = AXIS_A; + adjusted_target[a] += mr.spring_offset[a]; + } // Convert target position to steps // Bucket-brigade the old target down the chain before getting the new target from kinematics @@ -1113,7 +1161,7 @@ static stat_t _exec_aline_segment() mr.encoder_steps[m] = en_read_encoder(m); // get current encoder position (time aligns to commanded_steps) mr.following_error[m] = mr.encoder_steps[m] - mr.commanded_steps[m]; } - kn_inverse_kinematics(mr.gm.target, mr.target_steps); // now determine the target steps... + kn_inverse_kinematics(adjusted_target, mr.target_steps); // now determine the target steps... for (uint8_t m=0; m