From b20bf1826db04e1f103d65f646466c050eceaa92 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 14 Apr 2016 00:00:21 -0700 Subject: [PATCH] Fix debugging of vector_3 --- Marlin/Marlin_main.cpp | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 09c4b7747b..cf6e046d35 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -496,6 +496,11 @@ void gcode_M114(); void print_xyz(const char* prefix, const float xyz[]) { print_xyz(prefix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]); } + #if ENABLED(AUTO_BED_LEVELING_FEATURE) + void print_xyz(const char* prefix, const vector_3 &xyz) { + print_xyz(prefix, xyz.x, xyz.y, xyz.z); + } + #endif #define DEBUG_POS(PREFIX,VAR) do{ SERIAL_ECHOPGM(PREFIX); print_xyz(" > " STRINGIFY(VAR), VAR); }while(0) #endif @@ -1373,6 +1378,11 @@ static void setup_for_endstop_move() { static void set_bed_level_equation_lsq(double* plane_equation_coefficients) { + vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1); + plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + + //plan_bed_level_matrix.debug("bed level before"); + #if ENABLED(DEBUG_LEVELING_FEATURE) plan_bed_level_matrix.set_to_identity(); if (DEBUGGING(LEVELING)) { @@ -1382,17 +1392,8 @@ static void setup_for_endstop_move() { } #endif - vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1); - // planeNormal.debug("planeNormal"); - plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); - //bedLevel.debug("bedLevel"); - - //plan_bed_level_matrix.debug("bed level before"); - //vector_3 uncorrected_position = plan_get_position(); - //uncorrected_position.debug("position before"); - vector_3 corrected_position = plan_get_position(); - //corrected_position.debug("position after"); + current_position[X_AXIS] = corrected_position.x; current_position[Y_AXIS] = corrected_position.y; current_position[Z_AXIS] = corrected_position.z; @@ -1426,12 +1427,20 @@ static void setup_for_endstop_move() { plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); vector_3 corrected_position = plan_get_position(); + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + vector_3 uncorrected_position = corrected_position; + DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position); + } + #endif + current_position[X_AXIS] = corrected_position.x; current_position[Y_AXIS] = corrected_position.y; current_position[Z_AXIS] = corrected_position.z; #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", current_position); + if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position); #endif sync_plan_position();