Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-22 19:43:53 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

First pass at spring-compensation

Browse Source
This commit is contained in:
Rob Giseburt
2017-12-01 17:11:20 -06:00
parent c9b4e4a91a
commit e6b41a3f12
6 changed files with 105 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/canonical_machine.cpp
+27
View File
@@ -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);}
g2core/canonical_machine.h
+10
View File
@@ -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);
g2core/plan_exec.cpp
+49 -1
View File
@@ -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<AXES; a++) {
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
#else
// recompute the new spring offset
if (a == AXIS_A) {
float axis_velocity = mr.target_velocity * mr.unit[a];
float new_spring_offset = 0.0;
if (fabs(axis_velocity) < 0.00001) {
// this axis isn't moving, so execute retraction vibration
if (mr.spring_retraction_backward[a] && mr.spring_offset[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<MOTORS; m++) { // and compute the distances to be traveled
travel_steps[m] = mr.target_steps[m] - mr.position_steps[m];
}
g2core/plan_line.cpp
+1
View File
@@ -77,6 +77,7 @@ void mp_zero_segment_velocity() { mr.segment_velocity = 0; }
float mp_get_runtime_velocity(void) { return (mr.segment_velocity); }
float mp_get_runtime_absolute_position(uint8_t axis) { return (mr.position[axis]); }
void mp_set_runtime_work_offset(float offset[]) { copy_vector(mr.gm.work_offset, offset); }
float mp_get_runtime_spring_value(uint8_t axis) { return (mr.spring_offset[axis]); }
// We have to handle rotation - "rotate" by the transverse of the matrix to got "normal" coordinates
float mp_get_runtime_work_position(uint8_t axis) {
g2core/planner.h
+3
View File
@@ -489,6 +489,8 @@ typedef struct mpMotionRuntimeSingleton { // persistent runtime variables
sectionState section_state; // state within a move section
float unit[AXES]; // unit vector for axis scaling & planning
float spring_offset[AXES]; // amount of spring offset compensation in effect per axis
bool spring_retraction_backward[AXES]; // true if that axis is retracting
bool axis_flags[AXES]; // set true for axes participating in the move
float target[AXES]; // final target for bf (used to correct rounding errors)
float position[AXES]; // current move position
@@ -592,6 +594,7 @@ float mp_get_runtime_work_position(uint8_t axis);
void mp_set_runtime_work_offset(float offset[]);
bool mp_get_runtime_busy(void) HOT_FUNC;
bool mp_runtime_is_idle(void) HOT_FUNC;
float mp_get_runtime_spring_value(uint8_t axis);
stat_t mp_aline(GCodeState_t *gm_in) HOT_FUNC; // line planning...
void mp_plan_block_list(void) HOT_FUNC;
g2core/settings/settings_Ultimaker_2_Plus.h
+15 -10
View File
@@ -88,7 +88,7 @@
// #define STATUS_REPORT_DEFAULTS "line","posx","posy","posz","posa","he1t","he1st","he1at","he1op","he3t","he3st","he3at","he3op","feed","vel","unit","path","stat","1ts","1sgr","1csa","2ts","2sgr","2csa","3ts","3sgr","3csa","4ts","4sgr","4csa"
// Defaults for thermistor tuning: cut out: ,"he2t","he2st","he2at","he2tr","he2tv","he2op",
#define STATUS_REPORT_DEFAULTS "line","posx","posy","posz","posa","he1t","he1st","he1at","he1tr","he1tv","he1op","he3t","he3st","he3at","he3tr","he3tv","he3op","feed","vel","unit","path","stat","_xs1","_xs2","_xs3","_xs4","_fe1","_fe2","_fe3","_fe4"
#define STATUS_REPORT_DEFAULTS "line","posx","posy","posz","posa","aso","he1t","he1st","he1at","he1tr","he1tv","he1op","he3t","he3st","he3at","he3tr","he3tv","he3op","feed","vel","unit","path","stat","_xs1","_xs2","_xs3","_xs4","_fe1","_fe2","_fe3","_fe4"
// Gcode startup defaults
#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES
#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ
@@ -189,7 +189,7 @@
#define M4_MOTOR_MAP AXIS_A
#define M4_STEP_ANGLE 1.8
#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev
#define M4_MICROSTEPS 128
#define M4_MICROSTEPS 16
#define M4_POLARITY 0
#define M4_POWER_MODE MOTOR_POWER_MODE
#define M4_POWER_LEVEL 0.8
@@ -231,7 +231,7 @@
// *** axis settings **********************************************************************************
#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values
#define X_VELOCITY_MAX 8700 // xvm G0 max velocity in mm/min
#define X_VELOCITY_MAX 15000 // xvm G0 max velocity in mm/min
#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min
#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing
#define X_TRAVEL_MAX 230 // xtm travel between switches or crashes
@@ -247,7 +247,7 @@
//{yjm:5000}
#define Y_AXIS_MODE AXIS_STANDARD
#define Y_VELOCITY_MAX 8700
#define Y_VELOCITY_MAX 15000
#define Y_FEEDRATE_MAX Y_VELOCITY_MAX
#define Y_TRAVEL_MIN 0
#define Y_TRAVEL_MAX 224.5
@@ -299,18 +299,18 @@
#define A_AXIS_MODE AXIS_RADIUS
#define A_RADIUS 1.428
//#define A_VELOCITY_MAX 288886.4
#define A_VELOCITY_MAX 288886.4 // {avm:288886.4}
//#define A_VELOCITY_MAX 144443.0 // G0 rate ~60 mm/s, 3,600 mm/min
#define A_VELOCITY_MAX 72221.5 // G0 rate ~30 mm/s, 3,600 mm/min
//#define A_VELOCITY_MAX 72221.5 // G0 rate ~30 mm/s, 3,600 mm/min
//NYLON
//#define A_VELOCITY_MAX 30000.0 // G0 rate ~20 mm/s {avm:60000.0}
//#define A_FEEDRATE_MAX 48147.7 // ~20 mm/s
//#define A_FEEDRATE_MAX 36110.8 // ~15 mm/s
//#define A_FEEDRATE_MAX 24073.9 // ~10 mm/s
//#define A_FEEDRATE_MAX 12036.95 // ~5 mm/s
#define A_FEEDRATE_MAX 12036.95 // ~5 mm/s
//#define A_FEEDRATE_MAX 6018.475 // ~2.5 mm/s
#define A_FEEDRATE_MAX 3000.0 // ~0.415 mm/s {afr:2000}
//#define A_FEEDRATE_MAX 3000.0 // ~0.415 mm/s {afr:2000}
// NYLON
//#define A_FEEDRATE_MAX 800.0 // {afr:800}
//#define A_FEEDRATE_MAX 500.0 // ~0.2075 mm/s
@@ -318,7 +318,8 @@
#define A_TRAVEL_MAX 10
//#define A_JERK_MAX 288886.4 // ~120 million mm/min^3
//#define A_JERK_MAX 144443.2 // ~60 million mm/min^3
#define A_JERK_MAX 40000.0 // ~20 million mm/min^3 {ajm:48147.7}
//#define A_JERK_MAX 40000.0 // ~20 million mm/min^3 {ajm:48147.7}
#define A_JERK_MAX 2000.0 // ~20 million mm/min^3 {ajm:48147.7}
//NYLON
//#define A_JERK_MAX 25000.0 // ~20 million mm/min^3 {ajm:15000.0}
#define A_HOMING_INPUT 0
@@ -330,10 +331,14 @@
//#define A_JERK_HIGH_SPEED 288886.4 // ~120 million mm/min^3
//#define A_JERK_HIGH_SPEED 240739.0 // ~100 million mm/min^3
//#define A_JERK_HIGH_SPEED 144443.2 // ~60 million mm/min^3
#define A_JERK_HIGH_SPEED 120000.0 //
//#define A_JERK_HIGH_SPEED 120000.0 //
//#define A_JERK_HIGH_SPEED 95000.0 // ~40 million mm/min^3
#define A_JERK_HIGH_SPEED 2000.0
// NYLON
//#define A_JERK_HIGH_SPEED 35000.0 // ~30 million mm/min^3 {ajh:35000.0}
#define A_SPRING_OFFSET_FACTOR 0
#define A_SPRING_OFFSET_MAX 20
#define A_SPRING_RETRACTION_FACTOR -0.001
#define B_AXIS_MODE AXIS_RADIUS