From f2b3ff466b75b9139533dbdd9624399349fa8e59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gu=C3=B0ni=20M=C3=A1r=20Gilbert?= Date: Fri, 9 Aug 2024 15:18:58 +0000 Subject: [PATCH] Remove legacy Auto Bed Leveling code The goal is to clean up the project --- CMakeLists.txt | 2 - Firmware/Configuration.h | 118 -- Firmware/ConfigurationStore.cpp | 4 - Firmware/ConfigurationStore.h | 2 +- Firmware/Marlin.h | 2 +- Firmware/Marlin_main.cpp | 710 +----------- Firmware/messages.cpp | 5 - Firmware/messages.h | 5 - Firmware/planner.cpp | 32 - Firmware/planner.h | 25 - Firmware/qr_solve.cpp | 1930 ------------------------------- Firmware/qr_solve.h | 22 - Firmware/vector_3.cpp | 167 --- Firmware/vector_3.h | 62 - 14 files changed, 3 insertions(+), 3083 deletions(-) delete mode 100644 Firmware/qr_solve.cpp delete mode 100644 Firmware/qr_solve.h delete mode 100644 Firmware/vector_3.cpp delete mode 100644 Firmware/vector_3.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 464535aae3..23a91e6751 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -246,7 +246,6 @@ set(FW_SOURCES power_panic.cpp printer_state.cpp Prusa_farm.cpp - qr_solve.cpp rbuf.c Sd2Card.cpp SdBaseFile.cpp @@ -272,7 +271,6 @@ set(FW_SOURCES uart2.c ultralcd.cpp util.cpp - vector_3.cpp xflash.c xflash_dump.cpp xyzcal.cpp diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h index b959352238..82425396ac 100644 --- a/Firmware/Configuration.h +++ b/Firmware/Configuration.h @@ -290,117 +290,6 @@ your extruder heater takes 2 minutes to hit the target on heating. #define HOME_Z_SEARCH_THRESHOLD 0.15f // Threshold of the Z height in calibration -//============================= Bed Auto Leveling =========================== - -//#define ENABLE_AUTO_BED_LEVELING // Delete the comment to enable (remove // at the start of the line) -#define Z_PROBE_REPEATABILITY_TEST // If not commented out, Z-Probe Repeatability test will be included if Auto Bed Leveling is Enabled. - -#ifdef ENABLE_AUTO_BED_LEVELING - -// There are 2 different ways to pick the X and Y locations to probe: - -// - "grid" mode -// Probe every point in a rectangular grid -// You must specify the rectangle, and the density of sample points -// This mode is preferred because there are more measurements. -// It used to be called ACCURATE_BED_LEVELING but "grid" is more descriptive - -// - "3-point" mode -// Probe 3 arbitrary points on the bed (that aren't colinear) -// You must specify the X & Y coordinates of all 3 points - - #define AUTO_BED_LEVELING_GRID - // with AUTO_BED_LEVELING_GRID, the bed is sampled in a - // AUTO_BED_LEVELING_GRID_POINTSxAUTO_BED_LEVELING_GRID_POINTS grid - // and least squares solution is calculated - // Note: this feature occupies 10'206 byte - #ifdef AUTO_BED_LEVELING_GRID - - // set the rectangle in which to probe - #define LEFT_PROBE_BED_POSITION 15 - #define RIGHT_PROBE_BED_POSITION 170 - #define BACK_PROBE_BED_POSITION 180 - #define FRONT_PROBE_BED_POSITION 20 - - // set the number of grid points per dimension - // I wouldn't see a reason to go above 3 (=9 probing points on the bed) - #define AUTO_BED_LEVELING_GRID_POINTS 2 - - - #else // not AUTO_BED_LEVELING_GRID - // with no grid, just probe 3 arbitrary points. A simple cross-product - // is used to esimate the plane of the print bed - - #define ABL_PROBE_PT_1_X 15 - #define ABL_PROBE_PT_1_Y 180 - #define ABL_PROBE_PT_2_X 15 - #define ABL_PROBE_PT_2_Y 20 - #define ABL_PROBE_PT_3_X 170 - #define ABL_PROBE_PT_3_Y 20 - - #endif // AUTO_BED_LEVELING_GRID - - - // these are the offsets to the probe relative to the extruder tip (Hotend - Probe) - // X and Y offsets must be integers - #define X_PROBE_OFFSET_FROM_EXTRUDER -25 - #define Y_PROBE_OFFSET_FROM_EXTRUDER -29 - #define Z_PROBE_OFFSET_FROM_EXTRUDER -12.35 - - #define Z_RAISE_BEFORE_HOMING 5 // (in mm) Raise Z before homing (G28) for Probe Clearance. - // Be sure you have this distance over your Z_MAX_POS in case - - #define XY_TRAVEL_SPEED 8000 // X and Y axis travel speed between probes, in mm/min - - #define Z_RAISE_BEFORE_PROBING 15 //How much the extruder will be raised before traveling to the first probing point. - #define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when traveling from between next probing points - - //#define Z_PROBE_SLED // turn on if you have a z-probe mounted on a sled like those designed by Charles Bell - //#define SLED_DOCKING_OFFSET 5 // the extra distance the X axis must travel to pickup the sled. 0 should be fine but you can push it further if you'd like. - -//If you have enabled the Bed Auto Leveling and are using the same Z Probe for Z Homing, -//it is highly recommended you let this Z_SAFE_HOMING enabled! - - //#define Z_SAFE_HOMING // This feature is meant to avoid Z homing with probe outside the bed area. - // When defined, it will: - // - Allow Z homing only after X and Y homing AND stepper drivers still enabled - // - If stepper drivers timeout, it will need X and Y homing again before Z homing - // - Position the probe in a defined XY point before Z Homing when homing all axis (G28) - // - Block Z homing only when the probe is outside bed area. - - #ifdef Z_SAFE_HOMING - - #define Z_SAFE_HOMING_X_POINT (X_MAX_LENGTH/2) // X point for Z homing when homing all axis (G28) - #define Z_SAFE_HOMING_Y_POINT (Y_MAX_LENGTH/2) // Y point for Z homing when homing all axis (G28) - - #endif - - #ifdef AUTO_BED_LEVELING_GRID // Check if Probe_Offset * Grid Points is greater than Probing Range - #if X_PROBE_OFFSET_FROM_EXTRUDER < 0 - #if (-(X_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION)) - #error "The X axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS" - #endif - #else - #if ((X_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION)) - #error "The X axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS" - #endif - #endif - #if Y_PROBE_OFFSET_FROM_EXTRUDER < 0 - #if (-(Y_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION)) - #error "The Y axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS" - #endif - #else - #if ((Y_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION)) - #error "The Y axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS" - #endif - #endif - - - #endif - -#endif // ENABLE_AUTO_BED_LEVELING - - // The position of the homing switches //#define MANUAL_HOME_POSITIONS // If defined, MANUAL_*_HOME_POS below will be used //#define BED_CENTER_AT_0_0 // If defined, the center of the bed is at (X=0, Y=0) @@ -420,13 +309,6 @@ your extruder heater takes 2 minutes to hit the target on heating. // Custom M code points #define CUSTOM_M_CODES -#ifdef CUSTOM_M_CODES -#ifdef ENABLE_AUTO_BED_LEVELING - #define CUSTOM_M_CODE_SET_Z_PROBE_OFFSET 851 - #define Z_PROBE_OFFSET_RANGE_MIN -15 - #define Z_PROBE_OFFSET_RANGE_MAX -5 -#endif // ENABLE_AUTO_BED_LEVELING -#endif // CUSTOM_M_CODES // Host Keepalive diff --git a/Firmware/ConfigurationStore.cpp b/Firmware/ConfigurationStore.cpp index 709f5fa22e..5826b0ae4b 100644 --- a/Firmware/ConfigurationStore.cpp +++ b/Firmware/ConfigurationStore.cpp @@ -116,10 +116,6 @@ static_assert (EXTRUDERS == 1, "ConfigurationStore M500_conf not implemented for static_assert (NUM_AXIS == 4, "ConfigurationStore M500_conf not implemented for more axis." "Fix axis_steps_per_mm max_feedrate_normal max_acceleration_mm_per_s2_normal max_jerk max_feedrate_silent" " max_acceleration_mm_per_s2_silent array size."); -#ifdef ENABLE_AUTO_BED_LEVELING -static_assert (false, "zprobe_zoffset was not initialized in printers in field to -(Z_PROBE_OFFSET_FROM_EXTRUDER), so it contains" - "0.0, if this is not acceptable, increment EEPROM_VERSION to force use default_conf"); -#endif static_assert (sizeof(M500_conf) == 209, "sizeof(M500_conf) has changed, ensure that EEPROM_VERSION has been incremented, " "or if you added members in the end of struct, ensure that historically uninitialized values will be initialized." diff --git a/Firmware/ConfigurationStore.h b/Firmware/ConfigurationStore.h index 098890f0be..b2acad6d10 100644 --- a/Firmware/ConfigurationStore.h +++ b/Firmware/ConfigurationStore.h @@ -19,7 +19,7 @@ typedef struct uint32_t min_segment_time_us; //!< (µs) M205 B float max_jerk[4]; //!< Jerk is a maximum immediate velocity change. float add_homing[3]; - float zprobe_zoffset; //!< Only used with define ENABLE_AUTO_BED_LEVELING + float zprobe_zoffset; //!< unused float Kp; float Ki; float Kd; diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h index 6640f08a41..bb2c6ed1ea 100755 --- a/Firmware/Marlin.h +++ b/Firmware/Marlin.h @@ -414,7 +414,7 @@ extern uint8_t calc_percent_done(); /*enum MarlinBusyState { NOT_BUSY, // Not in a handler IN_HANDLER, // Processing a GCode - IN_PROCESS, // Known to be blocking command input (as in G29) + IN_PROCESS, // Known to be blocking command input PAUSED_FOR_USER, // Blocking pending any input PAUSED_FOR_INPUT // Blocking pending text input (concept) };*/ diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index eb049a05c7..612f29cb43 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -49,13 +49,6 @@ #include "macros.h" -#ifdef ENABLE_AUTO_BED_LEVELING -#include "vector_3.h" - #ifdef AUTO_BED_LEVELING_GRID - #include "qr_solve.h" - #endif -#endif // ENABLE_AUTO_BED_LEVELING - #ifdef MESH_BED_LEVELING #include "mesh_bed_leveling.h" #include "mesh_bed_calibration.h" @@ -1888,139 +1881,6 @@ static void clean_up_after_endstop_move(int original_feedmultiply) { previous_millis_cmd.start(); } - - -#ifdef ENABLE_AUTO_BED_LEVELING -#ifdef AUTO_BED_LEVELING_GRID -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); - 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_mm(); - //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; - - // put the bed at 0 so we don't go below it. - current_position[Z_AXIS] = cs.zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure - - plan_set_position_curposXYZE(); -} - -#else // not AUTO_BED_LEVELING_GRID - -static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) { - - plan_bed_level_matrix.set_to_identity(); - - vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1); - vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2); - vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3); - - vector_3 from_2_to_1 = (pt1 - pt2).get_normal(); - vector_3 from_2_to_3 = (pt3 - pt2).get_normal(); - vector_3 planeNormal = vector_3::cross(from_2_to_1, from_2_to_3).get_normal(); - planeNormal = vector_3(planeNormal.x, planeNormal.y, abs(planeNormal.z)); - - plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); - - vector_3 corrected_position = plan_get_position(); - current_position[X_AXIS] = corrected_position.x; - current_position[Y_AXIS] = corrected_position.y; - current_position[Z_AXIS] = corrected_position.z; - - // put the bed at 0 so we don't go below it. - current_position[Z_AXIS] = cs.zprobe_zoffset; - - plan_set_position_curposXYZE(); - -} - -#endif // AUTO_BED_LEVELING_GRID - -static void run_z_probe() { - plan_bed_level_matrix.set_to_identity(); - feedrate = homing_feedrate[Z_AXIS]; - - // move down until you find the bed - float zPosition = -10; - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60); - st_synchronize(); - - // we have to let the planner know where we are right now as it is not where we said to go. - zPosition = st_get_position_mm(Z_AXIS); - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]); - - // move up the retract distance - zPosition += home_retract_mm(Z_AXIS); - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60); - st_synchronize(); - - // move back down slowly to find bed - feedrate = homing_feedrate[Z_AXIS]/4; - zPosition -= home_retract_mm(Z_AXIS) * 2; - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60); - st_synchronize(); - - current_position[Z_AXIS] = st_get_position_mm(Z_AXIS); - // make sure the planner knows where we are as it may be a bit different than we last said to move to - plan_set_position_curposXYZE(); -} - -static void do_blocking_move_to(float x, float y, float z) { - float oldFeedRate = feedrate; - - feedrate = homing_feedrate[Z_AXIS]; - - current_position[Z_AXIS] = z; - plan_buffer_line_curposXYZE(feedrate/60, active_extruder); - st_synchronize(); - - feedrate = XY_TRAVEL_SPEED; - - current_position[X_AXIS] = x; - current_position[Y_AXIS] = y; - plan_buffer_line_curposXYZE(feedrate/60, active_extruder); - st_synchronize(); - - feedrate = oldFeedRate; -} - -static void do_blocking_move_relative(float offset_x, float offset_y, float offset_z) { - do_blocking_move_to(current_position[X_AXIS] + offset_x, current_position[Y_AXIS] + offset_y, current_position[Z_AXIS] + offset_z); -} - - -/// Probe bed height at position (x,y), returns the measured z value -static float probe_pt(float x, float y, float z_before) { - // move to right place - do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before); - do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]); - - run_z_probe(); - float measured_z = current_position[Z_AXIS]; - - SERIAL_PROTOCOLRPGM(_T(MSG_BED)); - SERIAL_PROTOCOLPGM(" x: "); - SERIAL_PROTOCOL(x); - SERIAL_PROTOCOLPGM(" y: "); - SERIAL_PROTOCOL(y); - SERIAL_PROTOCOLPGM(" z: "); - SERIAL_PROTOCOL(measured_z); - SERIAL_PROTOCOLPGM("\n"); - return measured_z; -} - -#endif // #ifdef ENABLE_AUTO_BED_LEVELING - #ifdef LIN_ADVANCE /** * M900: Set and/or Get advance K factor @@ -2533,9 +2393,6 @@ static void gcode_G28(bool home_x_axis, long home_x_value, bool home_y_axis, lon if (home_all_axes) { raise_z_above(MESH_HOME_Z_SEARCH); } -#ifdef ENABLE_AUTO_BED_LEVELING - plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data) -#endif //ENABLE_AUTO_BED_LEVELING // Reset world2machine_rotation_and_skew and world2machine_shift, therefore // the planner will not perform any adjustments in the XY plane. @@ -2632,11 +2489,7 @@ static void gcode_G28(bool home_x_axis, long home_x_value, bool home_y_axis, lon current_position[Y_AXIS]=home_y_value+cs.add_homing[Y_AXIS]; #if Z_HOME_DIR < 0 // If homing towards BED do Z last - #ifndef Z_SAFE_HOMING if(home_z) { - #if defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0) - raise_z_above(Z_RAISE_BEFORE_HOMING); - #endif // defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0) #ifdef MESH_BED_LEVELING // If Mesh bed leveling, move X&Y to safe position for home raise_z_above(MESH_HOME_Z_SEARCH); if (!axis_known_position[X_AXIS]) homeaxis(X_AXIS); @@ -2672,57 +2525,10 @@ static void gcode_G28(bool home_x_axis, long home_x_value, bool home_y_axis, lon homeaxis(Z_AXIS); #endif // MESH_BED_LEVELING } - #else // defined(Z_SAFE_HOMING): Z Safe mode activated. - if(home_all_axes) { - destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER); - destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER); - destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed - feedrate = XY_TRAVEL_SPEED/60; - current_position[Z_AXIS] = 0; - - plan_set_position_curposXYZE(); - plan_buffer_line_destinationXYZE(feedrate); - st_synchronize(); - current_position[X_AXIS] = destination[X_AXIS]; - current_position[Y_AXIS] = destination[Y_AXIS]; - - homeaxis(Z_AXIS); - } - // Let's see if X and Y are homed and probe is inside bed area. - if(home_z) { - if ( (axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]) \ - && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER >= X_MIN_POS) \ - && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER <= X_MAX_POS) \ - && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER >= Y_MIN_POS) \ - && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER <= Y_MAX_POS)) { - - current_position[Z_AXIS] = 0; - plan_set_position_curposXYZE(); - destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1); // Set destination away from bed - feedrate = max_feedrate[Z_AXIS]; - plan_buffer_line_destinationXYZE(feedrate); - st_synchronize(); - - homeaxis(Z_AXIS); - } else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) { - LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN); - SERIAL_ECHO_START; - SERIAL_ECHOLNRPGM(MSG_POSITION_UNKNOWN); - } else { - LCD_MESSAGERPGM(MSG_ZPROBE_OUT); - SERIAL_ECHO_START; - SERIAL_ECHOLNRPGM(MSG_ZPROBE_OUT); - } - } - #endif // Z_SAFE_HOMING #endif // Z_HOME_DIR < 0 if(home_z_axis && home_z_value != 0) current_position[Z_AXIS]=home_z_value+cs.add_homing[Z_AXIS]; - #ifdef ENABLE_AUTO_BED_LEVELING - if(home_z) - current_position[Z_AXIS] += cs.zprobe_zoffset; //Add Z_Probe offset (the distance is negative) - #endif // Set the planner and stepper routine positions. // At this point the mesh bed leveling and world2machine corrections are disabled and current_position @@ -3803,10 +3609,7 @@ extern uint8_t st_backlash_y; //!@n G11 - Retract recover //!@n G21 - Sets Units to Millimters //!@n G28 - Home all Axes one at a time -//!@n G29 - Detailed Z-Probe //!@n G30 - Single Z Probe -//!@n G31 - Dock the sled -//!@n G32 - Undock sled //!@n G75 - Print temperature interpolation //!@n G76 - PINDA probe temperature calibration //!@n G80 - Mesh-based Z probe @@ -3927,7 +3730,6 @@ extern uint8_t st_backlash_y; //!@n M708 - Write to MMU register //!@n M709 - MMU power & reset //!@n M850 - Sheet parameters -//!@n M851 - Set Z-Probe Offset //!@n M860 - Wait for extruder temperature (PINDA) //!@n M861 - Set/Get PINDA temperature compensation offsets //!@n M862 - Print checking @@ -3971,9 +3773,6 @@ void process_commands() #endif /* CMDBUFFER_DEBUG */ unsigned long codenum; //throw away variable -#ifdef ENABLE_AUTO_BED_LEVELING - float x_tmp, y_tmp, z_tmp, real_z; -#endif // PRUSA GCODES KEEPALIVE_STATE(IN_HANDLER); @@ -4412,203 +4211,6 @@ void process_commands() break; } -#ifdef ENABLE_AUTO_BED_LEVELING - - /*! - ### G29 - Detailed Z-Probe G29: Detailed Z-Probe - In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code. - - See `G81` - */ - case 29: - { - #if Z_MIN_PIN == -1 - #error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature! Z_MIN_PIN must point to a valid hardware pin." - #endif - - // Prevent user from running a G29 without first homing in X and Y - if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ) - { - LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN); - SERIAL_ECHO_START; - SERIAL_ECHOLNRPGM(MSG_POSITION_UNKNOWN); - break; // abort G29, since we don't know where we are - } - - st_synchronize(); - // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly - //vector_3 corrected_position = plan_get_position_mm(); - //corrected_position.debug("position before G29"); - plan_bed_level_matrix.set_to_identity(); - vector_3 uncorrected_position = plan_get_position(); - //uncorrected_position.debug("position durring G29"); - current_position[X_AXIS] = uncorrected_position.x; - current_position[Y_AXIS] = uncorrected_position.y; - current_position[Z_AXIS] = uncorrected_position.z; - plan_set_position_curposXYZE(); - int l_feedmultiply = setup_for_endstop_move(); - - feedrate = homing_feedrate[Z_AXIS]; -#ifdef AUTO_BED_LEVELING_GRID - // probe at the points of a lattice grid - - int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1); - int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1); - - - // solve the plane equation ax + by + d = z - // A is the matrix with rows [x y 1] for all the probed points - // B is the vector of the Z positions - // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0 - // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z - - // "A" matrix of the linear system of equations - double eqnAMatrix[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS*3]; - // "B" vector of Z points - double eqnBVector[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS]; - - - int probePointCounter = 0; - bool zig = true; - - for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing) - { - int xProbe, xInc; - if (zig) - { - xProbe = LEFT_PROBE_BED_POSITION; - //xEnd = RIGHT_PROBE_BED_POSITION; - xInc = xGridSpacing; - zig = false; - } else // zag - { - xProbe = RIGHT_PROBE_BED_POSITION; - //xEnd = LEFT_PROBE_BED_POSITION; - xInc = -xGridSpacing; - zig = true; - } - - for (int xCount=0; xCount < AUTO_BED_LEVELING_GRID_POINTS; xCount++) - { - float z_before; - if (probePointCounter == 0) - { - // raise before probing - z_before = Z_RAISE_BEFORE_PROBING; - } else - { - // raise extruder - z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS; - } - - float measured_z = probe_pt(xProbe, yProbe, z_before); - - eqnBVector[probePointCounter] = measured_z; - - eqnAMatrix[probePointCounter + 0*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = xProbe; - eqnAMatrix[probePointCounter + 1*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = yProbe; - eqnAMatrix[probePointCounter + 2*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = 1; - probePointCounter++; - xProbe += xInc; - } - } - clean_up_after_endstop_move(l_feedmultiply); - - // solve lsq problem - double *plane_equation_coefficients = qr_solve(AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS, 3, eqnAMatrix, eqnBVector); - - SERIAL_PROTOCOLPGM("Eqn coefficients: a: "); - SERIAL_PROTOCOL(plane_equation_coefficients[0]); - SERIAL_PROTOCOLPGM(" b: "); - SERIAL_PROTOCOL(plane_equation_coefficients[1]); - SERIAL_PROTOCOLPGM(" d: "); - SERIAL_PROTOCOLLN(plane_equation_coefficients[2]); - - - set_bed_level_equation_lsq(plane_equation_coefficients); - - free(plane_equation_coefficients); - -#else // AUTO_BED_LEVELING_GRID not defined - - // Probe at 3 arbitrary points - // probe 1 - float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING); - - // probe 2 - float z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS); - - // probe 3 - float z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS); - - clean_up_after_endstop_move(l_feedmultiply); - - set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3); - - -#endif // AUTO_BED_LEVELING_GRID - st_synchronize(); - - // The following code correct the Z height difference from z-probe position and hotend tip position. - // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend. - // When the bed is uneven, this height must be corrected. - real_z = float(st_get_position(Z_AXIS))/cs.axis_steps_per_mm[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane) - x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER; - y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER; - z_tmp = current_position[Z_AXIS]; - - apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset - current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner. - plan_set_position_curposXYZE(); - } - break; -#ifndef Z_PROBE_SLED - - /*! - ### G30 - Single Z Probe G30: Single Z-Probe - In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code. - */ - case 30: - { - st_synchronize(); - // TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly - int l_feedmultiply = setup_for_endstop_move(); - - feedrate = homing_feedrate[Z_AXIS]; - - run_z_probe(); - SERIAL_PROTOCOLPGM(_T(MSG_BED)); - SERIAL_PROTOCOLPGM(" X: "); - SERIAL_PROTOCOL(current_position[X_AXIS]); - SERIAL_PROTOCOLPGM(" Y: "); - SERIAL_PROTOCOL(current_position[Y_AXIS]); - SERIAL_PROTOCOLPGM(" Z: "); - SERIAL_PROTOCOLLN(current_position[Z_AXIS]); - - clean_up_after_endstop_move(l_feedmultiply); - } - break; -#else - - /*! - ### G31 - Dock the sled G31: Dock Z Probe sled - In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code. - */ - case 31: - dock_sled(true); - break; - - - /*! - ### G32 - Undock the sled G32: Undock Z Probe sled - In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code. - */ - case 32: - dock_sled(false); - break; -#endif // Z_PROBE_SLED -#endif // ENABLE_AUTO_BED_LEVELING - #ifdef MESH_BED_LEVELING /*! @@ -5511,262 +5113,6 @@ void process_commands() break; #endif //!TMC2130 -#ifdef ENABLE_AUTO_BED_LEVELING -#ifdef Z_PROBE_REPEATABILITY_TEST - - /*! - ### M48 - Z-Probe repeatability measurement function M48: Measure Z-Probe repeatability - - This function assumes the bed has been homed. Specifically, that a G28 command as been issued prior to invoking the M48 Z-Probe repeatability measurement function. Any information generated by a prior G29 Bed leveling command will be lost and needs to be regenerated. - - The number of samples will default to 10 if not specified. You can use upper or lower case letters for any of the options EXCEPT n. n must be in lower case because Marlin uses a capital N for its communication protocol and will get horribly confused if you send it a capital N. - - #### Usage - - M48 [ n | X | Y | V | L ] - - #### Parameters - - `n` - Number of samples. Valid values 4-50 - - `X` - X position for samples - - `Y` - Y position for samples - - `V` - Verbose level. Valid values 1-4 - - `L` - Legs of movementprior to doing probe. Valid values 1-15 - */ - case 48: // M48 Z-Probe repeatability - { - #if Z_MIN_PIN == -1 - #error "You must have a Z_MIN endstop in order to enable calculation of Z-Probe repeatability." - #endif - - double sum=0.0; - double mean=0.0; - double sigma=0.0; - double sample_set[50]; - int verbose_level=1, n=0, j, n_samples = 10, n_legs=0; - double X_current, Y_current, Z_current; - double X_probe_location, Y_probe_location, Z_start_location, ext_position; - - if (code_seen('V')) { - verbose_level = code_value(); - if (verbose_level<0 || verbose_level>4 ) { - SERIAL_PROTOCOLPGM("?Verbose Level not plausable.\n"); - goto Sigma_Exit; - } - } - - if (verbose_level > 0) { - SERIAL_PROTOCOLPGM("M48 Z-Probe Repeatability test. Version 2.00\n"); - SERIAL_PROTOCOLPGM("Full support at: http://3dprintboard.com/forum.php\n"); - } - - if (code_seen('n')) { - n_samples = code_value(); - if (n_samples<4 || n_samples>50 ) { - SERIAL_PROTOCOLPGM("?Specified sample size not plausable.\n"); - goto Sigma_Exit; - } - } - - X_current = X_probe_location = st_get_position_mm(X_AXIS); - Y_current = Y_probe_location = st_get_position_mm(Y_AXIS); - Z_current = st_get_position_mm(Z_AXIS); - Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING; - ext_position = st_get_position_mm(E_AXIS); - - if (code_seen('X') || code_seen('x') ) { - X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER; - if (X_probe_locationX_MAX_POS ) { - SERIAL_PROTOCOLPGM("?Specified X position out of range.\n"); - goto Sigma_Exit; - } - } - - if (code_seen('Y') || code_seen('y') ) { - Y_probe_location = code_value() - Y_PROBE_OFFSET_FROM_EXTRUDER; - if (Y_probe_locationY_MAX_POS ) { - SERIAL_PROTOCOLPGM("?Specified Y position out of range.\n"); - goto Sigma_Exit; - } - } - - if (code_seen('L') || code_seen('l') ) { - n_legs = code_value(); - if ( n_legs==1 ) - n_legs = 2; - if ( n_legs<0 || n_legs>15 ) { - SERIAL_PROTOCOLPGM("?Specified number of legs in movement not plausable.\n"); - goto Sigma_Exit; - } - } - -// -// Do all the preliminary setup work. First raise the probe. -// - - st_synchronize(); - plan_bed_level_matrix.set_to_identity(); - plan_buffer_line( X_current, Y_current, Z_start_location, - ext_position, - homing_feedrate[Z_AXIS]/60); - st_synchronize(); - -// -// Now get everything to the specified probe point So we can safely do a probe to -// get us close to the bed. If the Z-Axis is far from the bed, we don't want to -// use that as a starting point for each probe. -// - if (verbose_level > 2) - SERIAL_PROTOCOL("Positioning probe for the test.\n"); - - plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location, - ext_position, - homing_feedrate[X_AXIS]/60); - st_synchronize(); - - current_position[X_AXIS] = X_current = st_get_position_mm(X_AXIS); - current_position[Y_AXIS] = Y_current = st_get_position_mm(Y_AXIS); - current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); - current_position[E_AXIS] = ext_position = st_get_position_mm(E_AXIS); - -// -// OK, do the inital probe to get us close to the bed. -// Then retrace the right amount and use that in subsequent probes -// - - int l_feedmultiply = setup_for_endstop_move(); - run_z_probe(); - - current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); - Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING; - - plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location, - ext_position, - homing_feedrate[X_AXIS]/60); - st_synchronize(); - current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); - - for( n=0; nX_MAX_POS) - X_current = X_MAX_POS; - - if ( Y_currentY_MAX_POS) - Y_current = Y_MAX_POS; - - if (verbose_level>3 ) { - SERIAL_ECHOPAIR("x: ", X_current); - SERIAL_ECHOPAIR("y: ", Y_current); - SERIAL_PROTOCOLLNPGM(""); - } - - do_blocking_move_to( X_current, Y_current, Z_current ); - } - do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location - } - - int l_feedmultiply = setup_for_endstop_move(); - run_z_probe(); - - sample_set[n] = current_position[Z_AXIS]; - -// -// Get the current mean for the data points we have so far -// - sum=0.0; - for( j=0; j<=n; j++) { - sum = sum + sample_set[j]; - } - mean = sum / (double (n+1)); -// -// Now, use that mean to calculate the standard deviation for the -// data points we have so far -// - - sum=0.0; - for( j=0; j<=n; j++) { - sum = sum + (sample_set[j]-mean) * (sample_set[j]-mean); - } - sigma = sqrt( sum / (double (n+1)) ); - - if (verbose_level > 1) { - SERIAL_PROTOCOL(n+1); - SERIAL_PROTOCOL(" of "); - SERIAL_PROTOCOL(n_samples); - SERIAL_PROTOCOLPGM(" z: "); - SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6); - } - - if (verbose_level > 2) { - SERIAL_PROTOCOL(" mean: "); - SERIAL_PROTOCOL_F(mean,6); - - SERIAL_PROTOCOL(" sigma: "); - SERIAL_PROTOCOL_F(sigma,6); - } - - if (verbose_level > 0) - SERIAL_PROTOCOLPGM("\n"); - - plan_buffer_line( X_probe_location, Y_probe_location, Z_start_location, - current_position[E_AXIS], homing_feedrate[Z_AXIS]/60); - st_synchronize(); - - } - - _delay(1000); - - clean_up_after_endstop_move(l_feedmultiply); - -// enable_endstops(true); - - if (verbose_level > 0) { - SERIAL_PROTOCOLPGM("Mean: "); - SERIAL_PROTOCOL_F(mean, 6); - SERIAL_PROTOCOLPGM("\n"); - } - -SERIAL_PROTOCOLPGM("Standard Deviation: "); -SERIAL_PROTOCOL_F(sigma, 6); -SERIAL_PROTOCOLPGM("\n\n"); - -Sigma_Exit: - break; - } -#endif // Z_PROBE_REPEATABILITY_TEST -#endif // ENABLE_AUTO_BED_LEVELING - /*! ### M72 - Set/get Printer State M72: Set/get Printer State Without any parameter get printer state @@ -7546,55 +6892,6 @@ SERIAL_PROTOCOLPGM("\n\n"); break; #endif -#ifdef ENABLE_AUTO_BED_LEVELING - /*! - ### M851 - Set Z-Probe Offset M851: Set Z-Probe Offset" - Sets the Z-probe Z offset. This offset is used to determine the actual Z position of the nozzle when using a probe to home Z with G28. This value may also be used by G81 (Prusa) / G29 (Marlin) to apply correction to the Z position. - This value represents the distance from nozzle to the bed surface at the point where the probe is triggered. This value will be negative for typical switch probes, inductive probes, and setups where the nozzle makes a circuit with a raised metal contact. This setting will be greater than zero on machines where the nozzle itself is used as the probe, pressing down on the bed to press a switch. (This is a common setup on delta machines.) - #### Usage - - M851 [ Z ] - - #### Parameters - - `Z` - Z offset probe to nozzle. - */ - #ifdef CUSTOM_M_CODE_SET_Z_PROBE_OFFSET - case CUSTOM_M_CODE_SET_Z_PROBE_OFFSET: - { - float value; - if (code_seen('Z')) - { - value = code_value(); - if ((Z_PROBE_OFFSET_RANGE_MIN <= value) && (value <= Z_PROBE_OFFSET_RANGE_MAX)) - { - cs.zprobe_zoffset = -value; // compare w/ line 278 of ConfigurationStore.cpp - SERIAL_ECHO_START; - SERIAL_ECHOLNRPGM(CAT4(MSG_ZPROBE_ZOFFSET, " ", MSG_OK,PSTR(""))); - SERIAL_PROTOCOLLN(); - } - else - { - SERIAL_ECHO_START; - SERIAL_ECHORPGM(MSG_ZPROBE_ZOFFSET); - SERIAL_ECHORPGM(MSG_Z_MIN); - SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MIN); - SERIAL_ECHORPGM(MSG_Z_MAX); - SERIAL_ECHO(Z_PROBE_OFFSET_RANGE_MAX); - SERIAL_PROTOCOLLN(); - } - } - else - { - SERIAL_ECHO_START; - SERIAL_ECHOLNRPGM(CAT2(MSG_ZPROBE_ZOFFSET, PSTR(" : "))); - SERIAL_ECHO(-cs.zprobe_zoffset); - SERIAL_PROTOCOLLN(); - } - break; - } - #endif // CUSTOM_M_CODE_SET_Z_PROBE_OFFSET -#endif // ENABLE_AUTO_BED_LEVELING - /*! ### M552 - Set IP address M552: Set IP address, enable/disable network interface" Sets the printer IP address that is shown in the support menu. Designed to be used with the help of host software. @@ -9321,12 +8618,7 @@ void clamp_to_software_endstops(float target[3]) // Clamp the Z coordinate. if (min_software_endstops) { - float negative_z_offset = 0; - #ifdef ENABLE_AUTO_BED_LEVELING - if (Z_PROBE_OFFSET_FROM_EXTRUDER < 0) negative_z_offset = negative_z_offset + Z_PROBE_OFFSET_FROM_EXTRUDER; - if (cs.add_homing[Z_AXIS] < 0) negative_z_offset = negative_z_offset + cs.add_homing[Z_AXIS]; - #endif - if (target[Z_AXIS] < min_pos[Z_AXIS]+negative_z_offset) target[Z_AXIS] = min_pos[Z_AXIS]+negative_z_offset; + if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS]; } if (max_software_endstops) { if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS]; diff --git a/Firmware/messages.cpp b/Firmware/messages.cpp index 47ed124d98..a818a46e1d 100644 --- a/Firmware/messages.cpp +++ b/Firmware/messages.cpp @@ -380,16 +380,11 @@ const char MSG_WELCOME[] PROGMEM_N1 = WELCOME_MSG; const char MSG_BROWNOUT_RESET[] PROGMEM_N1 = " Brown out Reset"; //// const char MSG_EXTERNAL_RESET[] PROGMEM_N1 = " External Reset"; //// const char MSG_FILE_SAVED[] PROGMEM_N1 = "Done saving file."; //// -const char MSG_POSITION_UNKNOWN[] PROGMEM_N1 = "Home X/Y before Z"; //// const char MSG_SOFTWARE_RESET[] PROGMEM_N1 = " Software Reset"; //// const char MSG_UNKNOWN_COMMAND[] PROGMEM_N1 = "Unknown command: \""; //// const char MSG_WATCHDOG_RESET[] PROGMEM_N1 = " Watchdog Reset"; //// const char MSG_Z_MAX[] PROGMEM_N1 = "z_max: "; //// const char MSG_Z_MIN[] PROGMEM_N1 = "z_min: "; //// -const char MSG_ZPROBE_OUT[] PROGMEM_N1 = "Z probe out. bed"; //// -#ifdef ENABLE_AUTO_BED_LEVELING -const char MSG_ZPROBE_ZOFFSET[] PROGMEM_N1 = "Z Offset"; //// -#endif const char MSG_TMC_OVERTEMP[] PROGMEM_N1 = "TMC DRIVER OVERTEMP"; //// const char MSG_Enqueing[] PROGMEM_N1 = "enqueing \""; //// const char MSG_SD_ERR_WRITE_TO_FILE[] PROGMEM_N1 = "error writing to file"; //// diff --git a/Firmware/messages.h b/Firmware/messages.h index c39669bbf9..872f4bea58 100644 --- a/Firmware/messages.h +++ b/Firmware/messages.h @@ -380,16 +380,11 @@ extern const char MSG_WELCOME[]; extern const char MSG_BROWNOUT_RESET[]; extern const char MSG_EXTERNAL_RESET[]; extern const char MSG_FILE_SAVED[]; -extern const char MSG_POSITION_UNKNOWN[]; extern const char MSG_SOFTWARE_RESET[]; extern const char MSG_UNKNOWN_COMMAND[]; extern const char MSG_WATCHDOG_RESET[]; extern const char MSG_Z_MAX[]; extern const char MSG_Z_MIN[]; -extern const char MSG_ZPROBE_OUT[]; -#ifdef ENABLE_AUTO_BED_LEVELING -extern const char MSG_ZPROBE_ZOFFSET[]; -#endif extern const char MSG_TMC_OVERTEMP[]; extern const char MSG_Enqueing[]; extern const char MSG_SD_ERR_WRITE_TO_FILE[]; diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 91e9956c62..3ea1793b7b 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -84,15 +84,6 @@ float* max_feedrate = cs.max_feedrate_normal; uint32_t* max_acceleration_mm_per_s2 = cs.max_acceleration_mm_per_s2_normal; uint32_t max_acceleration_steps_per_s2[NUM_AXIS]; -#ifdef ENABLE_AUTO_BED_LEVELING -// this holds the required transform to compensate for bed level -matrix_3x3 plan_bed_level_matrix = { - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, -}; -#endif // #ifdef ENABLE_AUTO_BED_LEVELING - // The current position of the tool in absolute steps long position[NUM_AXIS]; //rescaled from extern when axis_steps_per_mm are changed by gcode static float previous_speed[NUM_AXIS]; // Speed of previous path line segment @@ -757,10 +748,6 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate plan_reset_next_e_sched = true; } -#ifdef ENABLE_AUTO_BED_LEVELING - apply_rotation_xyz(plan_bed_level_matrix, x, y, z); -#endif // ENABLE_AUTO_BED_LEVELING - // Apply the machine correction matrix. world2machine(x, y); @@ -1250,27 +1237,8 @@ Having the real displacement of the head, we can calculate the total movement le ENABLE_STEPPER_DRIVER_INTERRUPT(); } -#ifdef ENABLE_AUTO_BED_LEVELING -vector_3 plan_get_position() { - vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS)); - - //position.debug("in plan_get position"); - //plan_bed_level_matrix.debug("in plan_get bed_level"); - matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix); - //inverse.debug("in plan_get inverse"); - position.apply_rotation(inverse); - //position.debug("after rotation"); - - return position; -} -#endif // ENABLE_AUTO_BED_LEVELING - void plan_set_position(float x, float y, float z, const float &e) { -#ifdef ENABLE_AUTO_BED_LEVELING - apply_rotation_xyz(plan_bed_level_matrix, x, y, z); -#endif // ENABLE_AUTO_BED_LEVELING - world2machine(x, y); position[X_AXIS] = lround(x*cs.axis_steps_per_mm[X_AXIS]); diff --git a/Firmware/planner.h b/Firmware/planner.h index 4e6342671b..fcd1beb37f 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -26,10 +26,6 @@ #include "Marlin.h" -#ifdef ENABLE_AUTO_BED_LEVELING -#include "vector_3.h" -#endif // ENABLE_AUTO_BED_LEVELING - enum BlockFlag { // Planner flag to recalculate trapezoids on entry junction. // This flag has an optimization purpose only. @@ -130,24 +126,9 @@ typedef struct { extern float extruder_advance_K; // Linear-advance K factor #endif -#ifdef ENABLE_AUTO_BED_LEVELING -// this holds the required transform to compensate for bed level -extern matrix_3x3 plan_bed_level_matrix; -#endif // #ifdef ENABLE_AUTO_BED_LEVELING - // Initialize the motion plan subsystem void plan_init(); -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimaters. Feed rate specifies the speed of the motion. - -#ifdef ENABLE_AUTO_BED_LEVELING -void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate); - -// Get the position applying the bed level matrix if enabled -vector_3 plan_get_position(); -#else - /// Extracting common call of /// plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], ... /// saves almost 5KB. @@ -159,15 +140,9 @@ void plan_buffer_line_destinationXYZE(float feed_rate); void plan_set_position_curposXYZE(); void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const float* gcode_start_position = NULL, uint16_t segment_idx = 0); -//void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder); -#endif // ENABLE_AUTO_BED_LEVELING // Set position. Used for G92 instructions. -//#ifdef ENABLE_AUTO_BED_LEVELING void plan_set_position(float x, float y, float z, const float &e); -//#else -//void plan_set_position(const float &x, const float &y, const float &z, const float &e); -//#endif // ENABLE_AUTO_BED_LEVELING void plan_set_z_position(const float &z); void plan_set_e_position(const float &e); diff --git a/Firmware/qr_solve.cpp b/Firmware/qr_solve.cpp deleted file mode 100644 index b8f2fdb388..0000000000 --- a/Firmware/qr_solve.cpp +++ /dev/null @@ -1,1930 +0,0 @@ -#include "qr_solve.h" - -#ifdef AUTO_BED_LEVELING_GRID - -#include -#include - -//# include "r8lib.h" - -int i4_min ( int i1, int i2 ) - -/******************************************************************************/ -/* - Purpose: - - I4_MIN returns the smaller of two I4's. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 29 August 2006 - - Author: - - John Burkardt - - Parameters: - - Input, int I1, I2, two integers to be compared. - - Output, int I4_MIN, the smaller of I1 and I2. -*/ -{ - int value; - - if ( i1 < i2 ) - { - value = i1; - } - else - { - value = i2; - } - return value; -} - -double r8_epsilon ( void ) - -/******************************************************************************/ -/* - Purpose: - - R8_EPSILON returns the R8 round off unit. - - Discussion: - - R8_EPSILON is a number R which is a power of 2 with the property that, - to the precision of the computer's arithmetic, - 1 < 1 + R - but - 1 = ( 1 + R / 2 ) - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 01 September 2012 - - Author: - - John Burkardt - - Parameters: - - Output, double R8_EPSILON, the R8 round-off unit. -*/ -{ - const double value = 2.220446049250313E-016; - - return value; -} - -double r8_max ( double x, double y ) - -/******************************************************************************/ -/* - Purpose: - - R8_MAX returns the maximum of two R8's. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 May 2006 - - Author: - - John Burkardt - - Parameters: - - Input, double X, Y, the quantities to compare. - - Output, double R8_MAX, the maximum of X and Y. -*/ -{ - double value; - - if ( y < x ) - { - value = x; - } - else - { - value = y; - } - return value; -} - -double r8_abs ( double x ) - -/******************************************************************************/ -/* - Purpose: - - R8_ABS returns the absolute value of an R8. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 May 2006 - - Author: - - John Burkardt - - Parameters: - - Input, double X, the quantity whose absolute value is desired. - - Output, double R8_ABS, the absolute value of X. -*/ -{ - double value; - - if ( 0.0 <= x ) - { - value = + x; - } - else - { - value = - x; - } - return value; -} - -double r8_sign ( double x ) - -/******************************************************************************/ -/* - Purpose: - - R8_SIGN returns the sign of an R8. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 08 May 2006 - - Author: - - John Burkardt - - Parameters: - - Input, double X, the number whose sign is desired. - - Output, double R8_SIGN, the sign of X. -*/ -{ - double value; - - if ( x < 0.0 ) - { - value = - 1.0; - } - else - { - value = + 1.0; - } - return value; -} - -double r8mat_amax ( int m, int n, double a[] ) - -/******************************************************************************/ -/* - Purpose: - - R8MAT_AMAX returns the maximum absolute value entry of an R8MAT. - - Discussion: - - An R8MAT is a doubly dimensioned array of R8 values, stored as a vector - in column-major order. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 September 2012 - - Author: - - John Burkardt - - Parameters: - - Input, int M, the number of rows in A. - - Input, int N, the number of columns in A. - - Input, double A[M*N], the M by N matrix. - - Output, double R8MAT_AMAX, the maximum absolute value entry of A. -*/ -{ - int i; - int j; - double value; - - value = r8_abs ( a[0+0*m] ); - - for ( j = 0; j < n; j++ ) - { - for ( i = 0; i < m; i++ ) - { - if ( value < r8_abs ( a[i+j*m] ) ) - { - value = r8_abs ( a[i+j*m] ); - } - } - } - return value; -} - -double *r8mat_copy_new ( int m, int n, double a1[] ) - -/******************************************************************************/ -/* - Purpose: - - R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT. - - Discussion: - - An R8MAT is a doubly dimensioned array of R8 values, stored as a vector - in column-major order. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 26 July 2008 - - Author: - - John Burkardt - - Parameters: - - Input, int M, N, the number of rows and columns. - - Input, double A1[M*N], the matrix to be copied. - - Output, double R8MAT_COPY_NEW[M*N], the copy of A1. -*/ -{ - double *a2; - int i; - int j; - - a2 = ( double * ) malloc ( m * n * sizeof ( double ) ); - - for ( j = 0; j < n; j++ ) - { - for ( i = 0; i < m; i++ ) - { - a2[i+j*m] = a1[i+j*m]; - } - } - - return a2; -} - -/******************************************************************************/ - -void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy ) - -/******************************************************************************/ -/* - Purpose: - - DAXPY computes constant times a vector plus a vector. - - Discussion: - - This routine uses unrolled loops for increments equal to one. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of elements in DX and DY. - - Input, double DA, the multiplier of DX. - - Input, double DX[*], the first vector. - - Input, int INCX, the increment between successive entries of DX. - - Input/output, double DY[*], the second vector. - On output, DY[*] has been replaced by DY[*] + DA * DX[*]. - - Input, int INCY, the increment between successive entries of DY. -*/ -{ - int i; - int ix; - int iy; - int m; - - if ( n <= 0 ) - { - return; - } - - if ( da == 0.0 ) - { - return; - } -/* - Code for unequal increments or equal increments - not equal to 1. -*/ - if ( incx != 1 || incy != 1 ) - { - if ( 0 <= incx ) - { - ix = 0; - } - else - { - ix = ( - n + 1 ) * incx; - } - - if ( 0 <= incy ) - { - iy = 0; - } - else - { - iy = ( - n + 1 ) * incy; - } - - for ( i = 0; i < n; i++ ) - { - dy[iy] = dy[iy] + da * dx[ix]; - ix = ix + incx; - iy = iy + incy; - } - } -/* - Code for both increments equal to 1. -*/ - else - { - m = n % 4; - - for ( i = 0; i < m; i++ ) - { - dy[i] = dy[i] + da * dx[i]; - } - - for ( i = m; i < n; i = i + 4 ) - { - dy[i ] = dy[i ] + da * dx[i ]; - dy[i+1] = dy[i+1] + da * dx[i+1]; - dy[i+2] = dy[i+2] + da * dx[i+2]; - dy[i+3] = dy[i+3] + da * dx[i+3]; - } - } - return; -} -/******************************************************************************/ - -double ddot ( int n, double dx[], int incx, double dy[], int incy ) - -/******************************************************************************/ -/* - Purpose: - - DDOT forms the dot product of two vectors. - - Discussion: - - This routine uses unrolled loops for increments equal to one. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vectors. - - Input, double DX[*], the first vector. - - Input, int INCX, the increment between successive entries in DX. - - Input, double DY[*], the second vector. - - Input, int INCY, the increment between successive entries in DY. - - Output, double DDOT, the sum of the product of the corresponding - entries of DX and DY. -*/ -{ - double dtemp; - int i; - int ix; - int iy; - int m; - - dtemp = 0.0; - - if ( n <= 0 ) - { - return dtemp; - } -/* - Code for unequal increments or equal increments - not equal to 1. -*/ - if ( incx != 1 || incy != 1 ) - { - if ( 0 <= incx ) - { - ix = 0; - } - else - { - ix = ( - n + 1 ) * incx; - } - - if ( 0 <= incy ) - { - iy = 0; - } - else - { - iy = ( - n + 1 ) * incy; - } - - for ( i = 0; i < n; i++ ) - { - dtemp = dtemp + dx[ix] * dy[iy]; - ix = ix + incx; - iy = iy + incy; - } - } -/* - Code for both increments equal to 1. -*/ - else - { - m = n % 5; - - for ( i = 0; i < m; i++ ) - { - dtemp = dtemp + dx[i] * dy[i]; - } - - for ( i = m; i < n; i = i + 5 ) - { - dtemp = dtemp + dx[i ] * dy[i ] - + dx[i+1] * dy[i+1] - + dx[i+2] * dy[i+2] - + dx[i+3] * dy[i+3] - + dx[i+4] * dy[i+4]; - } - } - return dtemp; -} -/******************************************************************************/ - -double dnrm2 ( int n, double x[], int incx ) - -/******************************************************************************/ -/* - Purpose: - - DNRM2 returns the euclidean norm of a vector. - - Discussion: - - DNRM2 ( X ) = sqrt ( X' * X ) - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vector. - - Input, double X[*], the vector whose norm is to be computed. - - Input, int INCX, the increment between successive entries of X. - - Output, double DNRM2, the Euclidean norm of X. -*/ -{ - double absxi; - int i; - int ix; - double norm; - double scale; - double ssq; - double value; - - if ( n < 1 || incx < 1 ) - { - norm = 0.0; - } - else if ( n == 1 ) - { - norm = r8_abs ( x[0] ); - } - else - { - scale = 0.0; - ssq = 1.0; - ix = 0; - - for ( i = 0; i < n; i++ ) - { - if ( x[ix] != 0.0 ) - { - absxi = r8_abs ( x[ix] ); - if ( scale < absxi ) - { - ssq = 1.0 + ssq * ( scale / absxi ) * ( scale / absxi ); - scale = absxi; - } - else - { - ssq = ssq + ( absxi / scale ) * ( absxi / scale ); - } - } - ix = ix + incx; - } - - norm = scale * sqrt ( ssq ); - } - - return norm; -} -/******************************************************************************/ - -void dqrank ( double a[], int lda, int m, int n, double tol, int *kr, - int jpvt[], double qraux[] ) - -/******************************************************************************/ -/* - Purpose: - - DQRANK computes the QR factorization of a rectangular matrix. - - Discussion: - - This routine is used in conjunction with DQRLSS to solve - overdetermined, underdetermined and singular linear systems - in a least squares sense. - - DQRANK uses the LINPACK subroutine DQRDC to compute the QR - factorization, with column pivoting, of an M by N matrix A. - The numerical rank is determined using the tolerance TOL. - - Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate - of the condition number of the matrix of independent columns, - and of R. This estimate will be <= 1/TOL. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 21 April 2012 - - Author: - - C version by John Burkardt. - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979, - ISBN13: 978-0-898711-72-1, - LC: QA214.L56. - - Parameters: - - Input/output, double A[LDA*N]. On input, the matrix whose - decomposition is to be computed. On output, the information from DQRDC. - The triangular matrix R of the QR factorization is contained in the - upper triangle and information needed to recover the orthogonal - matrix Q is stored below the diagonal in A and in the vector QRAUX. - - Input, int LDA, the leading dimension of A, which must - be at least M. - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, double TOL, a relative tolerance used to determine the - numerical rank. The problem should be scaled so that all the elements - of A have roughly the same absolute accuracy, EPS. Then a reasonable - value for TOL is roughly EPS divided by the magnitude of the largest - element. - - Output, int *KR, the numerical rank. - - Output, int JPVT[N], the pivot information from DQRDC. - Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly - independent to within the tolerance TOL and the remaining columns - are linearly dependent. - - Output, double QRAUX[N], will contain extra information defining - the QR factorization. -*/ -{ - int i; - int j; - int job; - int k; - double *work; - - for ( i = 0; i < n; i++ ) - { - jpvt[i] = 0; - } - - work = ( double * ) malloc ( n * sizeof ( double ) ); - job = 1; - - dqrdc ( a, lda, m, n, qraux, jpvt, work, job ); - - *kr = 0; - k = i4_min ( m, n ); - - for ( j = 0; j < k; j++ ) - { - if ( r8_abs ( a[j+j*lda] ) <= tol * r8_abs ( a[0+0*lda] ) ) - { - return; - } - *kr = j + 1; - } - - free ( work ); - - return; -} -/******************************************************************************/ - -void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[], - double work[], int job ) - -/******************************************************************************/ -/* - Purpose: - - DQRDC computes the QR factorization of a real rectangular matrix. - - Discussion: - - DQRDC uses Householder transformations. - - Column pivoting based on the 2-norms of the reduced columns may be - performed at the user's option. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 June 2005 - - Author: - - C version by John Burkardt. - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart, - LINPACK User's Guide, - SIAM, (Society for Industrial and Applied Mathematics), - 3600 University City Science Center, - Philadelphia, PA, 19104-2688. - ISBN 0-89871-172-X - - Parameters: - - Input/output, double A(LDA,P). On input, the N by P matrix - whose decomposition is to be computed. On output, A contains in - its upper triangle the upper triangular matrix R of the QR - factorization. Below its diagonal A contains information from - which the orthogonal part of the decomposition can be recovered. - Note that if pivoting has been requested, the decomposition is not that - of the original matrix A but that of A with its columns permuted - as described by JPVT. - - Input, int LDA, the leading dimension of the array A. LDA must - be at least N. - - Input, int N, the number of rows of the matrix A. - - Input, int P, the number of columns of the matrix A. - - Output, double QRAUX[P], contains further information required - to recover the orthogonal part of the decomposition. - - Input/output, integer JPVT[P]. On input, JPVT contains integers that - control the selection of the pivot columns. The K-th column A(*,K) of A - is placed in one of three classes according to the value of JPVT(K). - > 0, then A(K) is an initial column. - = 0, then A(K) is a free column. - < 0, then A(K) is a final column. - Before the decomposition is computed, initial columns are moved to - the beginning of the array A and final columns to the end. Both - initial and final columns are frozen in place during the computation - and only free columns are moved. At the K-th stage of the - reduction, if A(*,K) is occupied by a free column it is interchanged - with the free column of largest reduced norm. JPVT is not referenced - if JOB == 0. On output, JPVT(K) contains the index of the column of the - original matrix that has been interchanged into the K-th column, if - pivoting was requested. - - Workspace, double WORK[P]. WORK is not referenced if JOB == 0. - - Input, int JOB, initiates column pivoting. - 0, no pivoting is done. - nonzero, pivoting is done. -*/ -{ - int j; - int jp; - int l; - int lup; - int maxj; - double maxnrm; - double nrmxl; - int pl; - int pu; - int swapj; - double t; - double tt; - - pl = 1; - pu = 0; -/* - If pivoting is requested, rearrange the columns. -*/ - if ( job != 0 ) - { - for ( j = 1; j <= p; j++ ) - { - swapj = ( 0 < jpvt[j-1] ); - - if ( jpvt[j-1] < 0 ) - { - jpvt[j-1] = -j; - } - else - { - jpvt[j-1] = j; - } - - if ( swapj ) - { - if ( j != pl ) - { - dswap ( n, a+0+(pl-1)*lda, 1, a+0+(j-1), 1 ); - } - jpvt[j-1] = jpvt[pl-1]; - jpvt[pl-1] = j; - pl = pl + 1; - } - } - pu = p; - - for ( j = p; 1 <= j; j-- ) - { - if ( jpvt[j-1] < 0 ) - { - jpvt[j-1] = -jpvt[j-1]; - - if ( j != pu ) - { - dswap ( n, a+0+(pu-1)*lda, 1, a+0+(j-1)*lda, 1 ); - jp = jpvt[pu-1]; - jpvt[pu-1] = jpvt[j-1]; - jpvt[j-1] = jp; - } - pu = pu - 1; - } - } - } -/* - Compute the norms of the free columns. -*/ - for ( j = pl; j <= pu; j++ ) - { - qraux[j-1] = dnrm2 ( n, a+0+(j-1)*lda, 1 ); - } - - for ( j = pl; j <= pu; j++ ) - { - work[j-1] = qraux[j-1]; - } -/* - Perform the Householder reduction of A. -*/ - lup = i4_min ( n, p ); - - for ( l = 1; l <= lup; l++ ) - { -/* - Bring the column of largest norm into the pivot position. -*/ - if ( pl <= l && l < pu ) - { - maxnrm = 0.0; - maxj = l; - for ( j = l; j <= pu; j++ ) - { - if ( maxnrm < qraux[j-1] ) - { - maxnrm = qraux[j-1]; - maxj = j; - } - } - - if ( maxj != l ) - { - dswap ( n, a+0+(l-1)*lda, 1, a+0+(maxj-1)*lda, 1 ); - qraux[maxj-1] = qraux[l-1]; - work[maxj-1] = work[l-1]; - jp = jpvt[maxj-1]; - jpvt[maxj-1] = jpvt[l-1]; - jpvt[l-1] = jp; - } - } -/* - Compute the Householder transformation for column L. -*/ - qraux[l-1] = 0.0; - - if ( l != n ) - { - nrmxl = dnrm2 ( n-l+1, a+l-1+(l-1)*lda, 1 ); - - if ( nrmxl != 0.0 ) - { - if ( a[l-1+(l-1)*lda] != 0.0 ) - { - nrmxl = nrmxl * r8_sign ( a[l-1+(l-1)*lda] ); - } - - dscal ( n-l+1, 1.0 / nrmxl, a+l-1+(l-1)*lda, 1 ); - a[l-1+(l-1)*lda] = 1.0 + a[l-1+(l-1)*lda]; -/* - Apply the transformation to the remaining columns, updating the norms. -*/ - for ( j = l + 1; j <= p; j++ ) - { - t = -ddot ( n-l+1, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 ) - / a[l-1+(l-1)*lda]; - daxpy ( n-l+1, t, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 ); - - if ( pl <= j && j <= pu ) - { - if ( qraux[j-1] != 0.0 ) - { - tt = 1.0 - pow ( r8_abs ( a[l-1+(j-1)*lda] ) / qraux[j-1], 2 ); - tt = r8_max ( tt, 0.0 ); - t = tt; - tt = 1.0 + 0.05 * tt * pow ( qraux[j-1] / work[j-1], 2 ); - - if ( tt != 1.0 ) - { - qraux[j-1] = qraux[j-1] * sqrt ( t ); - } - else - { - qraux[j-1] = dnrm2 ( n-l, a+l+(j-1)*lda, 1 ); - work[j-1] = qraux[j-1]; - } - } - } - } -/* - Save the transformation. -*/ - qraux[l-1] = a[l-1+(l-1)*lda]; - a[l-1+(l-1)*lda] = -nrmxl; - } - } - } - return; -} -/******************************************************************************/ - -int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[], - double x[], double rsd[], int jpvt[], double qraux[], int itask ) - -/******************************************************************************/ -/* - Purpose: - - DQRLS factors and solves a linear system in the least squares sense. - - Discussion: - - The linear system may be overdetermined, underdetermined or singular. - The solution is obtained using a QR factorization of the - coefficient matrix. - - DQRLS can be efficiently used to solve several least squares - problems with the same matrix A. The first system is solved - with ITASK = 1. The subsequent systems are solved with - ITASK = 2, to avoid the recomputation of the matrix factors. - The parameters KR, JPVT, and QRAUX must not be modified - between calls to DQRLS. - - DQRLS is used to solve in a least squares sense - overdetermined, underdetermined and singular linear systems. - The system is A*X approximates B where A is M by N. - B is a given M-vector, and X is the N-vector to be computed. - A solution X is found which minimimzes the sum of squares (2-norm) - of the residual, A*X - B. - - The numerical rank of A is determined using the tolerance TOL. - - DQRLS uses the LINPACK subroutine DQRDC to compute the QR - factorization, with column pivoting, of an M by N matrix A. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 10 September 2012 - - Author: - - C version by John Burkardt. - - Reference: - - David Kahaner, Cleve Moler, Steven Nash, - Numerical Methods and Software, - Prentice Hall, 1989, - ISBN: 0-13-627258-4, - LC: TA345.K34. - - Parameters: - - Input/output, double A[LDA*N], an M by N matrix. - On input, the matrix whose decomposition is to be computed. - In a least squares data fitting problem, A(I,J) is the - value of the J-th basis (model) function at the I-th data point. - On output, A contains the output from DQRDC. The triangular matrix R - of the QR factorization is contained in the upper triangle and - information needed to recover the orthogonal matrix Q is stored - below the diagonal in A and in the vector QRAUX. - - Input, int LDA, the leading dimension of A. - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, double TOL, a relative tolerance used to determine the - numerical rank. The problem should be scaled so that all the elements - of A have roughly the same absolute accuracy EPS. Then a reasonable - value for TOL is roughly EPS divided by the magnitude of the largest - element. - - Output, int *KR, the numerical rank. - - Input, double B[M], the right hand side of the linear system. - - Output, double X[N], a least squares solution to the linear - system. - - Output, double RSD[M], the residual, B - A*X. RSD may - overwrite B. - - Workspace, int JPVT[N], required if ITASK = 1. - Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly - independent to within the tolerance TOL and the remaining columns - are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate - of the condition number of the matrix of independent columns, - and of R. This estimate will be <= 1/TOL. - - Workspace, double QRAUX[N], required if ITASK = 1. - - Input, int ITASK. - 1, DQRLS factors the matrix A and solves the least squares problem. - 2, DQRLS assumes that the matrix A was factored with an earlier - call to DQRLS, and only solves the least squares problem. - - Output, int DQRLS, error code. - 0: no error - -1: LDA < M (fatal error) - -2: N < 1 (fatal error) - -3: ITASK < 1 (fatal error) -*/ -{ - int ind; - - if ( lda < m ) - { - /*fprintf ( stderr, "\n" ); - fprintf ( stderr, "DQRLS - Fatal error!\n" ); - fprintf ( stderr, " LDA < M.\n" );*/ - ind = -1; - return ind; - } - - if ( n <= 0 ) - { - /*fprintf ( stderr, "\n" ); - fprintf ( stderr, "DQRLS - Fatal error!\n" ); - fprintf ( stderr, " N <= 0.\n" );*/ - ind = -2; - return ind; - } - - if ( itask < 1 ) - { - /*fprintf ( stderr, "\n" ); - fprintf ( stderr, "DQRLS - Fatal error!\n" ); - fprintf ( stderr, " ITASK < 1.\n" );*/ - ind = -3; - return ind; - } - - ind = 0; -/* - Factor the matrix. -*/ - if ( itask == 1 ) - { - dqrank ( a, lda, m, n, tol, kr, jpvt, qraux ); - } -/* - Solve the least-squares problem. -*/ - dqrlss ( a, lda, m, n, *kr, b, x, rsd, jpvt, qraux ); - - return ind; -} -/******************************************************************************/ - -void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[], - double rsd[], int jpvt[], double qraux[] ) - -/******************************************************************************/ -/* - Purpose: - - DQRLSS solves a linear system in a least squares sense. - - Discussion: - - DQRLSS must be preceded by a call to DQRANK. - - The system is to be solved is - A * X = B - where - A is an M by N matrix with rank KR, as determined by DQRANK, - B is a given M-vector, - X is the N-vector to be computed. - - A solution X, with at most KR nonzero components, is found which - minimizes the 2-norm of the residual (A*X-B). - - Once the matrix A has been formed, DQRANK should be - called once to decompose it. Then, for each right hand - side B, DQRLSS should be called once to obtain the - solution and residual. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 10 September 2012 - - Author: - - C version by John Burkardt - - Parameters: - - Input, double A[LDA*N], the QR factorization information - from DQRANK. The triangular matrix R of the QR factorization is - contained in the upper triangle and information needed to recover - the orthogonal matrix Q is stored below the diagonal in A and in - the vector QRAUX. - - Input, int LDA, the leading dimension of A, which must - be at least M. - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, int KR, the rank of the matrix, as estimated by DQRANK. - - Input, double B[M], the right hand side of the linear system. - - Output, double X[N], a least squares solution to the - linear system. - - Output, double RSD[M], the residual, B - A*X. RSD may - overwrite B. - - Input, int JPVT[N], the pivot information from DQRANK. - Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly - independent to within the tolerance TOL and the remaining columns - are linearly dependent. - - Input, double QRAUX[N], auxiliary information from DQRANK - defining the QR factorization. -*/ -{ - int i; - int info; - int j; - int job; - int k; - double t; - - if ( kr != 0 ) - { - job = 110; - info = dqrsl ( a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job ); - } - - for ( i = 0; i < n; i++ ) - { - jpvt[i] = - jpvt[i]; - } - - for ( i = kr; i < n; i++ ) - { - x[i] = 0.0; - } - - for ( j = 1; j <= n; j++ ) - { - if ( jpvt[j-1] <= 0 ) - { - k = - jpvt[j-1]; - jpvt[j-1] = k; - - while ( k != j ) - { - t = x[j-1]; - x[j-1] = x[k-1]; - x[k-1] = t; - jpvt[k-1] = -jpvt[k-1]; - k = jpvt[k-1]; - } - } - } - return; -} -/******************************************************************************/ - -int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[], - double qy[], double qty[], double b[], double rsd[], double ab[], int job ) - -/******************************************************************************/ -/* - Purpose: - - DQRSL computes transformations, projections, and least squares solutions. - - Discussion: - - DQRSL requires the output of DQRDC. - - For K <= min(N,P), let AK be the matrix - - AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) ) - - formed from columns JPVT[0], ..., JPVT(K) of the original - N by P matrix A that was input to DQRDC. If no pivoting was - done, AK consists of the first K columns of A in their - original order. DQRDC produces a factored orthogonal matrix Q - and an upper triangular matrix R such that - - AK = Q * (R) - (0) - - This information is contained in coded form in the arrays - A and QRAUX. - - The parameters QY, QTY, B, RSD, and AB are not referenced - if their computation is not requested and in this case - can be replaced by dummy variables in the calling program. - To save storage, the user may in some cases use the same - array for different parameters in the calling sequence. A - frequently occurring example is when one wishes to compute - any of B, RSD, or AB and does not need Y or QTY. In this - case one may identify Y, QTY, and one of B, RSD, or AB, while - providing separate arrays for anything else that is to be - computed. - - Thus the calling sequence - - dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info ) - - will result in the computation of B and RSD, with RSD - overwriting Y. More generally, each item in the following - list contains groups of permissible identifications for - a single calling sequence. - - 1. (Y,QTY,B) (RSD) (AB) (QY) - - 2. (Y,QTY,RSD) (B) (AB) (QY) - - 3. (Y,QTY,AB) (B) (RSD) (QY) - - 4. (Y,QY) (QTY,B) (RSD) (AB) - - 5. (Y,QY) (QTY,RSD) (B) (AB) - - 6. (Y,QY) (QTY,AB) (B) (RSD) - - In any group the value returned in the array allocated to - the group corresponds to the last member of the group. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 June 2005 - - Author: - - C version by John Burkardt. - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart, - LINPACK User's Guide, - SIAM, (Society for Industrial and Applied Mathematics), - 3600 University City Science Center, - Philadelphia, PA, 19104-2688. - ISBN 0-89871-172-X - - Parameters: - - Input, double A[LDA*P], contains the output of DQRDC. - - Input, int LDA, the leading dimension of the array A. - - Input, int N, the number of rows of the matrix AK. It must - have the same value as N in DQRDC. - - Input, int K, the number of columns of the matrix AK. K - must not be greater than min(N,P), where P is the same as in the - calling sequence to DQRDC. - - Input, double QRAUX[P], the auxiliary output from DQRDC. - - Input, double Y[N], a vector to be manipulated by DQRSL. - - Output, double QY[N], contains Q * Y, if requested. - - Output, double QTY[N], contains Q' * Y, if requested. - - Output, double B[K], the solution of the least squares problem - minimize norm2 ( Y - AK * B), - if its computation has been requested. Note that if pivoting was - requested in DQRDC, the J-th component of B will be associated with - column JPVT(J) of the original matrix A that was input into DQRDC. - - Output, double RSD[N], the least squares residual Y - AK * B, - if its computation has been requested. RSD is also the orthogonal - projection of Y onto the orthogonal complement of the column space - of AK. - - Output, double AB[N], the least squares approximation Ak * B, - if its computation has been requested. AB is also the orthogonal - projection of Y onto the column space of A. - - Input, integer JOB, specifies what is to be computed. JOB has - the decimal expansion ABCDE, with the following meaning: - - if A != 0, compute QY. - if B != 0, compute QTY. - if C != 0, compute QTY and B. - if D != 0, compute QTY and RSD. - if E != 0, compute QTY and AB. - - Note that a request to compute B, RSD, or AB automatically triggers - the computation of QTY, for which an array must be provided in the - calling sequence. - - Output, int DQRSL, is zero unless the computation of B has - been requested and R is exactly singular. In this case, INFO is the - index of the first zero diagonal element of R, and B is left unaltered. -*/ -{ - int cab; - int cb; - int cqty; - int cqy; - int cr; - int i; - int info; - int j; - int jj; - int ju; - double t; - double temp; -/* - Set INFO flag. -*/ - info = 0; -/* - Determine what is to be computed. -*/ - cqy = ( job / 10000 != 0 ); - cqty = ( ( job % 10000 ) != 0 ); - cb = ( ( job % 1000 ) / 100 != 0 ); - cr = ( ( job % 100 ) / 10 != 0 ); - cab = ( ( job % 10 ) != 0 ); - - ju = i4_min ( k, n-1 ); -/* - Special action when N = 1. -*/ - if ( ju == 0 ) - { - if ( cqy ) - { - qy[0] = y[0]; - } - - if ( cqty ) - { - qty[0] = y[0]; - } - - if ( cab ) - { - ab[0] = y[0]; - } - - if ( cb ) - { - if ( a[0+0*lda] == 0.0 ) - { - info = 1; - } - else - { - b[0] = y[0] / a[0+0*lda]; - } - } - - if ( cr ) - { - rsd[0] = 0.0; - } - return info; - } -/* - Set up to compute QY or QTY. -*/ - if ( cqy ) - { - for ( i = 1; i <= n; i++ ) - { - qy[i-1] = y[i-1]; - } - } - - if ( cqty ) - { - for ( i = 1; i <= n; i++ ) - { - qty[i-1] = y[i-1]; - } - } -/* - Compute QY. -*/ - if ( cqy ) - { - for ( jj = 1; jj <= ju; jj++ ) - { - j = ju - jj + 1; - - if ( qraux[j-1] != 0.0 ) - { - temp = a[j-1+(j-1)*lda]; - a[j-1+(j-1)*lda] = qraux[j-1]; - t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qy+j-1, 1 ) / a[j-1+(j-1)*lda]; - daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qy+j-1, 1 ); - a[j-1+(j-1)*lda] = temp; - } - } - } -/* - Compute Q'*Y. -*/ - if ( cqty ) - { - for ( j = 1; j <= ju; j++ ) - { - if ( qraux[j-1] != 0.0 ) - { - temp = a[j-1+(j-1)*lda]; - a[j-1+(j-1)*lda] = qraux[j-1]; - t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qty+j-1, 1 ) / a[j-1+(j-1)*lda]; - daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qty+j-1, 1 ); - a[j-1+(j-1)*lda] = temp; - } - } - } -/* - Set up to compute B, RSD, or AB. -*/ - if ( cb ) - { - for ( i = 1; i <= k; i++ ) - { - b[i-1] = qty[i-1]; - } - } - - if ( cab ) - { - for ( i = 1; i <= k; i++ ) - { - ab[i-1] = qty[i-1]; - } - } - - if ( cr && k < n ) - { - for ( i = k+1; i <= n; i++ ) - { - rsd[i-1] = qty[i-1]; - } - } - - if ( cab && k+1 <= n ) - { - for ( i = k+1; i <= n; i++ ) - { - ab[i-1] = 0.0; - } - } - - if ( cr ) - { - for ( i = 1; i <= k; i++ ) - { - rsd[i-1] = 0.0; - } - } -/* - Compute B. -*/ - if ( cb ) - { - for ( jj = 1; jj <= k; jj++ ) - { - j = k - jj + 1; - - if ( a[j-1+(j-1)*lda] == 0.0 ) - { - info = j; - break; - } - - b[j-1] = b[j-1] / a[j-1+(j-1)*lda]; - - if ( j != 1 ) - { - t = -b[j-1]; - daxpy ( j-1, t, a+0+(j-1)*lda, 1, b, 1 ); - } - } - } -/* - Compute RSD or AB as required. -*/ - if ( cr || cab ) - { - for ( jj = 1; jj <= ju; jj++ ) - { - j = ju - jj + 1; - - if ( qraux[j-1] != 0.0 ) - { - temp = a[j-1+(j-1)*lda]; - a[j-1+(j-1)*lda] = qraux[j-1]; - - if ( cr ) - { - t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 ) - / a[j-1+(j-1)*lda]; - daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 ); - } - - if ( cab ) - { - t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, ab+j-1, 1 ) - / a[j-1+(j-1)*lda]; - daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, ab+j-1, 1 ); - } - a[j-1+(j-1)*lda] = temp; - } - } - } - - return info; -} -/******************************************************************************/ - -/******************************************************************************/ - -void dscal ( int n, double sa, double x[], int incx ) - -/******************************************************************************/ -/* - Purpose: - - DSCAL scales a vector by a constant. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vector. - - Input, double SA, the multiplier. - - Input/output, double X[*], the vector to be scaled. - - Input, int INCX, the increment between successive entries of X. -*/ -{ - int i; - int ix; - int m; - - if ( n <= 0 ) - { - } - else if ( incx == 1 ) - { - m = n % 5; - - for ( i = 0; i < m; i++ ) - { - x[i] = sa * x[i]; - } - - for ( i = m; i < n; i = i + 5 ) - { - x[i] = sa * x[i]; - x[i+1] = sa * x[i+1]; - x[i+2] = sa * x[i+2]; - x[i+3] = sa * x[i+3]; - x[i+4] = sa * x[i+4]; - } - } - else - { - if ( 0 <= incx ) - { - ix = 0; - } - else - { - ix = ( - n + 1 ) * incx; - } - - for ( i = 0; i < n; i++ ) - { - x[ix] = sa * x[ix]; - ix = ix + incx; - } - } - return; -} -/******************************************************************************/ - - -void dswap ( int n, double x[], int incx, double y[], int incy ) - -/******************************************************************************/ -/* - Purpose: - - DSWAP interchanges two vectors. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vectors. - - Input/output, double X[*], one of the vectors to swap. - - Input, int INCX, the increment between successive entries of X. - - Input/output, double Y[*], one of the vectors to swap. - - Input, int INCY, the increment between successive elements of Y. -*/ -{ - int i; - int ix; - int iy; - int m; - double temp; - - if ( n <= 0 ) - { - } - else if ( incx == 1 && incy == 1 ) - { - m = n % 3; - - for ( i = 0; i < m; i++ ) - { - temp = x[i]; - x[i] = y[i]; - y[i] = temp; - } - - for ( i = m; i < n; i = i + 3 ) - { - temp = x[i]; - x[i] = y[i]; - y[i] = temp; - - temp = x[i+1]; - x[i+1] = y[i+1]; - y[i+1] = temp; - - temp = x[i+2]; - x[i+2] = y[i+2]; - y[i+2] = temp; - } - } - else - { - if ( 0 <= incx ) - { - ix = 0; - } - else - { - ix = ( - n + 1 ) * incx; - } - - if ( 0 <= incy ) - { - iy = 0; - } - else - { - iy = ( - n + 1 ) * incy; - } - - for ( i = 0; i < n; i++ ) - { - temp = x[ix]; - x[ix] = y[iy]; - y[iy] = temp; - ix = ix + incx; - iy = iy + incy; - } - - } - - return; -} -/******************************************************************************/ - -/******************************************************************************/ - -double *qr_solve ( int m, int n, double a[], double b[] ) - -/******************************************************************************/ -/* - Purpose: - - QR_SOLVE solves a linear system in the least squares sense. - - Discussion: - - If the matrix A has full column rank, then the solution X should be the - unique vector that minimizes the Euclidean norm of the residual. - - If the matrix A does not have full column rank, then the solution is - not unique; the vector X will minimize the residual norm, but so will - various other vectors. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 11 September 2012 - - Author: - - John Burkardt - - Reference: - - David Kahaner, Cleve Moler, Steven Nash, - Numerical Methods and Software, - Prentice Hall, 1989, - ISBN: 0-13-627258-4, - LC: TA345.K34. - - Parameters: - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, double A[M*N], the matrix. - - Input, double B[M], the right hand side. - - Output, double QR_SOLVE[N], the least squares solution. -*/ -{ - double *a_qr; - int ind; - int itask; - int *jpvt; - int kr; - int lda; - double *qraux; - double *r; - double tol; - double *x; - - a_qr = r8mat_copy_new ( m, n, a ); - lda = m; - tol = r8_epsilon ( ) / r8mat_amax ( m, n, a_qr ); - x = ( double * ) malloc ( n * sizeof ( double ) ); - jpvt = ( int * ) malloc ( n * sizeof ( int ) ); - qraux = ( double * ) malloc ( n * sizeof ( double ) ); - r = ( double * ) malloc ( m * sizeof ( double ) ); - itask = 1; - - ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); - - free ( a_qr ); - free ( jpvt ); - free ( qraux ); - free ( r ); - - return x; -} -/******************************************************************************/ - -#endif diff --git a/Firmware/qr_solve.h b/Firmware/qr_solve.h deleted file mode 100644 index a16850a6c4..0000000000 --- a/Firmware/qr_solve.h +++ /dev/null @@ -1,22 +0,0 @@ -#include "Configuration.h" - -#ifdef AUTO_BED_LEVELING_GRID - -void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy ); -double ddot ( int n, double dx[], int incx, double dy[], int incy ); -double dnrm2 ( int n, double x[], int incx ); -void dqrank ( double a[], int lda, int m, int n, double tol, int *kr, - int jpvt[], double qraux[] ); -void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[], - double work[], int job ); -int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[], - double x[], double rsd[], int jpvt[], double qraux[], int itask ); -void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[], - double rsd[], int jpvt[], double qraux[] ); -int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[], - double qy[], double qty[], double b[], double rsd[], double ab[], int job ); -void dscal ( int n, double sa, double x[], int incx ); -void dswap ( int n, double x[], int incx, double y[], int incy ); -double *qr_solve ( int m, int n, double a[], double b[] ); - -#endif diff --git a/Firmware/vector_3.cpp b/Firmware/vector_3.cpp deleted file mode 100644 index 719ca51ae3..0000000000 --- a/Firmware/vector_3.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/* - vector_3.cpp - Vector library for bed leveling - Copyright (c) 2012 Lars Brubaker. All right reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ -#include -#include "Marlin.h" - -#ifdef ENABLE_AUTO_BED_LEVELING -#include "vector_3.h" - -vector_3::vector_3() : x(0), y(0), z(0) { } - -vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { } - -vector_3 vector_3::cross(vector_3 left, vector_3 right) -{ - return vector_3(left.y * right.z - left.z * right.y, - left.z * right.x - left.x * right.z, - left.x * right.y - left.y * right.x); -} - -vector_3 vector_3::operator+(vector_3 v) -{ - return vector_3((x + v.x), (y + v.y), (z + v.z)); -} - -vector_3 vector_3::operator-(vector_3 v) -{ - return vector_3((x - v.x), (y - v.y), (z - v.z)); -} - -vector_3 vector_3::get_normal() -{ - vector_3 normalized = vector_3(x, y, z); - normalized.normalize(); - return normalized; -} - -float vector_3::get_length() -{ - float length = sqrt((x * x) + (y * y) + (z * z)); - return length; -} - -void vector_3::normalize() -{ - float length = get_length(); - x /= length; - y /= length; - z /= length; -} - -void vector_3::apply_rotation(matrix_3x3 matrix) -{ - float resultX = x * matrix.matrix[3*0+0] + y * matrix.matrix[3*1+0] + z * matrix.matrix[3*2+0]; - float resultY = x * matrix.matrix[3*0+1] + y * matrix.matrix[3*1+1] + z * matrix.matrix[3*2+1]; - float resultZ = x * matrix.matrix[3*0+2] + y * matrix.matrix[3*1+2] + z * matrix.matrix[3*2+2]; - - x = resultX; - y = resultY; - z = resultZ; -} - -void vector_3::debug(char* title) -{ - SERIAL_PROTOCOL(title); - SERIAL_PROTOCOLPGM(" x: "); - SERIAL_PROTOCOL(x); - SERIAL_PROTOCOLPGM(" y: "); - SERIAL_PROTOCOL(y); - SERIAL_PROTOCOLPGM(" z: "); - SERIAL_PROTOCOL(z); - SERIAL_PROTOCOLPGM("\n"); -} - -void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z) -{ - vector_3 vector = vector_3(x, y, z); - vector.apply_rotation(matrix); - x = vector.x; - y = vector.y; - z = vector.z; -} - -matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) -{ - //row_0.debug("row_0"); - //row_1.debug("row_1"); - //row_2.debug("row_2"); - matrix_3x3 new_matrix; - new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z; - new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z; - new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z; - //new_matrix.debug("new_matrix"); - - return new_matrix; -} - -void matrix_3x3::set_to_identity() -{ - matrix[0] = 1; matrix[1] = 0; matrix[2] = 0; - matrix[3] = 0; matrix[4] = 1; matrix[5] = 0; - matrix[6] = 0; matrix[7] = 0; matrix[8] = 1; -} - -matrix_3x3 matrix_3x3::create_look_at(vector_3 target) -{ - vector_3 z_row = target.get_normal(); - vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal(); - vector_3 y_row = vector_3::cross(z_row, x_row).get_normal(); - - // x_row.debug("x_row"); - // y_row.debug("y_row"); - // z_row.debug("z_row"); - - - // create the matrix already correctly transposed - matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); - - // rot.debug("rot"); - return rot; -} - - -matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) -{ - matrix_3x3 new_matrix; - new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6]; - new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7]; - new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8]; - return new_matrix; -} - -void matrix_3x3::debug(char* title) -{ - SERIAL_PROTOCOL(title); - SERIAL_PROTOCOL("\n"); - int count = 0; - for(int i=0; i<3; i++) - { - for(int j=0; j<3; j++) - { - SERIAL_PROTOCOL(matrix[count]); - SERIAL_PROTOCOLPGM(" "); - count++; - } - - SERIAL_PROTOCOLPGM("\n"); - } -} - -#endif // #ifdef ENABLE_AUTO_BED_LEVELING - diff --git a/Firmware/vector_3.h b/Firmware/vector_3.h deleted file mode 100644 index 7afbfefd4f..0000000000 --- a/Firmware/vector_3.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - vector_3.cpp - Vector library for bed leveling - Copyright (c) 2012 Lars Brubaker. All right reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ -#ifndef VECTOR_3_H -#define VECTOR_3_H - -#ifdef ENABLE_AUTO_BED_LEVELING -class matrix_3x3; - -struct vector_3 -{ - float x, y, z; - - vector_3(); - vector_3(float x, float y, float z); - - static vector_3 cross(vector_3 a, vector_3 b); - - vector_3 operator+(vector_3 v); - vector_3 operator-(vector_3 v); - void normalize(); - float get_length(); - vector_3 get_normal(); - - void debug(char* title); - - void apply_rotation(matrix_3x3 matrix); -}; - -struct matrix_3x3 -{ - float matrix[9]; - - static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2); - static matrix_3x3 create_look_at(vector_3 target); - static matrix_3x3 transpose(matrix_3x3 original); - - void set_to_identity(); - - void debug(char* title); -}; - - -void apply_rotation_xyz(matrix_3x3 rotationMatrix, float &x, float& y, float& z); -#endif // ENABLE_AUTO_BED_LEVELING - -#endif // VECTOR_3_H