andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #3466 from thinkyhead/rc_look_for_leveling_bug 

Add CORE support to st_set_position and plan_set_position
pull/1/head
Scott Lahteine 9 years ago
parent
47e3564bbd 3e5312f116
commit
b1bb1c7989
4 changed files with 70 additions and 15 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      Marlin/Marlin_main.cpp
  2. 12
      Marlin/planner.cpp
  3. 50
      Marlin/stepper.cpp
  4. 2
      Marlin/stepper.h

21
Marlin/Marlin_main.cpp
View File

@ -1351,8 +1351,14 @@ static void setup_for_endstop_move() {
#if DISABLED(DELTA)
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
#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
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;
#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
sync_plan_position();
@ -3059,7 +3065,11 @@ inline void gcode_G28() {
#else //!DELTA
#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
//vector_3 corrected_position = plan_get_position();
@ -3069,12 +3079,13 @@ inline void gcode_G28() {
current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z;
sync_plan_position();
#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
sync_plan_position();
#endif // !DELTA
}

12
Marlin/planner.cpp
View File

@ -1090,6 +1090,12 @@ float junction_deviation = 0.1;
} // plan_buffer_line()
#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 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;
}
#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)
void plan_set_position(float x, float y, float z, const float& e)
#else

50
Marlin/stepper.cpp
View File

@ -1084,11 +1084,36 @@ void st_init() {
*/
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) {
CRITICAL_SECTION_START;
count_position[X_AXIS] = x;
count_position[Y_AXIS] = y;
count_position[Z_AXIS] = z;
#if ENABLED(COREXY)
// 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;
CRITICAL_SECTION_END;
}
@ -1099,15 +1124,22 @@ void st_set_e_position(const long& e) {
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;
long count_pos = count_position[axis];
CRITICAL_SECTION_END;
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 axis_pos;
float axis_steps;
#if ENABLED(COREXY) | ENABLED(COREXZ)
if (axis == X_AXIS || axis == CORE_AXIS_2) {
CRITICAL_SECTION_START;
@ -1116,14 +1148,14 @@ float st_get_axis_position_mm(AxisEnum axis) {
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 -> (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
axis_pos = st_get_position(axis);
axis_steps = st_get_position(axis);
#else
axis_pos = st_get_position(axis);
axis_steps = st_get_position(axis);
#endif
return axis_pos / axis_steps_per_unit[axis];
return axis_steps / axis_steps_per_unit[axis];
}
void finishAndDisableSteppers() {

2
Marlin/stepper.h
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);
// Get current position in steps
long st_get_position(uint8_t axis);
long st_get_position(AxisEnum axis);
// Get current axis position in mm
float st_get_axis_position_mm(AxisEnum axis);

Write Preview
Loading…
Cancel
Save