Merge pull request #3466 from thinkyhead/rc_look_for_leveling_bug

Add CORE support to st_set_position and plan_set_position
This commit is contained in:
Scott Lahteine 2016-04-11 16:45:00 -07:00
commit b1bb1c7989
4 changed files with 70 additions and 15 deletions

View File

@ -1351,8 +1351,14 @@ static void setup_for_endstop_move() {
#if DISABLED(DELTA) #if DISABLED(DELTA)
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) { static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("BEFORE set_bed_level_equation_lsq", current_position); plan_bed_level_matrix.set_to_identity();
if (DEBUGGING(LEVELING)) {
vector_3 uncorrected_position = plan_get_position();
DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position);
DEBUG_POS(">>> set_bed_level_equation_lsq", current_position);
}
#endif #endif
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1); vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
@ -1371,7 +1377,7 @@ static void setup_for_endstop_move() {
current_position[Z_AXIS] = corrected_position.z; current_position[Z_AXIS] = corrected_position.z;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER set_bed_level_equation_lsq", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", current_position);
#endif #endif
sync_plan_position(); sync_plan_position();
@ -3059,7 +3065,11 @@ inline void gcode_G28() {
#else //!DELTA #else //!DELTA
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("BEFORE matrix.set_to_identity", current_position); if (DEBUGGING(LEVELING)) {
vector_3 corrected_position = plan_get_position();
DEBUG_POS("BEFORE matrix.set_to_identity", corrected_position);
DEBUG_POS("BEFORE matrix.set_to_identity", current_position);
}
#endif #endif
//vector_3 corrected_position = plan_get_position(); //vector_3 corrected_position = plan_get_position();
@ -3069,12 +3079,13 @@ inline void gcode_G28() {
current_position[X_AXIS] = uncorrected_position.x; current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y; current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z; current_position[Z_AXIS] = uncorrected_position.z;
sync_plan_position();
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position);
#endif #endif
sync_plan_position();
#endif // !DELTA #endif // !DELTA
} }

View File

@ -1090,6 +1090,12 @@ float junction_deviation = 0.1;
} // plan_buffer_line() } // plan_buffer_line()
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA) #if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
/**
* Get the XYZ position of the steppers as a vector_3.
*
* On CORE machines XYZ is derived from ABC.
*/
vector_3 plan_get_position() { vector_3 plan_get_position() {
vector_3 position = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS)); vector_3 position = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS));
@ -1102,8 +1108,14 @@ float junction_deviation = 0.1;
return position; return position;
} }
#endif // AUTO_BED_LEVELING_FEATURE && !DELTA #endif // AUTO_BED_LEVELING_FEATURE && !DELTA
/**
* Directly set the planner XYZ position (hence the stepper positions).
*
* On CORE machines stepper ABC will be translated from the given XYZ.
*/
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float& e) void plan_set_position(float x, float y, float z, const float& e)
#else #else

View File

@ -1084,11 +1084,36 @@ void st_init() {
*/ */
void st_synchronize() { while (blocks_queued()) idle(); } void st_synchronize() { while (blocks_queued()) idle(); }
/**
* Set the stepper positions directly in steps
*
* The input is based on the typical per-axis XYZ steps.
* For CORE machines XYZ needs to be translated to ABC.
*
* This allows st_get_axis_position_mm to correctly
* derive the current XYZ position later on.
*/
void st_set_position(const long& x, const long& y, const long& z, const long& e) { void st_set_position(const long& x, const long& y, const long& z, const long& e) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
count_position[X_AXIS] = x;
count_position[Y_AXIS] = y; #if ENABLED(COREXY)
count_position[Z_AXIS] = z; // corexy positioning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
count_position[A_AXIS] = x + y;
count_position[B_AXIS] = x - y;
count_position[Z_AXIS] = z;
#elif ENABLED(COREXZ)
// corexz planning
count_position[A_AXIS] = x + z;
count_position[Y_AXIS] = y;
count_position[C_AXIS] = x - z;
#else
// default non-h-bot planning
count_position[X_AXIS] = x;
count_position[Y_AXIS] = y;
count_position[Z_AXIS] = z;
#endif
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
@ -1099,15 +1124,22 @@ void st_set_e_position(const long& e) {
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
long st_get_position(uint8_t axis) { /**
* Get a stepper's position in steps.
*/
long st_get_position(AxisEnum axis) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
long count_pos = count_position[axis]; long count_pos = count_position[axis];
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
return count_pos; return count_pos;
} }
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
*/
float st_get_axis_position_mm(AxisEnum axis) { float st_get_axis_position_mm(AxisEnum axis) {
float axis_pos; float axis_steps;
#if ENABLED(COREXY) | ENABLED(COREXZ) #if ENABLED(COREXY) | ENABLED(COREXZ)
if (axis == X_AXIS || axis == CORE_AXIS_2) { if (axis == X_AXIS || axis == CORE_AXIS_2) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
@ -1116,14 +1148,14 @@ float st_get_axis_position_mm(AxisEnum axis) {
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1 // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
// ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2 // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
axis_pos = (pos1 + ((axis == X_AXIS) ? pos2 : -pos2)) / 2.0f; axis_steps = (pos1 + ((axis == X_AXIS) ? pos2 : -pos2)) / 2.0f;
} }
else else
axis_pos = st_get_position(axis); axis_steps = st_get_position(axis);
#else #else
axis_pos = st_get_position(axis); axis_steps = st_get_position(axis);
#endif #endif
return axis_pos / axis_steps_per_unit[axis]; return axis_steps / axis_steps_per_unit[axis];
} }
void finishAndDisableSteppers() { void finishAndDisableSteppers() {

View File

@ -61,7 +61,7 @@ void st_set_position(const long& x, const long& y, const long& z, const long& e)
void st_set_e_position(const long& e); void st_set_e_position(const long& e);
// Get current position in steps // Get current position in steps
long st_get_position(uint8_t axis); long st_get_position(AxisEnum axis);
// Get current axis position in mm // Get current axis position in mm
float st_get_axis_position_mm(AxisEnum axis); float st_get_axis_position_mm(AxisEnum axis);