melch/Marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Use unapply_leveling in set_current_from_steppers_for_axis

Browse Source
This commit is contained in:
Scott Lahteine 2016-09-15 15:02:36 -05:00
parent e69d1e2879
commit fafafcf9d1
1 changed files with 11 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
Marlin/Marlin_main.cpp
View File

@ -2139,39 +2139,7 @@ static void clean_up_after_endstop_or_probe_move() {
#endif // AUTO_BED_LEVELING_FEATURE #endif // AUTO_BED_LEVELING_FEATURE
#if ENABLED(AUTO_BED_LEVELING_LINEAR) #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
/**
* Get the stepper positions, apply the rotation matrix
* using the home XY and Z0 position as the fulcrum.
*/
vector_3 untilted_stepper_position() {
get_cartesian_from_steppers();
vector_3 pos = vector_3(
cartes[X_AXIS] - X_TILT_FULCRUM,
cartes[Y_AXIS] - Y_TILT_FULCRUM,
cartes[Z_AXIS]
);
matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
//pos.debug("untilted_stepper_position offset");
//bed_level_matrix.debug("untilted_stepper_position");
//inverse.debug("in untilted_stepper_position");
pos.apply_rotation(inverse);
pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
pos.z = LOGICAL_Z_POSITION(pos.z);
//pos.debug("after rotation and reorientation");
return pos;
}
#elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
/** /**
* Extrapolate a single point from its neighbors * Extrapolate a single point from its neighbors
@ -6067,11 +6035,9 @@ inline void gcode_M400() { stepper.synchronize(); }
void quickstop_stepper() { void quickstop_stepper() {
stepper.quick_stop(); stepper.quick_stop();
#if DISABLED(SCARA) stepper.synchronize();
stepper.synchronize(); set_current_from_steppers_for_axis(ALL_AXES);
LOOP_XYZ(i) set_current_from_steppers_for_axis((AxisEnum)i); SYNC_PLAN_POSITION_KINEMATIC();
SYNC_PLAN_POSITION_KINEMATIC();
#endif
} }
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
@ -8020,19 +7986,16 @@ void get_cartesian_from_steppers() {
* Set the current_position for an axis based on * Set the current_position for an axis based on
* the stepper positions, removing any leveling that * the stepper positions, removing any leveling that
* may have been applied. * may have been applied.
*
* << INCOMPLETE! Still needs to unapply leveling! >>
*/ */
void set_current_from_steppers_for_axis(const AxisEnum axis) { void set_current_from_steppers_for_axis(const AxisEnum axis) {
#if ENABLED(AUTO_BED_LEVELING_LINEAR) get_cartesian_from_steppers();
vector_3 pos = untilted_stepper_position(); #if PLANNER_LEVELING
current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z; planner.unapply_leveling(cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS]);
#elif IS_KINEMATIC
get_cartesian_from_steppers();
current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
#else
current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently
#endif #endif
if (axis == ALL_AXES)
memcpy(current_position, cartes, sizeof(cartes));
else
current_position[axis] = cartes[axis];
} }
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)