From 238b8fd2a3f6e1be9f059e7f1ffb18361a8a7e54 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 18 Mar 2017 10:14:31 -0500 Subject: [PATCH] UBL core and support files --- Marlin/G26_Mesh_Validation_Tool.cpp | 1001 ++++++++++++++++++ Marlin/UBL.h | 331 ++++++ Marlin/UBL_Bed_Leveling.cpp | 296 ++++++ Marlin/UBL_G29.cpp | 1455 +++++++++++++++++++++++++++ Marlin/UBL_line_to_destination.cpp | 553 ++++++++++ Marlin/hex_print_routines.cpp | 47 + Marlin/hex_print_routines.h | 33 + 7 files changed, 3716 insertions(+) create mode 100644 Marlin/G26_Mesh_Validation_Tool.cpp create mode 100644 Marlin/UBL.h create mode 100644 Marlin/UBL_Bed_Leveling.cpp create mode 100644 Marlin/UBL_G29.cpp create mode 100644 Marlin/UBL_line_to_destination.cpp create mode 100644 Marlin/hex_print_routines.cpp create mode 100644 Marlin/hex_print_routines.h diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp new file mode 100644 index 0000000000..5cda27f055 --- /dev/null +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -0,0 +1,1001 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * Marlin Firmware -- G26 - Mesh Validation Tool + */ + +#define EXTRUSION_MULTIPLIER 1.0 // This is too much clutter for the main Configuration.h file But +#define RETRACTION_MULTIPLIER 1.0 // some user have expressed an interest in being able to customize +#define NOZZLE 0.3 // these numbers for thier printer so they don't need to type all +#define FILAMENT 1.75 // the options every time they do a Mesh Validation Print. +#define LAYER_HEIGHT 0.2 +#define PRIME_LENGTH 10.0 // So, we put these number in an easy to find and change place. +#define BED_TEMP 60.0 +#define HOTEND_TEMP 205.0 +#define OOOOZE_AMOUNT 0.3 + +#include "Marlin.h" +#include "Configuration.h" +#include "planner.h" +#include "stepper.h" +#include "temperature.h" +#include "UBL.h" +#include "ultralcd.h" + +#if ENABLED(AUTO_BED_LEVELING_UBL) + + #define SIZE_OF_INTERSECTION_CIRCLES 5 + #define SIZE_OF_CROSS_HAIRS 3 // cross hairs inside the circle. This number should be + // less than SIZE_OR_INTERSECTION_CIRCLES + + /** + * Roxy's G26 Mesh Validation Tool + * + * G26 Is a Mesh Validation Tool intended to provide support for the Marlin Unified Bed Leveling System. + * In order to fully utilize and benefit from the Marlin Unified Bed Leveling System an accurate Mesh must + * be defined. G29 is designed to allow the user to quickly validate the correctness of her Mesh. It will + * first heat the bed and nozzle. It will then print lines and circles along the Mesh Cell boundaries and + * the intersections of those lines (respectively). + * + * This action allows the user to immediately see where the Mesh is properly defined and where it needs to + * be edited. The command will generate the Mesh lines closest to the nozzle's starting position. Alternatively + * the user can specify the X and Y position of interest with command parameters. This allows the user to + * focus on a particular area of the Mesh where attention is needed. + * + * B # Bed Set the Bed Temperature. If not specified, a default of 60 C. will be assumed. + * + * C Current When searching for Mesh Intersection points to draw, use the current nozzle location + * as the base for any distance comparison. + * + * D Disable Disable the Unified Bed Leveling System. In the normal case the user is invoking this + * command to see how well a Mesh as been adjusted to match a print surface. In order to do + * this the Unified Bed Leveling System is turned on by the G26 command. The D parameter + * alters the command's normal behaviour and disables the Unified Bed Leveling System even if + * it is on. + * + * H # Hotend Set the Nozzle Temperature. If not specified, a default of 205 C. will be assumed. + * + * F # Filament Used to specify the diameter of the filament being used. If not specified + * 1.75mm filament is assumed. If you are not getting acceptable results by using the + * 'correct' numbers, you can scale this number up or down a little bit to change the amount + * of filament that is being extruded during the printing of the various lines on the bed. + * + * K Keep-On Keep the heaters turned on at the end of the command. + * + * L # Layer Layer height. (Height of nozzle above bed) If not specified .20mm will be used. + * + * Q # Multiplier Retraction Multiplier. Normally not needed. Retraction defaults to 1.0mm and + * un-retraction is at 1.2mm These numbers will be scaled by the specified amount + * + * N # Nozzle Used to control the size of nozzle diameter. If not specified, a .4mm nozzle is assumed. + * + * O # Ooooze How much your nozzle will Ooooze filament while getting in position to print. This + * is over kill, but using this parameter will let you get the very first 'cicle' perfect + * so you have a trophy to peel off of the bed and hang up to show how perfectly you have your + * Mesh calibrated. If not specified, a filament length of .3mm is assumed. + * + * P # Prime Prime the nozzle with specified length of filament. If this parameter is not + * given, no prime action will take place. If the parameter specifies an amount, that much + * will be purged before continuing. If no amount is specified the command will start + * purging filament until the user provides an LCD Click and then it will continue with + * printing the Mesh. You can carefully remove the spent filament with a needle nose + * pliers while holding the LCD Click wheel in a depressed state. + * + * R # Random Randomize the order that the circles are drawn on the bed. The search for the closest + * undrawn cicle is still done. But the distance to the location for each circle has a + * random number of the size specified added to it. Specifying R50 will give an interesting + * deviation from the normal behaviour on a 10 x 10 Mesh. + * + * X # X coordinate Specify the starting location of the drawing activity. + * + * Y # Y coordinate Specify the starting location of the drawing activity. + */ + + extern int UBL_has_control_of_LCD_Panel; + extern float feedrate; + //extern bool relative_mode; + extern Planner planner; + //#if ENABLED(ULTRA_LCD) + extern char lcd_status_message[]; + //#endif + extern float destination[]; + extern void set_destination_to_current(); + extern void set_current_to_destination(); + extern float code_value_float(); + extern bool code_value_bool(); + extern bool code_has_value(); + extern void lcd_init(); + #define PLANNER_XY_FEEDRATE() (min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])) //bob + bool prepare_move_to_destination_cartesian(); + void line_to_destination(); + void line_to_destination(float ); + void gcode_G28(); + void sync_plan_position_e(); + void un_retract_filament(); + void retract_filament(); + void look_for_lines_to_connect(); + bool parse_G26_parameters(); + void move_to(const float&, const float&, const float&, const float&) ; + void print_line_from_here_to_there(float sx, float sy, float sz, float ex, float ey, float ez); + bool turn_on_heaters(); + bool prime_nozzle(); + void chirp_at_user(); + + static uint16_t circle_flags[16], horizontal_mesh_line_flags[16], vertical_mesh_line_flags[16], Continue_with_closest = 0; + float G26_E_AXIS_feedrate = 0.020, + Random_Deviation = 0.0, + Layer_Height = LAYER_HEIGHT; + + bool retracted = false; // We keep track of the state of the nozzle to know if it + // is currently retracted or not. This allows us to be + // less careful because mis-matched retractions and un-retractions + // won't leave us in a bad state. + #if ENABLED(ULTRA_LCD) + void lcd_setstatus(const char* message, bool persist); + #endif + + float valid_trig_angle(float); + mesh_index_pair find_closest_circle_to_print(float, float); + void debug_current_and_destination(char *title); + void UBL_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t); + //uint16_t x_splits = 0xFFFF, uint16_t y_splits = 0xFFFF); /* needed for the old mesh_buffer_line() routine */ + + static float E_Pos_Delta, + Extrusion_Multiplier = EXTRUSION_MULTIPLIER, + Retraction_Multiplier = RETRACTION_MULTIPLIER, + Nozzle = NOZZLE, + Filament = FILAMENT, + Prime_Length = PRIME_LENGTH, + X_Pos, Y_Pos, + bed_temp = BED_TEMP, + hotend_temp = HOTEND_TEMP, + Ooooze_Amount = OOOOZE_AMOUNT; + + int8_t Prime_Flag = 0; + + bool Keep_Heaters_On = false, + G26_Debug_flag = false; + + /** + * These support functions allow the use of large bit arrays of flags that take very + * little RAM. Currently they are limited to being 16x16 in size. Changing the declaration + * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed + * in the future. + */ + void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); } + void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); } + bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); } + + /** + * G26: Mesh Validation Pattern generation. + * + * Used to interactively edit UBL's Mesh by placing the + * nozzle in a problem area and doing a G29 P4 R command. + */ + void gcode_G26() { + float circle_x, circle_y, x, y, xe, ye, tmp, + start_angle, end_angle; + int i, xi, yi, lcd_init_counter = 0; + mesh_index_pair location; + + if (axis_unhomed_error(true, true, true)) // Don't allow Mesh Validation without homing first + gcode_G28(); + + if (parse_G26_parameters()) return; // If the paramter parsing did not go OK, we abort the command + + if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) { + do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); + stepper.synchronize(); + set_current_to_destination(); + } + + if (turn_on_heaters()) // Turn on the heaters, leave the command if anything + goto LEAVE; // has gone wrong. + + axis_relative_modes[E_AXIS] = false; // Get things setup so we can take control of the + //relative_mode = false; // planner and stepper motors! + current_position[E_AXIS] = 0.0; + sync_plan_position_e(); + + if (Prime_Flag && prime_nozzle()) // if prime_nozzle() returns an error, we just bail out. + goto LEAVE; + + /** + * Bed is preheated + * + * Nozzle is at temperature + * + * Filament is primed! + * + * It's "Show Time" !!! + */ + + // Clear all of the flags we need + ZERO(circle_flags); + ZERO(horizontal_mesh_line_flags); + ZERO(vertical_mesh_line_flags); + + // + // Move nozzle to the specified height for the first layer + // + set_destination_to_current(); + destination[Z_AXIS] = Layer_Height; + move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0.0); + move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], Ooooze_Amount); + + UBL_has_control_of_LCD_Panel = 1; // Take control of the LCD Panel! + debug_current_and_destination((char *)"Starting G26 Mesh Validation Pattern."); + + do { + if (G29_lcd_clicked()) { // Check if the user wants to stop the Mesh Validation + strcpy(lcd_status_message, "Mesh Validation Stopped."); // We can't do lcd_setstatus() without having it continue; + while (G29_lcd_clicked()) idle(); // Debounce the switch click + #if ENABLED(ULTRA_LCD) + lcd_setstatus("Mesh Validation Stopped.", true); + lcd_quick_feedback(); + #endif + goto LEAVE; + } + + if (Continue_with_closest) + location = find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS]); + else + location = find_closest_circle_to_print(X_Pos, Y_Pos); // Find the closest Mesh Intersection to where we are now. + + if (location.x_index >= 0 && location.y_index >= 0) { + circle_x = blm.map_x_index_to_bed_location(location.x_index); + circle_y = blm.map_y_index_to_bed_location(location.y_index); + + // Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem + #ifdef DELTA + if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) { + SERIAL_PROTOCOLLNPGM("?Error: Attempt to print outside of DELTA_PRINTABLE_RADIUS."); + goto LEAVE; + } + #endif + + if (circle_x < X_MIN_POS || circle_x > X_MAX_POS || circle_y < Y_MIN_POS || circle_y > Y_MAX_POS) { + SERIAL_PROTOCOLLNPGM("?Error: Attempt to print off the bed."); + goto LEAVE; + } + + xi = location.x_index; // Just to shrink the next few lines and make them easier to understand + yi = location.y_index; + + if (G26_Debug_flag) { + SERIAL_ECHOPGM(" Doing circle at: (xi="); + SERIAL_ECHO(xi); + SERIAL_ECHOPGM(", yi="); + SERIAL_ECHO(yi); + SERIAL_ECHOLNPGM(")"); + } + + start_angle = 0.0; // assume it is going to be a full circle + end_angle = 360.0; + if (xi == 0) { // Check for bottom edge + start_angle = -90.0; + end_angle = 90.0; + if (yi == 0) // it is an edge, check for the two left corners + start_angle = 0.0; + else if (yi == UBL_MESH_NUM_Y_POINTS - 1) + end_angle = 0.0; + } + else if (xi == UBL_MESH_NUM_X_POINTS - 1) { // Check for top edge + start_angle = 90.0; + end_angle = 270.0; + if (yi == 0) // it is an edge, check for the two right corners + end_angle = 180.0; + else if (yi == UBL_MESH_NUM_Y_POINTS - 1) + start_angle = 180.0; + } + else if (yi == 0) { + start_angle = 0.0; // only do the top side of the cirlce + end_angle = 180.0; + } + else if (yi == UBL_MESH_NUM_Y_POINTS - 1) { + start_angle = 180.0; // only do the bottom side of the cirlce + end_angle = 360.0; + } + + /** + * Declare and generate a sin() & cos() table to be used during the circle drawing. This will lighten + * the CPU load and make the arc drawing faster and more smooth + */ + float sin_table[360 / 30 + 1], cos_table[360 / 30 + 1]; + int tmp_div_30; + for (i = 0; i <= 360 / 30; i++) { + cos_table[i] = SIZE_OF_INTERSECTION_CIRCLES * cos(RADIANS(valid_trig_angle(i * 30.0))); + sin_table[i] = SIZE_OF_INTERSECTION_CIRCLES * sin(RADIANS(valid_trig_angle(i * 30.0))); + } + + for (tmp = start_angle; tmp < end_angle - 0.1; tmp += 30.0) { + tmp_div_30 = tmp / 30.0; + if (tmp_div_30 < 0) tmp_div_30 += 360 / 30; + + x = circle_x + cos_table[tmp_div_30]; // for speed, these are now a lookup table entry + y = circle_y + sin_table[tmp_div_30]; + + if (tmp_div_30 > 11) tmp_div_30 -= 360 / 30; + xe = circle_x + cos_table[tmp_div_30 + 1]; // for speed, these are now a lookup table entry + ye = circle_y + sin_table[tmp_div_30 + 1]; + #ifdef DELTA + if (HYPOT2(x, y) > sq(DELTA_PRINTABLE_RADIUS)) // Check to make sure this part of + continue; // the 'circle' is on the bed. If + #else // not, we need to skip + x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops + y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1); + xe = constrain(xe, X_MIN_POS + 1, X_MAX_POS - 1); + ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1); + #endif + + if (G26_Debug_flag) { + char ccc, *cptr, seg_msg[50], seg_num[10]; + strcpy(seg_msg, " segment: "); + strcpy(seg_num, " \n"); + cptr = (char *) "01234567890ABCDEF????????"; + ccc = cptr[tmp_div_30]; + seg_num[1] = ccc; + strcat(seg_msg, seg_num); + debug_current_and_destination(seg_msg); + } + + print_line_from_here_to_there(x, y, Layer_Height, xe, ye, Layer_Height); + } + lcd_init_counter++; + if (lcd_init_counter > 10) { + lcd_init_counter = 0; + lcd_init(); // Some people's LCD Displays are locking up. This might help them + } + + debug_current_and_destination((char *)"Looking for lines to connect."); + look_for_lines_to_connect(); + debug_current_and_destination((char *)"Done with line connect."); + } + + debug_current_and_destination((char *)"Done with current circle."); + + } + while (location.x_index >= 0 && location.y_index >= 0) ; + + LEAVE: + + retract_filament(); + destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Raise the nozzle + + debug_current_and_destination((char *)"ready to do Z-Raise."); + move_to( destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle + debug_current_and_destination((char *)"done doing Z-Raise."); + + destination[X_AXIS] = X_Pos; // Move back to the starting position + destination[Y_AXIS] = Y_Pos; + destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is + + move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position + debug_current_and_destination((char *)"done doing X/Y move."); + + UBL_has_control_of_LCD_Panel = 0; // Give back control of the LCD Panel! + + if (!Keep_Heaters_On) { + #if HAS_TEMP_BED + thermalManager.setTargetBed(0.0); + #endif + thermalManager.setTargetHotend(0.0, 0); + } + lcd_init(); // Some people's LCD Displays are locking up. This might help them + } + + + float valid_trig_angle(float d) { + while (d > 360.0) d -= 360.0; + while (d < 0.0) d += 360.0; + return d; + } + + mesh_index_pair find_closest_circle_to_print( float X, float Y) { + float f, mx, my, dx, dy, closest = 99999.99; + mesh_index_pair return_val; + + return_val.x_index = return_val.y_index = -1; + + for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (uint8_t j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + if (!is_bit_set(circle_flags, i, j)) { + mx = blm.map_x_index_to_bed_location(i); // We found a circle that needs to be printed + my = blm.map_y_index_to_bed_location(j); + + dx = X - mx; // Get the distance to this intersection + dy = Y - my; + f = HYPOT(dx, dy); + + dx = X_Pos - mx; // It is possible that we are being called with the values + dy = Y_Pos - my; // to let us find the closest circle to the start position. + f += HYPOT(dx, dy) / 15.0; // But if this is not the case, + // we are going to add in a small + // weighting to the distance calculation to help it choose + // a better place to continue. + + if (Random_Deviation > 1.0) + f += random(0.0, Random_Deviation); // Add in the specified amount of Random Noise to our search + + if (f < closest) { + closest = f; // We found a closer location that is still + return_val.x_index = i; // un-printed --- save the data for it + return_val.y_index = j; + return_val.distance= closest; + } + } + } + } + bit_set(circle_flags, return_val.x_index, return_val.y_index); // Mark this location as done. + return return_val; + } + + void look_for_lines_to_connect() { + float sx, sy, ex, ey; + + for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (uint8_t j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + + if (i < UBL_MESH_NUM_X_POINTS) { // We can't connect to anything to the right than UBL_MESH_NUM_X_POINTS. + // This is already a half circle because we are at the edge of the bed. + + if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i + 1, j)) { // check if we can do a line to the left + if (!is_bit_set(horizontal_mesh_line_flags, i, j)) { + + // + // We found two circles that need a horizontal line to connect them + // Print it! + // + sx = blm.map_x_index_to_bed_location(i); + sx = sx + SIZE_OF_INTERSECTION_CIRCLES - SIZE_OF_CROSS_HAIRS; // get the right edge of the circle + sy = blm.map_y_index_to_bed_location(j); + + ex = blm.map_x_index_to_bed_location(i + 1); + ex = ex - SIZE_OF_INTERSECTION_CIRCLES + SIZE_OF_CROSS_HAIRS; // get the left edge of the circle + ey = sy; + + sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops + sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); + ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); + ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); + + if (G26_Debug_flag) { + SERIAL_ECHOPGM(" Connecting with horizontal line (sx="); + SERIAL_ECHO(sx); + SERIAL_ECHOPGM(", sy="); + SERIAL_ECHO(sy); + SERIAL_ECHOPGM(") -> (ex="); + SERIAL_ECHO(ex); + SERIAL_ECHOPGM(", ey="); + SERIAL_ECHO(ey); + SERIAL_ECHOLNPGM(")"); + debug_current_and_destination((char *)"Connecting horizontal line."); + } + + print_line_from_here_to_there(sx, sy, Layer_Height, ex, ey, Layer_Height); + bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again + } + } + + if (j < UBL_MESH_NUM_Y_POINTS) { // We can't connect to anything further back than UBL_MESH_NUM_Y_POINTS. + // This is already a half circle because we are at the edge of the bed. + + if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i, j + 1)) { // check if we can do a line straight down + if (!is_bit_set( vertical_mesh_line_flags, i, j)) { + // + // We found two circles that need a vertical line to connect them + // Print it! + // + sx = blm.map_x_index_to_bed_location(i); + sy = blm.map_y_index_to_bed_location(j); + sy = sy + SIZE_OF_INTERSECTION_CIRCLES - SIZE_OF_CROSS_HAIRS; // get the top edge of the circle + + ex = sx; + ey = blm.map_y_index_to_bed_location(j + 1); + ey = ey - SIZE_OF_INTERSECTION_CIRCLES + SIZE_OF_CROSS_HAIRS; // get the bottom edge of the circle + + sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops + sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); + ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); + ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); + + if (G26_Debug_flag) { + SERIAL_ECHOPGM(" Connecting with vertical line (sx="); + SERIAL_ECHO(sx); + SERIAL_ECHOPGM(", sy="); + SERIAL_ECHO(sy); + SERIAL_ECHOPGM(") -> (ex="); + SERIAL_ECHO(ex); + SERIAL_ECHOPGM(", ey="); + SERIAL_ECHO(ey); + SERIAL_ECHOLNPGM(")"); + debug_current_and_destination((char *)"Connecting vertical line."); + } + print_line_from_here_to_there(sx, sy, Layer_Height, ex, ey, Layer_Height); + bit_set( vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again + } + } + } + } + } + } + } + + void debug_current_and_destination(char *title) { + float dx, dy, de, xy_dist, fpmm; + + // if the title message starts with a '!' it is so important, we are going to + // ignore the status of the G26_Debug_Flag + if (*title != '!' && !G26_Debug_flag) return; + + dx = current_position[X_AXIS] - destination[X_AXIS]; + dy = current_position[Y_AXIS] - destination[Y_AXIS]; + de = destination[E_AXIS] - current_position[E_AXIS]; + if (de == 0.0) return; + + xy_dist = HYPOT(dx, dy); + if (xy_dist == 0.0) { + return; + //SERIAL_ECHOPGM(" FPMM="); + //fpmm = de; + //SERIAL_PROTOCOL_F(fpmm, 6); + } + else { + SERIAL_ECHOPGM(" fpmm="); + fpmm = de / xy_dist; + SERIAL_PROTOCOL_F(fpmm, 6); + } + + SERIAL_ECHOPGM(" current=( "); + SERIAL_PROTOCOL_F(current_position[X_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_PROTOCOL_F(current_position[Y_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_PROTOCOL_F(current_position[E_AXIS], 6); + SERIAL_ECHOPGM(" ) destination=( "); + if (current_position[X_AXIS] == destination[X_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_PROTOCOL_F(destination[X_AXIS], 6); + + SERIAL_ECHOPGM(", "); + + if (current_position[Y_AXIS] == destination[Y_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_PROTOCOL_F(destination[Y_AXIS], 6); + + SERIAL_ECHOPGM(", "); + + if (current_position[Z_AXIS] == destination[Z_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_PROTOCOL_F(destination[Z_AXIS], 6); + + SERIAL_ECHOPGM(", "); + + if (current_position[E_AXIS] == destination[E_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_PROTOCOL_F(destination[E_AXIS], 6); + + SERIAL_ECHOPGM(" ) "); + SERIAL_ECHO(title); + SERIAL_EOL; + + SET_INPUT_PULLUP(66); // Roxy's Left Switch is on pin 66. Right Switch is on pin 65 + + //if (been_to_2_6) { + //while ((digitalRead(66) & 0x01) != 0) + // idle(); + //} + } + + void move_to(const float &x, const float &y, const float &z, const float &e_delta) { + float feed_value; + static float last_z = -999.99; + + bool has_XY_component = (x != current_position[X_AXIS] || y != current_position[Y_AXIS]); // Check if X or Y is involved in the movement. + + if (G26_Debug_flag) { + SERIAL_ECHOPAIR("in move_to() has_XY_component:", (int)has_XY_component); + SERIAL_EOL; + } + + if (z != last_z) { + + if (G26_Debug_flag) { + SERIAL_ECHOPAIR("in move_to() changing Z to ", (int)z); + SERIAL_EOL; + } + last_z = z; + feed_value = planner.max_feedrate_mm_s[Z_AXIS]/(3.0); // Base the feed rate off of the configured Z_AXIS feed rate + + destination[X_AXIS] = current_position[X_AXIS]; + destination[Y_AXIS] = current_position[Y_AXIS]; + destination[Z_AXIS] = z; // We know the last_z==z or we wouldn't be in this block of code. + destination[E_AXIS] = current_position[E_AXIS]; + + UBL_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0); + + stepper.synchronize(); + set_destination_to_current(); + + if (G26_Debug_flag) + debug_current_and_destination((char *)" in move_to() done with Z move"); + } + + // Check if X or Y is involved in the movement. + // Yes: a 'normal' movement. No: a retract() or un_retract() + feed_value = has_XY_component ? PLANNER_XY_FEEDRATE() / 10.0 : planner.max_feedrate_mm_s[E_AXIS] / 1.5; + + if (G26_Debug_flag) { + SERIAL_ECHOPAIR("in move_to() feed_value for XY:", feed_value); + SERIAL_EOL; + } + + destination[X_AXIS] = x; + destination[Y_AXIS] = y; + destination[E_AXIS] += e_delta; + + if (G26_Debug_flag) + debug_current_and_destination((char *)" in move_to() doing last move"); + + UBL_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0); + + if (G26_Debug_flag) + debug_current_and_destination((char *)" in move_to() after last move"); + + stepper.synchronize(); + set_destination_to_current(); + } + + void retract_filament() { + if (!retracted) { // Only retract if we are not already retracted! + retracted = true; + if (G26_Debug_flag) SERIAL_ECHOLNPGM(" Decided to do retract."); + move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], -1.0 * Retraction_Multiplier); + if (G26_Debug_flag) SERIAL_ECHOLNPGM(" Retraction done."); + } + } + + void un_retract_filament() { + if (retracted) { // Only un-retract if we are retracted. + move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 1.2 * Retraction_Multiplier); + retracted = false; + if (G26_Debug_flag) SERIAL_ECHOLNPGM(" unretract done."); + } + } + + /** + * print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one + * to the other. But there are really three sets of coordinates involved. The first coordinate + * is the present location of the nozzle. We don't necessarily want to print from this location. + * We first need to move the nozzle to the start of line segment where we want to print. Once + * there, we can use the two coordinates supplied to draw the line. + * + * Note: Although we assume the first set of coordinates is the start of the line and the second + * set of coordinates is the end of the line, it does not always work out that way. This function + * optimizes the movement to minimize the travel distance before it can start printing. This saves + * a lot of time and eleminates a lot of non-sensical movement of the nozzle. However, it does + * cause a lot of very little short retracement of th nozzle when it draws the very first line + * segment of a 'circle'. The time this requires is very short and is easily saved by the other + * cases where the optimization comes into play. + */ + void print_line_from_here_to_there( float sx, float sy, float sz, float ex, float ey, float ez) { + float dx, dy, dx_s, dy_s, dx_e, dy_e, dist_start, dist_end, Line_Length; + + dx_s = current_position[X_AXIS] - sx; // find our distance from the start of the actual line segment + dy_s = current_position[Y_AXIS] - sy; + dist_start = HYPOT2(dx_s, dy_s); // We don't need to do a sqrt(), we can compare the distance^2 + // to save computation time + dx_e = current_position[X_AXIS] - ex; // find our distance from the end of the actual line segment + dy_e = current_position[Y_AXIS] - ey; + dist_end = HYPOT2(dx_e, dy_e); + + dx = ex - sx; + dy = ey - sy; + Line_Length = HYPOT(dx, dy); + + // If the end point of the line is closer to the nozzle, we are going to + // flip the direction of this line. We will print it from the end to the start. + // On very small lines we don't do the optimization because it just isn't worth it. + // + if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(Line_Length)) { + if (G26_Debug_flag) + SERIAL_ECHOLNPGM(" Reversing start and end of print_line_from_here_to_there()"); + print_line_from_here_to_there(ex, ey, ez, sx, sy, sz); + return; + } + + // Now decide if we should retract. + + if (dist_start > 2.0) { + retract_filament(); + if (G26_Debug_flag) + SERIAL_ECHOLNPGM(" filament retracted."); + } + move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion + + E_Pos_Delta = Line_Length * G26_E_AXIS_feedrate * Extrusion_Multiplier; + + un_retract_filament(); + if (G26_Debug_flag) { + SERIAL_ECHOLNPGM(" doing printing move."); + debug_current_and_destination((char *)"doing final move_to() inside print_line_from_here_to_there()"); + } + move_to(ex, ey, ez, E_Pos_Delta); // Get to the ending point with an appropriate amount of extrusion + } + + /** + * This function used to be inline code in G26. But there are so many + * parameters it made sense to turn them into static globals and get + * this code out of sight of the main routine. + */ + bool parse_G26_parameters() { + + Extrusion_Multiplier = EXTRUSION_MULTIPLIER; + Retraction_Multiplier = RETRACTION_MULTIPLIER; + Nozzle = NOZZLE; + Filament = FILAMENT; + Layer_Height = LAYER_HEIGHT; + Prime_Length = PRIME_LENGTH; + bed_temp = BED_TEMP; + hotend_temp = HOTEND_TEMP; + Ooooze_Amount = OOOOZE_AMOUNT; + Prime_Flag = 0; + Keep_Heaters_On = false; + + if (code_seen('B')) { + bed_temp = code_value_float(); + if (bed_temp < 15.0 || bed_temp > 140.0) { + SERIAL_PROTOCOLLNPGM("?Specified bed temperature not plausible."); + return UBL_ERR; + } + } + + if (code_seen('C')) Continue_with_closest++; + + if (code_seen('L')) { + Layer_Height = code_value_float(); + if (Layer_Height<0.0 || Layer_Height>2.0) { + SERIAL_PROTOCOLLNPGM("?Specified layer height not plausible."); + return UBL_ERR; + } + } + + if (code_seen('Q')) { + if (code_has_value()) { + Retraction_Multiplier = code_value_float(); + if (Retraction_Multiplier<.05 || Retraction_Multiplier>15.0) { + SERIAL_PROTOCOLLNPGM("?Specified Retraction Multiplier not plausible."); + return UBL_ERR; + } + } + else { + SERIAL_PROTOCOLLNPGM("?Retraction Multiplier must be specified."); + return UBL_ERR; + } + } + + if (code_seen('N')) { + Nozzle = code_value_float(); + if (Nozzle < 0.1 || Nozzle > 1.0) { + SERIAL_PROTOCOLLNPGM("?Specified nozzle size not plausible."); + return UBL_ERR; + } + } + + if (code_seen('K')) Keep_Heaters_On++; + + if (code_seen('O') && code_has_value()) + Ooooze_Amount = code_value_float(); + + if (code_seen('P')) { + if (!code_has_value()) + Prime_Flag = -1; + else { + Prime_Flag++; + Prime_Length = code_value_float(); + if (Prime_Length < 0.0 || Prime_Length > 25.0) { + SERIAL_PROTOCOLLNPGM("?Specified prime length not plausible."); + return UBL_ERR; + } + } + } + + if (code_seen('F')) { + Filament = code_value_float(); + if (Filament < 1.0 || Filament > 4.0) { + SERIAL_PROTOCOLLNPGM("?Specified filament size not plausible."); + return UBL_ERR; + } + } + Extrusion_Multiplier *= sq(1.75) / sq(Filament); // If we aren't using 1.75mm filament, we need to + // scale up or down the length needed to get the + // same volume of filament + Extrusion_Multiplier *= Filament * sq(Nozzle) / sq(0.3); // Scale up by nozzle size + + if (code_seen('H')) { + hotend_temp = code_value_float(); + if (hotend_temp < 165.0 || hotend_temp > 280.0) { + SERIAL_PROTOCOLLNPGM("?Specified nozzle temperature not plausible."); + return UBL_ERR; + } + } + + if (code_seen('R')) { + randomSeed(millis()); + Random_Deviation = code_has_value() ? code_value_float() : 50.0; + } + + X_Pos = current_position[X_AXIS]; + Y_Pos = current_position[Y_AXIS]; + + if (code_seen('X')) { + X_Pos = code_value_float(); + if (X_Pos < X_MIN_POS || X_Pos > X_MAX_POS) { + SERIAL_PROTOCOLLNPGM("?Specified X coordinate not plausible."); + return UBL_ERR; + } + } + else + + if (code_seen('Y')) { + Y_Pos = code_value_float(); + if (Y_Pos < Y_MIN_POS || Y_Pos > Y_MAX_POS) { + SERIAL_PROTOCOLLNPGM("?Specified Y coordinate not plausible."); + return UBL_ERR; + } + } + + /** + * We save the question of what to do with the Unified Bed Leveling System's Activation until the very + * end. The reason is, if one of the parameters specified up above is incorrect, we don't want to + * alter the system's status. We wait until we know everything is correct before altering the state + * of the system. + */ + blm.state.active = !code_seen('D'); + + return UBL_OK; + } + + /** + * Turn on the bed and nozzle heat and + * wait for them to get up to temperature. + */ + bool turn_on_heaters() { + #if HAS_TEMP_BED + #if ENABLED(ULTRA_LCD) + if (bed_temp > 25) { + lcd_setstatus("G26 Heating Bed.", true); + lcd_quick_feedback(); + #endif + UBL_has_control_of_LCD_Panel++; + thermalManager.setTargetBed(bed_temp); + while (abs(thermalManager.degBed() - bed_temp) > 3) { + if (G29_lcd_clicked()) { + strcpy(lcd_status_message, "Leaving G26"); // We can't do lcd_setstatus() without having it continue; + while (G29_lcd_clicked()) idle(); // Debounce the switch + lcd_setstatus("Leaving G26", true); // Now we do it right. + return UBL_ERR; + } + idle(); + } + #if ENABLED(ULTRA_LCD) + } + lcd_setstatus("G26 Heating Nozzle.", true); + lcd_quick_feedback(); + #endif + #endif + + // Start heating the nozzle and wait for it to reach temperature. + thermalManager.setTargetHotend(hotend_temp, 0); + while (abs(thermalManager.degHotend(0) - hotend_temp) > 3) { + if (G29_lcd_clicked()) { + strcpy(lcd_status_message, "Leaving G26"); // We can't do lcd_setstatus() without having it continue; + while (G29_lcd_clicked()) idle(); // Debounce the switch + lcd_setstatus("Leaving G26", true); // Now we do it right. + return UBL_ERR; + } + idle(); + } + + #if ENABLED(ULTRA_LCD) + lcd_setstatus("", true); + lcd_quick_feedback(); + #endif + return UBL_OK; + } + + /** + * Prime the nozzle if needed. Return true on error. + */ + bool prime_nozzle() { + float Total_Prime = 0.0; + + if (Prime_Flag == -1) { // The user wants to control how much filament gets purged + lcd_setstatus("User Controled Prime", true); + chirp_at_user(); + + set_destination_to_current(); + + un_retract_filament(); // Lets make sure the G26 command doesn't think the filament is + // retracted(). We are here because we want to prime the nozzle. + // So let's just unretract just to be sure. + + UBL_has_control_of_LCD_Panel++; + while (!G29_lcd_clicked()) { + chirp_at_user(); + destination[E_AXIS] += 0.25; + #ifdef PREVENT_LENGTHY_EXTRUDE + Total_Prime += 0.25; + if (Total_Prime >= EXTRUDE_MAXLENGTH) return UBL_ERR; + #endif + UBL_line_to_destination( + destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], + //planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0, 0xFFFF, 0xFFFF); + planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0 + ); + + stepper.synchronize(); // Without this synchronize, the purge is more consistent, + // but because the planner has a buffer, we won't be able + // to stop as quickly. So we put up with the less smooth + // action to give the user a more responsive 'Stop'. + set_destination_to_current(); + idle(); + } + + strcpy(lcd_status_message, "Done Priming"); // We can't do lcd_setstatus() without having it continue; + // So... We cheat to get a message up. + + while (G29_lcd_clicked()) idle(); // Debounce the switch + + #if ENABLED(ULTRA_LCD) + UBL_has_control_of_LCD_Panel = 0; + lcd_setstatus("Done Priming", true); // Now we do it right. + lcd_quick_feedback(); + #endif + } + else { + #if ENABLED(ULTRA_LCD) + lcd_setstatus("Fixed Length Prime.", true); + lcd_quick_feedback(); + #endif + set_destination_to_current(); + destination[E_AXIS] += Prime_Length; + UBL_line_to_destination( + destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], + //planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0, 0xFFFF, 0xFFFF); + planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0 + ); + stepper.synchronize(); + set_destination_to_current(); + retract_filament(); + } + return UBL_OK; + } + +#endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/UBL.h b/Marlin/UBL.h new file mode 100644 index 0000000000..2c26276288 --- /dev/null +++ b/Marlin/UBL.h @@ -0,0 +1,331 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "Marlin.h" +#include "math.h" + +#ifndef UNIFIED_BED_LEVELING_H +#define UNIFIED_BED_LEVELING_H + + #if ENABLED(AUTO_BED_LEVELING_UBL) + + #define UBL_OK false + #define UBL_ERR true + + typedef struct { + int x_index, y_index; + float distance; // Not always used. But when populated, it is the distance + // from the search location + } mesh_index_pair; + + struct vector { double dx, dy, dz; }; + + enum Mesh_Point_Type { INVALID, REAL, SET_IN_BITMAP }; + + bool axis_unhomed_error(bool, bool, bool); + void dump(char *str, float f); + bool G29_lcd_clicked(); + void probe_entire_mesh(float, float, bool, bool); + void UBL_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t); + void manually_probe_remaining_mesh(float, float, float, float, bool); + struct vector tilt_mesh_based_on_3pts(float, float, float); + void new_set_bed_level_equation_3pts(float, float, float); + float measure_business_card_thickness(float); + mesh_index_pair find_closest_mesh_point_of_type(Mesh_Point_Type, float, float, bool, unsigned int[16]); + void Find_Mean_Mesh_Height(); + void Shift_Mesh_Height(); + bool G29_Parameter_Parsing(); + void G29_What_Command(); + void G29_EEPROM_Dump(); + void G29_Kompare_Current_Mesh_to_Stored_Mesh(); + void fine_tune_mesh(float, float, float, bool); + void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y); + void bit_set(uint16_t bits[16], uint8_t x, uint8_t y); + bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y); + char *ftostr43sign(const float&, char); + + void gcode_G26(); + void gcode_G28(); + void gcode_G29(); + extern char conv[9]; + + void save_UBL_active_state_and_disable(); + void restore_UBL_active_state_and_leave(); + + /////////////////////////////////////////////////////////////////////////////////////////////////////// + + #if ENABLED(ULTRA_LCD) + extern char lcd_status_message[]; + void lcd_quick_feedback(); + #endif + + enum MBLStatus { MBL_STATUS_NONE = 0, MBL_STATUS_HAS_MESH_BIT = 0, MBL_STATUS_ACTIVE_BIT = 1 }; + + #define MESH_X_DIST ((float(UBL_MESH_MAX_X) - float(UBL_MESH_MIN_X)) / (float(UBL_MESH_NUM_X_POINTS) - 1.0)) + #define MESH_Y_DIST ((float(UBL_MESH_MAX_Y) - float(UBL_MESH_MIN_Y)) / (float(UBL_MESH_NUM_Y_POINTS) - 1.0)) + + extern bool G26_Debug_flag; + extern float last_specified_z; + extern float fade_scaling_factor_for_current_height; + extern float z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS]; + extern float mesh_index_to_X_location[UBL_MESH_NUM_X_POINTS + 1]; // +1 just because of paranoia that we might end up on the + extern float mesh_index_to_Y_location[UBL_MESH_NUM_Y_POINTS + 1]; // the last Mesh Line and that is the start of a whole new cell + + class bed_leveling { + public: + struct ubl_state { + bool active = false; + float z_offset = 0.0; + int EEPROM_storage_slot = -1, + n_x = UBL_MESH_NUM_X_POINTS, + n_y = UBL_MESH_NUM_Y_POINTS; + float mesh_x_min = UBL_MESH_MIN_X, + mesh_y_min = UBL_MESH_MIN_Y, + mesh_x_max = UBL_MESH_MAX_X, + mesh_y_max = UBL_MESH_MAX_Y, + mesh_x_dist = MESH_X_DIST, + mesh_y_dist = MESH_Y_DIST, + G29_Correction_Fade_Height = 10.0, + G29_Fade_Height_Multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating + // point divide. So, we keep this number in both forms. The first + // is for the user. The second one is the one that is actually used + // again and again and again during the correction calculations. + + unsigned char padding[24]; // This is just to allow room to add state variables without + // changing the location of data structures in the EEPROM. + // This is for compatability with future versions to keep + // people from having to regenerate thier mesh data. + // + // If you change the contents of this struct, please adjust + // the padding[] to keep the size the same! + } state, pre_initialized; + + bed_leveling(); + // ~bed_leveling(); // No destructor because this object never goes away! + + void display_map(int); + + void reset(); + void invalidate(); + + void store_state(); + void load_state(); + void store_mesh(int); + void load_mesh(int); + + bool sanity_check(); + + FORCE_INLINE float map_x_index_to_bed_location(int8_t i){ return ((float) UBL_MESH_MIN_X) + (((float) MESH_X_DIST) * (float) i); }; + FORCE_INLINE float map_y_index_to_bed_location(int8_t i){ return ((float) UBL_MESH_MIN_Y) + (((float) MESH_Y_DIST) * (float) i); }; + + void set_z(const int8_t px, const int8_t py, const float z) { z_values[px][py] = z; } + + int8_t get_cell_index_x(float x) { + int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); + return constrain(cx, 0, (UBL_MESH_NUM_X_POINTS) - 1); // -1 is appropriate if we want all movement to the X_MAX + } // position. But with this defined this way, it is possible + // to extrapolate off of this point even further out. Probably + // that is OK because something else should be keeping that from + // happening and should not be worried about at this level. + int8_t get_cell_index_y(float y) { + int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); + return constrain(cy, 0, (UBL_MESH_NUM_Y_POINTS) - 1); // -1 is appropriate if we want all movement to the Y_MAX + } // position. But with this defined this way, it is possible + // to extrapolate off of this point even further out. Probably + // that is OK because something else should be keeping that from + // happening and should not be worried about at this level. + + int8_t find_closest_x_index(float x) { + int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST)); + return (px >= 0 && px < (UBL_MESH_NUM_X_POINTS)) ? px : -1; + } + + int8_t find_closest_y_index(float y) { + int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST)); + return (py >= 0 && py < (UBL_MESH_NUM_Y_POINTS)) ? py : -1; + } + + /** + * z2 --| + * z0 | | + * | | + (z2-z1) + * z1 | | | + * ---+-------------+--------+-- --| + * a1 a0 a2 + * |<---delta_a---------->| + * + * calc_z0 is the basis for all the Mesh Based correction. It is used to + * find the expected Z Height at a position between two known Z-Height locations + * + * It is farly expensive with its 4 floating point additions and 2 floating point + * multiplications. + */ + inline float calc_z0(float a0, float a1, float z1, float a2, float z2) { + float delta_z = (z2 - z1); + float delta_a = (a0 - a1) / (a2 - a1); + return z1 + delta_a * delta_z; + } + + /** + * get_z_correction_at_Y_intercept(float x0, int x1_i, int yi) only takes + * three parameters. It assumes the x0 point is on a Mesh line denoted by yi. In theory + * we could use get_cell_index_x(float x) to obtain the 2nd parameter x1_i but any code calling + * the get_z_correction_along_vertical_mesh_line_at_specific_X routine will already have + * the X index of the x0 intersection available and we don't want to perform any extra floating + * point operations. + */ + inline float get_z_correction_along_horizontal_mesh_line_at_specific_X(float x0, int x1_i, int yi) { + if (x1_i < 0 || yi < 0 || x1_i >= UBL_MESH_NUM_X_POINTS || yi >= UBL_MESH_NUM_Y_POINTS) { + SERIAL_ECHOPAIR("? in get_z_correction_along_horizontal_mesh_line_at_specific_X(x0=", x0); + SERIAL_ECHOPAIR(",x1_i=", x1_i); + SERIAL_ECHOPAIR(",yi=", yi); + SERIAL_CHAR(')'); + SERIAL_EOL; + return NAN; + } + + const float a0ma1diva2ma1 = (x0 - mesh_index_to_X_location[x1_i]) * (1.0 / (MESH_X_DIST)), + z1 = z_values[x1_i][yi], + z2 = z_values[x1_i + 1][yi], + dz = (z2 - z1); + + return z1 + a0ma1diva2ma1 * dz; + } + + // + // See comments above for get_z_correction_along_horizontal_mesh_line_at_specific_X + // + inline float get_z_correction_along_vertical_mesh_line_at_specific_Y(float y0, int xi, int y1_i) { + if (xi < 0 || y1_i < 0 || xi >= UBL_MESH_NUM_X_POINTS || y1_i >= UBL_MESH_NUM_Y_POINTS) { + SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_X(y0=", y0); + SERIAL_ECHOPAIR(", x1_i=", xi); + SERIAL_ECHOPAIR(", yi=", y1_i); + SERIAL_CHAR(')'); + SERIAL_EOL; + return NAN; + } + + const float a0ma1diva2ma1 = (y0 - mesh_index_to_Y_location[y1_i]) * (1.0 / (MESH_Y_DIST)), + z1 = z_values[xi][y1_i], + z2 = z_values[xi][y1_i + 1], + dz = (z2 - z1); + + return z1 + a0ma1diva2ma1 * dz; + } + + /** + * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first + * does a linear interpolation along both of the bounding X-Mesh-Lines to find the + * Z-Height at both ends. Then it does a linear interpolation of these heights based + * on the Y position within the cell. + */ + float get_z_correction(float x0, float y0) { + int8_t cx = get_cell_index_x(x0), + cy = get_cell_index_y(y0); + + if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) { + + SERIAL_ECHOPAIR("? in get_z_correction(x0=", x0); + SERIAL_ECHOPAIR(", y0=", y0); + SERIAL_CHAR(')'); + SERIAL_EOL; + + #if ENABLED(ULTRA_LCD) + strcpy(lcd_status_message, "get_z_correction() indexes out of range."); + lcd_quick_feedback(); + #endif + return 0.0; // this used to return state.z_offset + } + + float z1 = calc_z0(x0, + map_x_index_to_bed_location(cx), z_values[cx][cy], + map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]); + float z2 = calc_z0(x0, + map_x_index_to_bed_location(cx), z_values[cx][cy + 1], + map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]); + float z0 = calc_z0(y0, + map_y_index_to_bed_location(cy), z1, + map_y_index_to_bed_location(cy + 1), z2); + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPAIR(" raw get_z_correction(", x0); + SERIAL_ECHOPAIR(",", y0); + SERIAL_ECHOPGM(")="); + SERIAL_PROTOCOL_F(z0, 6); + } + #endif + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPGM(" >>>---> "); + SERIAL_PROTOCOL_F(z0, 6); + SERIAL_EOL; + } + #endif + + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in blm.z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPGM("??? Yikes! NAN in get_z_correction( "); + SERIAL_ECHO(x0); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO(y0); + SERIAL_ECHOLNPGM(" )"); + } + #endif + } + return z0; // there used to be a +state.z_offset on this line + } + + /** + * This routine is used to scale the Z correction depending upon the current nozzle height. It is + * optimized for speed. It avoids floating point operations by checking if the requested scaling + * factor is going to be the same as the last time the function calculated a value. If so, it just + * returns it. + * + * If it must do a calcuation, it will return a scaling factor of 0.0 if the UBL System is not active + * or if the current Z Height is past the specified 'Fade Height' + */ + FORCE_INLINE float fade_scaling_factor_for_Z(float current_z) { + if (last_specified_z == current_z) + return fade_scaling_factor_for_current_height; + + last_specified_z = current_z; + fade_scaling_factor_for_current_height = + state.active && current_z < state.G29_Correction_Fade_Height + ? 1.0 - (current_z * state.G29_Fade_Height_Multiplier) + : 0.0; + return fade_scaling_factor_for_current_height; + } + }; + + extern bed_leveling blm; + extern int Unified_Bed_Leveling_EEPROM_start; + +#endif // AUTO_BED_LEVELING_UBL +#endif // UNIFIED_BED_LEVELING_H \ No newline at end of file diff --git a/Marlin/UBL_Bed_Leveling.cpp b/Marlin/UBL_Bed_Leveling.cpp new file mode 100644 index 0000000000..feff74e976 --- /dev/null +++ b/Marlin/UBL_Bed_Leveling.cpp @@ -0,0 +1,296 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "Marlin.h" +#include "math.h" + +#if ENABLED(AUTO_BED_LEVELING_UBL) + #include "UBL.h" + #include "hex_print_routines.h" + + /** + * These variables used to be declared inside the bed_leveling class. We are going to still declare + * them within the .cpp file for bed leveling. But there is only one instance of the bed leveling + * object and we can get rid of a level of inderection by not making them 'member data'. So, in the + * interest of speed, we do it this way. When we move to a 32-Bit processor, they can be moved + * back inside the bed leveling class. + */ + float last_specified_z, + fade_scaling_factor_for_current_height, + z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS], + mesh_index_to_X_location[UBL_MESH_NUM_X_POINTS + 1], // +1 just because of paranoia that we might end up on the + mesh_index_to_Y_location[UBL_MESH_NUM_Y_POINTS + 1]; // the last Mesh Line and that is the start of a whole new cell + + bed_leveling::bed_leveling() { + for (uint8_t i = 0; i <= UBL_MESH_NUM_X_POINTS; i++) // We go one past what we expect to ever need for safety + mesh_index_to_X_location[i] = double(UBL_MESH_MIN_X) + double(MESH_X_DIST) * double(i); + + for (uint8_t i = 0; i <= UBL_MESH_NUM_Y_POINTS; i++) // We go one past what we expect to ever need for safety + mesh_index_to_Y_location[i] = double(UBL_MESH_MIN_Y) + double(MESH_Y_DIST) * double(i); + + reset(); + } + + void bed_leveling::store_state() { + int k = E2END - sizeof(blm.state); + eeprom_write_block((void *)&blm.state, (void *)k, sizeof(blm.state)); + } + + void bed_leveling::load_state() { + int k = E2END - sizeof(blm.state); + eeprom_read_block((void *)&blm.state, (void *)k, sizeof(blm.state)); + + if (sanity_check()) + SERIAL_PROTOCOLLNPGM("?In load_state() sanity_check() failed.\n"); + + // These lines can go away in a few weeks. They are just + // to make sure people updating thier firmware won't be using + if (blm.state.G29_Fade_Height_Multiplier != 1.0 / blm.state.G29_Correction_Fade_Height) { // an incomplete Bed_Leveling.state structure. For speed + blm.state.G29_Fade_Height_Multiplier = 1.0 / blm.state.G29_Correction_Fade_Height; // we now multiply by the inverse of the Fade Height instead of + store_state(); // dividing by it. Soon... all of the old structures will be + } // updated, but until then, we try to ease the transition + // for our Beta testers. + } + + void bed_leveling::load_mesh(int m) { + int k = E2END - sizeof(blm.state), + j = (k - Unified_Bed_Leveling_EEPROM_start) / sizeof(z_values); + + if (m == -1) { + SERIAL_PROTOCOLLNPGM("?No mesh saved in EEPROM. Zeroing mesh in memory.\n"); + reset(); + return; + } + + if (m < 0 || m >= j || Unified_Bed_Leveling_EEPROM_start <= 0) { + SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n"); + return; + } + + j = k - (m + 1) * sizeof(z_values); + eeprom_read_block((void *)&z_values , (void *)j, sizeof(z_values)); + + SERIAL_PROTOCOLPGM("Mesh loaded from slot "); + SERIAL_PROTOCOL(m); + SERIAL_PROTOCOLPGM(" at offset 0x"); + prt_hex_word(j); + SERIAL_EOL; + } + + void bed_leveling:: store_mesh(int m) { + int k = E2END - sizeof(state), + j = (k - Unified_Bed_Leveling_EEPROM_start) / sizeof(z_values); + + if (m < 0 || m >= j || Unified_Bed_Leveling_EEPROM_start <= 0) { + SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n"); + SERIAL_PROTOCOL(m); + SERIAL_PROTOCOLLNPGM(" mesh slots available.\n"); + SERIAL_PROTOCOLLNPAIR("E2END : ", E2END); + SERIAL_PROTOCOLLNPAIR("k : ", k); + SERIAL_PROTOCOLLNPAIR("j : ", j); + SERIAL_PROTOCOLLNPAIR("m : ", m); + SERIAL_EOL; + return; + } + + j = k - (m + 1) * sizeof(z_values); + eeprom_write_block((const void *)&z_values, (void *)j, sizeof(z_values)); + + SERIAL_PROTOCOLPGM("Mesh saved in slot "); + SERIAL_PROTOCOL(m); + SERIAL_PROTOCOLPGM(" at offset 0x"); + prt_hex_word(j); + SERIAL_EOL; + } + + void bed_leveling::reset() { + state.active = false; + state.z_offset = 0; + state.EEPROM_storage_slot = -1; + + ZERO(z_values); + + last_specified_z = -999.9; // We can't pre-initialize these values in the declaration + fade_scaling_factor_for_current_height = 0.0; // due to C++11 constraints + } + + void bed_leveling::invalidate() { + prt_hex_word((unsigned int)this); + SERIAL_EOL; + + state.active = false; + state.z_offset = 0; + for (int x = 0; x < UBL_MESH_NUM_X_POINTS; x++) + for (int y = 0; y < UBL_MESH_NUM_Y_POINTS; y++) + z_values[x][y] = NAN; + } + + void bed_leveling::display_map(int map_type) { + float f, current_xi, current_yi; + int8_t i, j; + UNUSED(map_type); + + SERIAL_PROTOCOLLNPGM("\nBed Topography Report:\n"); + + SERIAL_ECHOPAIR("(", 0); + SERIAL_ECHOPAIR(", ", UBL_MESH_NUM_Y_POINTS - 1); + SERIAL_ECHOPGM(") "); + + current_xi = blm.get_cell_index_x(current_position[X_AXIS] + (MESH_X_DIST) / 2.0); + current_yi = blm.get_cell_index_y(current_position[Y_AXIS] + (MESH_Y_DIST) / 2.0); + + for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) + SERIAL_ECHOPGM(" "); + + SERIAL_ECHOPAIR("(", UBL_MESH_NUM_X_POINTS - 1); + SERIAL_ECHOPAIR(",", UBL_MESH_NUM_Y_POINTS - 1); + SERIAL_ECHOLNPGM(")"); + + // if (map_type || 1) { + SERIAL_ECHOPAIR("(", UBL_MESH_MIN_X); + SERIAL_ECHOPAIR(",", UBL_MESH_MAX_Y); + SERIAL_CHAR(')'); + + for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) + SERIAL_ECHOPGM(" "); + + SERIAL_ECHOPAIR("(", UBL_MESH_MAX_X); + SERIAL_ECHOPAIR(",", UBL_MESH_MAX_Y); + SERIAL_ECHOLNPGM(")"); + // } + + for (j = UBL_MESH_NUM_Y_POINTS - 1; j >= 0; j--) { + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + f = z_values[i][j]; + + // is the nozzle here? if so, mark the number + SERIAL_CHAR(i == current_xi && j == current_yi ? '[' : ' '); + + if (isnan(f)) + SERIAL_PROTOCOLPGM(" . "); + else { + // if we don't do this, the columns won't line up nicely + if (f >= 0.0) SERIAL_CHAR(' '); + SERIAL_PROTOCOL_F(f, 5); + idle(); + } + if (i == current_xi && j == current_yi) // is the nozzle here? if so, finish marking the number + SERIAL_CHAR(']'); + else + SERIAL_PROTOCOL(" "); + + SERIAL_CHAR(' '); + } + SERIAL_EOL; + if (j) { // we want the (0,0) up tight against the block of numbers + SERIAL_CHAR(' '); + SERIAL_EOL; + } + } + + // if (map_type) { + SERIAL_ECHOPAIR("(", int(UBL_MESH_MIN_X)); + SERIAL_ECHOPAIR(",", int(UBL_MESH_MIN_Y)); + SERIAL_ECHOPGM(") "); + + for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) + SERIAL_ECHOPGM(" "); + + SERIAL_ECHOPAIR("(", int(UBL_MESH_MAX_X)); + SERIAL_ECHOPAIR(",", int(UBL_MESH_MIN_Y)); + SERIAL_CHAR(')'); + // } + + SERIAL_ECHOPAIR("(", 0); + SERIAL_ECHOPAIR(",", 0); + SERIAL_ECHOPGM(") "); + + for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) + SERIAL_ECHOPGM(" "); + + SERIAL_ECHOPAIR("(", UBL_MESH_NUM_X_POINTS-1); + SERIAL_ECHOPAIR(",", 0); + SERIAL_CHAR(')'); + + SERIAL_CHAR(' '); + SERIAL_EOL; + } + + bool bed_leveling::sanity_check() { + uint8_t error_flag = 0; + + if (state.n_x != UBL_MESH_NUM_X_POINTS) { + SERIAL_PROTOCOLLNPGM("?UBL_MESH_NUM_X_POINTS set wrong\n"); + error_flag++; + } + + if (state.n_y != UBL_MESH_NUM_Y_POINTS) { + SERIAL_PROTOCOLLNPGM("?UBL_MESH_NUM_Y_POINTS set wrong\n"); + error_flag++; + } + + if (state.mesh_x_min != UBL_MESH_MIN_X) { + SERIAL_PROTOCOLLNPGM("?UBL_MESH_MIN_X set wrong\n"); + error_flag++; + } + + if (state.mesh_y_min != UBL_MESH_MIN_Y) { + SERIAL_PROTOCOLLNPGM("?UBL_MESH_MIN_Y set wrong\n"); + error_flag++; + } + + if (state.mesh_x_max != UBL_MESH_MAX_X) { + SERIAL_PROTOCOLLNPGM("?UBL_MESH_MAX_X set wrong\n"); + error_flag++; + } + + if (state.mesh_y_max != UBL_MESH_MAX_Y) { + SERIAL_PROTOCOLLNPGM("?UBL_MESH_MAX_Y set wrong\n"); + error_flag++; + } + + if (state.mesh_x_dist != MESH_X_DIST) { + SERIAL_PROTOCOLLNPGM("?MESH_X_DIST set wrong\n"); + error_flag++; + } + + if (state.mesh_y_dist != MESH_Y_DIST) { + SERIAL_PROTOCOLLNPGM("?MESH_Y_DIST set wrong\n"); + error_flag++; + } + + int k = E2END - sizeof(blm.state), + j = (k - Unified_Bed_Leveling_EEPROM_start) / sizeof(z_values); + + if (j < 1) { + SERIAL_PROTOCOLLNPGM("?No EEPROM storage available for a mesh of this size.\n"); + error_flag++; + } + + // SERIAL_PROTOCOLPGM("?sanity_check() return value: "); + // SERIAL_PROTOCOL(error_flag); + // SERIAL_EOL; + + return !!error_flag; + } + +#endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/UBL_G29.cpp b/Marlin/UBL_G29.cpp new file mode 100644 index 0000000000..7f4bdf3686 --- /dev/null +++ b/Marlin/UBL_G29.cpp @@ -0,0 +1,1455 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "Marlin.h" +#if ENABLED(AUTO_BED_LEVELING_UBL) + //#include "vector_3.h" + //#include "qr_solve.h" + + #include "UBL.h" + #include "hex_print_routines.h" + #include "configuration_store.h" + #include "planner.h" + #include "ultralcd.h" + + #include + + void lcd_babystep_z(); + void lcd_return_to_status(); + bool lcd_clicked(); + void lcd_implementation_clear(); + void lcd_mesh_edit_setup(float inital); + float lcd_mesh_edit(); + void lcd_z_offset_edit_setup(float); + float lcd_z_offset_edit(); + + extern float meshedit_done; + extern long babysteps_done; + extern float code_value_float(); + extern bool code_value_bool(); + extern bool code_has_value(); + extern float probe_pt(float x, float y, bool, int); + extern float zprobe_zoffset; + extern bool set_probe_deployed(bool); + #define DEPLOY_PROBE() set_probe_deployed(true) + #define STOW_PROBE() set_probe_deployed(false) + bool ProbeStay = true; + float ubl_3_point_1_X = UBL_PROBE_PT_1_X; + float ubl_3_point_1_Y = UBL_PROBE_PT_1_Y; + float ubl_3_point_2_X = UBL_PROBE_PT_2_X; + float ubl_3_point_2_Y = UBL_PROBE_PT_2_Y; + float ubl_3_point_3_X = UBL_PROBE_PT_3_X; + float ubl_3_point_3_Y = UBL_PROBE_PT_3_Y; + + #define SIZE_OF_LITTLE_RAISE 0 + #define BIG_RAISE_NOT_NEEDED 0 + extern void lcd_quick_feedback(); + + /** + * G29: Unified Bed Leveling by Roxy + */ + + // Transform required to compensate for bed level + //extern matrix_3x3 plan_bed_level_matrix; + + /** + * Get the position applying the bed level matrix + */ + + //vector_3 plan_get_position(); + + // static void set_bed_level_equation_lsq(double* plane_equation_coefficients); + // static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3); + + /** + * G29: Mesh Based Compensation System + * + * Parameters understood by this leveling system: + * + * A Activate Activate the Unified Bed Leveling system. + * + * B # Business Use the 'Business Card' mode of the Manual Probe subsystem. This is invoked as + * G29 P2 B The mode of G29 P2 allows you to use a bussiness card or recipe card + * as a shim that the nozzle will pinch as it is lowered. The idea is that you + * can easily feel the nozzle getting to the same height by the amount of resistance + * the business card exhibits to movement. You should try to achieve the same amount + * of resistance on each probed point to facilitate accurate and repeatable measurements. + * You should be very careful not to drive the nozzle into the bussiness card with a + * lot of force as it is very possible to cause damage to your printer if your are + * careless. If you use the B option with G29 P2 B you can leave the number parameter off + * on its first use to enable measurement of the business card thickness. Subsequent usage + * of the B parameter can have the number previously measured supplied to the command. + * Incidently, you are much better off using something like a Spark Gap feeler gauge than + * something that compresses like a Business Card. + * + * C Continue Continue, Constant, Current Location. This is not a primary command. C is used to + * further refine the behaviour of several other commands. Issuing a G29 P1 C will + * continue the generation of a partially constructed Mesh without invalidating what has + * been done. Issuing a G29 P2 C will tell the Manual Probe subsystem to use the current + * location in its search for the closest unmeasured Mesh Point. When used with a G29 Z C + * it indicates to use the current location instead of defaulting to the center of the print bed. + * + * D Disable Disable the Unified Bed Leveling system. + * + * E Stow_probe Stow the probe after each sampled point. + * + * F # Fade * Fade the amount of Mesh Based Compensation over a specified height. At the specified height, + * no correction is applied and natural printer kenimatics take over. If no number is specified + * for the command, 10mm is assummed to be reasonable. + * + * G # Grid * Perform a Grid Based Leveling of the current Mesh using a grid with n points on + * a side. + * + * H # Height Specify the Height to raise the nozzle after each manual probe of the bed. The + * default is 5mm. + * + * I # Invalidate Invalidate specified number of Mesh Points. The nozzle location is used unless + * the X and Y parameter are used. If no number is specified, only the closest Mesh + * point to the location is invalidated. The M parameter is available as well to produce + * a map after the operation. This command is useful to invalidate a portion of the + * Mesh so it can be adjusted using other tools in the Unified Bed Leveling System. When + * attempting to invalidate an isolated bad point in the mesh, the M option will indicate + * where the nozzle is positioned in the Mesh with (#). You can move the nozzle around on + * the bed and use this feature to select the center of the area (or cell) you want to + * invalidate. + * + * K # Kompare Kompare current Mesh with stored Mesh # replacing current Mesh with the result. This + * command litterly performs a difference between two Mesh. + * + * L Load * Load Mesh from the previously activated location in the EEPROM. + * + * L # Load * Load Mesh from the specified location in the EEPROM. Set this location as activated + * for subsequent Load and Store operations. + * + * O Map * Display the Mesh Map Topology. + * The parameter can be specified alone (ie. G29 O) or in combination with many of the + * other commands. The Mesh Map option works with all of the Phase + * commands (ie. G29 P4 R 5 X 50 Y100 C -.1 O) + * + * N No Home G29 normally insists that a G28 has been performed. You can over rule this with an + * N option. In general, you should not do this. This can only be done safely with + * commands that do not move the nozzle. + * + * The P or Phase commands are used for the bulk of the work to setup a Mesh. In general, your Mesh will + * start off being initialized with a G29 P0 or a G29 P1. Further refinement of the Mesh happens with + * each additional Phase that processes it. + * + * P0 Phase 0 Zero Mesh Data and turn off the Mesh Compensation System. This reverts the + * 3D Printer to the same state it was in before the Unified Bed Leveling Compensation + * was turned on. Setting the entire Mesh to Zero is a special case that allows + * a subsequent G or T leveling operation for backward compatability. + * + * P1 Phase 1 Invalidate entire Mesh and continue with automatic generation of the Mesh data using + * the Z-Probe. Depending upon the values of DELTA_PROBEABLE_RADIUS and + * DELTA_PRINTABLE_RADIUS some area of the bed will not have Mesh Data automatically + * generated. This will be handled in Phase 2. If the Phase 1 command is given the + * C (Continue) parameter it does not invalidate the Mesh prior to automatically + * probing needed locations. This allows you to invalidate portions of the Mesh but still + * use the automatic probing capabilities of the Unified Bed Leveling System. An X and Y + * parameter can be given to prioritize where the command should be trying to measure points. + * If the X and Y parameters are not specified the current probe position is used. Phase 1 + * allows you to specify the M (Map) parameter so you can watch the generation of the Mesh. + * Phase 1 also watches for the LCD Panel's Encoder Switch being held in a depressed state. + * It will suspend generation of the Mesh if it sees the user request that. (This check is + * only done between probe points. You will need to press and hold the switch until the + * Phase 1 command can detect it.) + * + * P2 Phase 2 Probe areas of the Mesh that can not be automatically handled. Phase 2 respects an H + * parameter to control the height between Mesh points. The default height for movement + * between Mesh points is 5mm. A smaller number can be used to make this part of the + * calibration less time consuming. You will be running the nozzle down until it just barely + * touches the glass. You should have the nozzle clean with no plastic obstructing your view. + * Use caution and move slowly. It is possible to damage your printer if you are careless. + * Note that this command will use the configuration #define SIZE_OF_LITTLE_RAISE if the + * nozzle is moving a distance of less than BIG_RAISE_NOT_NEEDED. + * + * The H parameter can be set negative if your Mesh dips in a large area. You can press + * and hold the LCD Panel's encoder wheel to terminate the current Phase 2 command. You + * can then re-issue the G29 P 2 command with an H parameter that is more suitable for the + * area you are manually probing. Note that the command tries to start you in a corner + * of the bed where movement will be predictable. You can force the location to be used in + * the distance calculations by using the X and Y parameters. You may find it is helpful to + * print out a Mesh Map (G29 O ) to understand where the mesh is invalidated and where + * the nozzle will need to move in order to complete the command. The C parameter is + * available on the Phase 2 command also and indicates the search for points to measure should + * be done based on the current location of the nozzle. + * + * A B parameter is also available for this command and described up above. It places the + * manual probe subsystem into Business Card mode where the thickness of a business care is + * measured and then used to accurately set the nozzle height in all manual probing for the + * duration of the command. (S for Shim mode would be a better parameter name, but S is needed + * for Save or Store of the Mesh to EEPROM) A Business card can be used, but you will have + * better results if you use a flexible Shim that does not compress very much. That makes it + * easier for you to get the nozzle to press with similar amounts of force against the shim so you + * can get accurate measurements. As you are starting to touch the nozzle against the shim try + * to get it to grasp the shim with the same force as when you measured the thickness of the + * shim at the start of the command. + * + * Phase 2 allows the O (Map) parameter to be specified. This helps the user see the progression + * of the Mesh being built. + * + * P3 Phase 3 Fill the unpopulated regions of the Mesh with a fixed value. The C parameter is used to + * specify the Constant value to fill all invalid areas of the Mesh. If no C parameter is + * specified, a value of 0.0 is assumed. The R parameter can be given to specify the number + * of points to set. If the R parameter is specified the current nozzle position is used to + * find the closest points to alter unless the X and Y parameter are used to specify the fill + * location. + * + * P4 Phase 4 Fine tune the Mesh. The Delta Mesh Compensation System assume the existance of + * an LCD Panel. It is possible to fine tune the mesh without the use of an LCD Panel. + * (More work and details on doing this later!) + * The System will search for the closest Mesh Point to the nozzle. It will move the + * nozzle to this location. The user can use the LCD Panel to carefully adjust the nozzle + * so it is just barely touching the bed. When the user clicks the control, the System + * will lock in that height for that point in the Mesh Compensation System. + * + * Phase 4 has several additional parameters that the user may find helpful. Phase 4 + * can be started at a specific location by specifying an X and Y parameter. Phase 4 + * can be requested to continue the adjustment of Mesh Points by using the R(epeat) + * parameter. If the Repetition count is not specified, it is assumed the user wishes + * to adjust the entire matrix. The nozzle is moved to the Mesh Point being edited. + * The command can be terminated early (or after the area of interest has been edited) by + * pressing and holding the encoder wheel until the system recognizes the exit request. + * Phase 4's general form is G29 P4 [R # of points] [X position] [Y position] + * + * Phase 4 is intended to be used with the G26 Mesh Validation Command. Using the + * information left on the printer's bed from the G26 command it is very straight forward + * and easy to fine tune the Mesh. One concept that is important to remember and that + * will make using the Phase 4 command easy to use is this: You are editing the Mesh Points. + * If you have too little clearance and not much plastic was extruded in an area, you want to + * LOWER the Mesh Point at the location. If you did not get good adheasion, you want to + * RAISE the Mesh Point at that location. + * + * + * P5 Phase 5 Find Mean Mesh Height and Standard Deviation. Typically, it is easier to use and + * work with the Mesh if it is Mean Adjusted. You can specify a C parameter to + * Correct the Mesh to a 0.00 Mean Height. Adding a C parameter will automatically + * execute a G29 P6 C . + * + * P6 Phase 6 Shift Mesh height. The entire Mesh's height is adjusted by the height specified + * with the C parameter. Being able to adjust the height of a Mesh is useful tool. It + * can be used to compensate for poorly calibrated Z-Probes and other errors. Ideally, + * you should have the Mesh adjusted for a Mean Height of 0.00 and the Z-Probe measuring + * 0.000 at the Z Home location. + * + * Q Test * Load specified Test Pattern to assist in checking correct operation of system. This + * command is not anticipated to be of much value to the typical user. It is intended + * for developers to help them verify correct operation of the Unified Bed Leveling System. + * + * S Store Store the current Mesh in the Activated area of the EEPROM. It will also store the + * current state of the Unified Bed Leveling system in the EEPROM. + * + * S # Store Store the current Mesh at the specified location in EEPROM. Activate this location + * for subsequent Load and Store operations. It will also store the current state of + * the Unified Bed Leveling system in the EEPROM. + * + * S -1 Store Store the current Mesh as a print out that is suitable to be feed back into + * the system at a later date. The text generated can be saved and later sent by PronterFace or + * Repetier Host to reconstruct the current mesh on another machine. + * + * T 3-Point Perform a 3 Point Bed Leveling on the current Mesh + * + * W What? Display valuable data the Unified Bed Leveling System knows. + * + * X # * * Specify X Location for this line of commands + * + * Y # * * Specify Y Location for this line of commands + * + * Z Zero * Probes to set the Z Height of the nozzle. The entire Mesh can be raised or lowered + * by just doing a G29 Z + * + * Z # Zero * The entire Mesh can be raised or lowered to conform with the specified difference. + * zprobe_zoffset is added to the calculation. + * + * + * Release Notes: + * You MUST do a M502 & M500 pair of commands to initialize the storage. Failure to do this + * will cause all kinds of problems. Enabling EEPROM Storage is highly recommended. With + * EEPROM Storage of the mesh, you are limited to 3-Point and Grid Leveling. (G29 P0 T and + * G29 P0 G respectively.) + * + * Z-Probe Sleds are not currently fully supported. There were too many complications caused + * by them to support them in the Unified Bed Leveling code. Support for them will be handled + * better in the upcoming Z-Probe Object that will happen during the Code Clean Up phase. (That + * is what they really are: A special case of the Z-Probe.) When a Z-Probe Object appears, it + * should slip in under the Unified Bed Leveling code without major trauma. + * + * When you do a G28 and then a G29 P1 to automatically build your first mesh, you are going to notice + * the Unified Bed Leveling probes points further and further away from the starting location. (The + * starting location defaults to the center of the bed.) The original Grid and Mesh leveling used + * a Zig Zag pattern. The new pattern is better, especially for people with Delta printers. This + * allows you to get the center area of the Mesh populated (and edited) quicker. This allows you to + * perform a small print and check out your settings quicker. You do not need to populate the + * entire mesh to use it. (You don't want to spend a lot of time generating a mesh only to realize + * you don't have the resolution or zprobe_zoffset set correctly. The Mesh generation + * gathers points closest to where the nozzle is located unless you specify an (X,Y) coordinate pair. + * + * The Unified Bed Leveling uses a lot of EEPROM storage to hold its data. And it takes some effort + * to get this Mesh data correct for a user's printer. We do not want this data destroyed as + * new versions of Marlin add or subtract to the items stored in EEPROM. So, for the benefit of + * the users, we store the Mesh data at the end of the EEPROM and do not keep it contiguous with the + * other data stored in the EEPROM. (For sure the developers are going to complain about this, but + * this is going to be helpful to the users!) + * + * The foundation of this Bed Leveling System is built on Epatel's Mesh Bed Leveling code. A big + * 'Thanks!' to him and the creators of 3-Point and Grid Based leveling. Combining thier contributions + * we now have the functionality and features of all three systems combined. + */ + + int Unified_Bed_Leveling_EEPROM_start = -1; + int UBL_has_control_of_LCD_Panel = 0; + volatile int G29_encoderDiff = 0; // This is volatile because it is getting changed at interrupt time. + + // We keep the simple parameter flags and values as 'static' because we break out the + // parameter parsing into a support routine. + + static int G29_Verbose_Level = 0, Test_Value = 0, + Phase_Value = -1, Repetition_Cnt = 1; + static bool Repeat_Flag = UBL_OK, C_Flag = false, X_Flag = UBL_OK, Y_Flag = UBL_OK, Statistics_Flag = UBL_OK, Business_Card_Mode = false; + static float X_Pos = 0.0, Y_Pos = 0.0, Height_Value = 5.0, measured_z, card_thickness = 0.0, Constant = 0.0; + static int Storage_Slot = 0, Test_Pattern = 0; + + #if ENABLED(ULTRA_LCD) + void lcd_setstatus(const char* message, bool persist); + #endif + + void gcode_G29() { + mesh_index_pair location; + int i, j, k; + float Z1, Z2, Z3; + + G29_Verbose_Level = 0; // These may change, but let's get some reasonable values into them. + Repeat_Flag = UBL_OK; + Repetition_Cnt = 1; + C_Flag = false; + + SERIAL_PROTOCOLPGM("Unified_Bed_Leveling_EEPROM_start="); + SERIAL_PROTOCOLLN(Unified_Bed_Leveling_EEPROM_start); + + if (Unified_Bed_Leveling_EEPROM_start < 0) { + SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it "); + SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n"); + return; + } + + if (!code_seen('N') && axis_unhomed_error(true, true, true)) // Don't allow auto-leveling without homing first + gcode_G28(); + + if (G29_Parameter_Parsing()) return; // abort if parsing the simple parameters causes a problem, + + // Invalidate Mesh Points. This command is a little bit asymetrical because + // it directly specifies the repetition count and does not use the 'R' parameter. + if (code_seen('I')) { + Repetition_Cnt = code_has_value() ? code_value_int() : 1; + while (Repetition_Cnt--) { + location = find_closest_mesh_point_of_type(REAL, X_Pos, Y_Pos, 0, NULL); // The '0' says we want to use the nozzle's position + if (location.x_index < 0) { + SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n"); + break; // No more invalid Mesh Points to populate + } + z_values[location.x_index][location.y_index] = NAN; + } + SERIAL_PROTOCOLLNPGM("Locations invalidated.\n"); + } + + if (code_seen('Q')) { + + if (code_has_value()) Test_Pattern = code_value_int(); + + if (Test_Pattern < 0 || Test_Pattern > 4) { + SERIAL_PROTOCOLLNPGM("Invalid Test_Pattern value. (0-4)\n"); + return; + } + SERIAL_PROTOCOLLNPGM("Loading Test_Pattern values.\n"); + switch (Test_Pattern) { + case 0: + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { // Create a bowl shape. This is + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { // similar to what a user would see with + Z1 = 0.5 * (UBL_MESH_NUM_X_POINTS) - i; // a poorly calibrated Delta. + Z2 = 0.5 * (UBL_MESH_NUM_Y_POINTS) - j; + z_values[i][j] += 2.0 * HYPOT(Z1, Z2); + } + } + break; + case 1: + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { // Create a diagonal line several Mesh + z_values[i][i] += 9.999; // cells thick that is raised + if (i < UBL_MESH_NUM_Y_POINTS - 1) + z_values[i][i + 1] += 9.999; // We want the altered line several mesh points thick + if (i > 0) + z_values[i][i - 1] += 9.999; // We want the altered line several mesh points thick + } + break; + case 2: + // Allow the user to specify the height because 10mm is + // a little bit extreme in some cases. + for (i = (UBL_MESH_NUM_X_POINTS) / 3.0; i < 2 * ((UBL_MESH_NUM_X_POINTS) / 3.0); i++) // Create a rectangular raised area in + for (j = (UBL_MESH_NUM_Y_POINTS) / 3.0; j < 2 * ((UBL_MESH_NUM_Y_POINTS) / 3.0); j++) // the center of the bed + z_values[i][j] += code_seen('C') ? Constant : 9.99; + break; + case 3: + break; + } + } + + if (code_seen('P')) { + Phase_Value = code_value_int(); + if (Phase_Value < 0 || Phase_Value > 7) { + SERIAL_PROTOCOLLNPGM("Invalid Phase value. (0-4)\n"); + return; + } + switch (Phase_Value) { + // + // Zero Mesh Data + // + case 0: + blm.reset(); + SERIAL_PROTOCOLLNPGM("Mesh zeroed.\n"); + break; + // + // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe + // + case 1: + if (!code_seen('C') ) { + blm.invalidate(); + SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.\n"); + } + if (G29_Verbose_Level > 1) { + SERIAL_ECHOPGM("Probing Mesh Points Closest to ("); + SERIAL_ECHO(X_Pos); + SERIAL_ECHOPAIR(",", Y_Pos); + SERIAL_PROTOCOLLNPGM(")\n"); + } + probe_entire_mesh( X_Pos+X_PROBE_OFFSET_FROM_EXTRUDER, Y_Pos+Y_PROBE_OFFSET_FROM_EXTRUDER, + code_seen('O') || code_seen('M'), code_seen('E')); + break; + // + // Manually Probe Mesh in areas that can not be reached by the probe + // + case 2: + SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n"); + do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); + if (!X_Flag && !Y_Flag) { // use a good default location for the path + X_Pos = X_MIN_POS; + Y_Pos = Y_MIN_POS; + if (X_PROBE_OFFSET_FROM_EXTRUDER > 0) // The flipped > and < operators on these two comparisons is + X_Pos = X_MAX_POS; // intentional. It should cause the probed points to follow a + + if (Y_PROBE_OFFSET_FROM_EXTRUDER < 0) // nice path on Cartesian printers. It may make sense to + Y_Pos = Y_MAX_POS; // have Delta printers default to the center of the bed. + + } // For now, until that is decided, it can be forced with the X + // and Y parameters. + if (code_seen('C')) { + X_Pos = current_position[X_AXIS]; + Y_Pos = current_position[Y_AXIS]; + } + + Height_Value = code_seen('H') && code_has_value() ? code_value_float() : Z_CLEARANCE_BETWEEN_PROBES; + + if ((Business_Card_Mode = code_seen('B'))) { + card_thickness = code_has_value() ? code_value_float() : measure_business_card_thickness(Height_Value); + + if (fabs(card_thickness) > 1.5) { + SERIAL_PROTOCOLLNPGM("?Error in Business Card measurment.\n"); + return; + } + } + manually_probe_remaining_mesh( X_Pos, Y_Pos, Height_Value, card_thickness, code_seen('O') || code_seen('M')); + break; + // + // Populate invalid Mesh areas with a constant + // + case 3: + Height_Value = 0.0; // Assume 0.0 until proven otherwise + if (code_seen('C')) Height_Value = Constant; + // If no repetition is specified, do the whole Mesh + if (!Repeat_Flag) Repetition_Cnt = 9999; + while (Repetition_Cnt--) { + location = find_closest_mesh_point_of_type( INVALID, X_Pos, Y_Pos, 0, NULL); // The '0' says we want to use the nozzle's position + if (location.x_index < 0) break; // No more invalid Mesh Points to populate + z_values[location.x_index][location.y_index] = Height_Value; + } + break; + // + // Fine Tune (Or Edit) the Mesh + // + case 4: + fine_tune_mesh(X_Pos, Y_Pos, Height_Value, code_seen('O') || code_seen('M')); + break; + case 5: + Find_Mean_Mesh_Height(); + break; + case 6: + Shift_Mesh_Height(); + break; + + case 10: + UBL_has_control_of_LCD_Panel++; // Debug code... Pan no attention to this stuff + SERIAL_ECHO_START; + SERIAL_ECHOPGM("Checking G29 has control of LCD Panel:\n"); + while(!G29_lcd_clicked()) { + idle(); + delay(250); + SERIAL_PROTOCOL(G29_encoderDiff); + G29_encoderDiff = 0; + SERIAL_EOL; + } + while (G29_lcd_clicked()) idle(); + UBL_has_control_of_LCD_Panel = 0;; + SERIAL_ECHOPGM("G29 giving back control of LCD Panel.\n"); + break; + } + } + + if (code_seen('T')) { + Z1 = probe_pt(ubl_3_point_1_X, ubl_3_point_1_Y, false /*Stow Flag*/, G29_Verbose_Level) + zprobe_zoffset; + Z2 = probe_pt(ubl_3_point_2_X, ubl_3_point_2_Y, false /*Stow Flag*/, G29_Verbose_Level) + zprobe_zoffset; + Z3 = probe_pt(ubl_3_point_3_X, ubl_3_point_3_Y, true /*Stow Flag*/, G29_Verbose_Level) + zprobe_zoffset; + + // We need to adjust Z1, Z2, Z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean + // the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is) + + Z1 -= blm.get_z_correction(ubl_3_point_1_X, ubl_3_point_1_Y); + Z2 -= blm.get_z_correction(ubl_3_point_2_X, ubl_3_point_2_Y); + Z3 -= blm.get_z_correction(ubl_3_point_3_X, ubl_3_point_3_Y); + + do_blocking_move_to_xy((X_MAX_POS - (X_MIN_POS)) / 2.0, (Y_MAX_POS - (Y_MIN_POS)) / 2.0); + tilt_mesh_based_on_3pts(Z1, Z2, Z3); + } + + // + // Much of the 'What?' command can be eliminated. But until we are fully debugged, it is + // good to have the extra information. Soon... we prune this to just a few items + // + if (code_seen('W')) G29_What_Command(); + + // + // When we are fully debugged, the EEPROM dump command will get deleted also. But + // right now, it is good to have the extra information. Soon... we prune this. + // + if (code_seen('J')) G29_EEPROM_Dump(); // EEPROM Dump + + // + // When we are fully debugged, this may go away. But there are some valid + // use cases for the users. So we can wait and see what to do with it. + // + + if (code_seen('K')) // Kompare Current Mesh Data to Specified Stored Mesh + G29_Kompare_Current_Mesh_to_Stored_Mesh(); + + // + // Load a Mesh from the EEPROM + // + + if (code_seen('L')) { // Load Current Mesh Data + Storage_Slot = code_has_value() ? code_value_int() : blm.state.EEPROM_storage_slot; + + k = E2END - sizeof(blm.state); + j = (k - Unified_Bed_Leveling_EEPROM_start) / sizeof(z_values); + + if (Storage_Slot < 0 || Storage_Slot >= j || Unified_Bed_Leveling_EEPROM_start <= 0) { + SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); + return; + } + blm.load_mesh(Storage_Slot); + blm.state.EEPROM_storage_slot = Storage_Slot; + if (Storage_Slot != blm.state.EEPROM_storage_slot) + blm.store_state(); + SERIAL_PROTOCOLLNPGM("Done.\n"); + } + + // + // Store a Mesh in the EEPROM + // + + if (code_seen('S')) { // Store (or Save) Current Mesh Data + Storage_Slot = code_has_value() ? code_value_int() : blm.state.EEPROM_storage_slot; + + if (Storage_Slot == -1) { // Special case, we are going to 'Export' the mesh to the + SERIAL_ECHOPGM("G29 I 999\n"); // host in a form it can be reconstructed on a different machine + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + if (!isnan(z_values[i][j])) { + SERIAL_ECHOPAIR("M421 I ", i); + SERIAL_ECHOPAIR(" J ", j); + SERIAL_ECHOPGM(" Z "); + SERIAL_PROTOCOL_F(z_values[i][j], 6); + SERIAL_EOL; + } + } + } + return; + } + + int k = E2END - sizeof(blm.state), + j = (k - Unified_Bed_Leveling_EEPROM_start) / sizeof(z_values); + + if (Storage_Slot < 0 || Storage_Slot >= j || Unified_Bed_Leveling_EEPROM_start <= 0) { + SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); + SERIAL_PROTOCOLLNPAIR("?Use 0 to ", j - 1); + goto LEAVE; + } + blm.store_mesh(Storage_Slot); + blm.state.EEPROM_storage_slot = Storage_Slot; + // + // if (Storage_Slot != blm.state.EEPROM_storage_slot) + blm.store_state(); // Always save an updated copy of the UBL State info + + SERIAL_PROTOCOLLNPGM("Done.\n"); + } + + if (code_seen('O') || code_seen('M')) { + i = code_has_value() ? code_value_int() : 0; + blm.display_map(i); + } + + if (code_seen('Z')) { + if (code_has_value()) { + blm.state.z_offset = code_value_float(); // do the simple case. Just lock in the specified value + } + else { + save_UBL_active_state_and_disable(); + //measured_z = probe_pt(X_Pos + X_PROBE_OFFSET_FROM_EXTRUDER, Y_Pos+Y_PROBE_OFFSET_FROM_EXTRUDER, ProbeDeployAndStow, G29_Verbose_Level); + + measured_z = 1.5; + do_blocking_move_to_z(measured_z); // Get close to the bed, but leave some space so we don't damage anything + // The user is not going to be locking in a new Z-Offset very often so + // it won't be that painful to spin the Encoder Wheel for 1.5mm + lcd_implementation_clear(); + lcd_z_offset_edit_setup(measured_z); + do { + measured_z = lcd_z_offset_edit(); + idle(); + do_blocking_move_to_z(measured_z); + } while (!G29_lcd_clicked()); + + UBL_has_control_of_LCD_Panel = 1; // There is a race condition for the Encoder Wheel getting clicked. + // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune( ) + // or here. So, until we are done looking for a long Encoder Wheel Press, + // we need to take control of the panel + millis_t nxt = millis() + 1500UL; + lcd_return_to_status(); + while (G29_lcd_clicked()) { // debounce and watch for abort + idle(); + if (ELAPSED(millis(), nxt)) { + SERIAL_PROTOCOLLNPGM("\nZ-Offset Adjustment Stopped."); + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); + lcd_setstatus("Z-Offset Stopped", true); + + while (G29_lcd_clicked()) idle(); + + UBL_has_control_of_LCD_Panel = 0; + restore_UBL_active_state_and_leave(); + goto LEAVE; + } + } + UBL_has_control_of_LCD_Panel = 0; + delay(20); // We don't want any switch noise. + + blm.state.z_offset = measured_z; + + lcd_implementation_clear(); + restore_UBL_active_state_and_leave(); + } + } + + LEAVE: + #if ENABLED(ULTRA_LCD) + lcd_setstatus(" ", true); + lcd_quick_feedback(); + #endif + UBL_has_control_of_LCD_Panel = 0; + } + + void Find_Mean_Mesh_Height() { + int i, j, n; + float sum, sum_of_diff_squared, sigma, difference, mean; + + sum = sum_of_diff_squared = 0.0; + n = 0; + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + if (!isnan(z_values[i][j])) { + sum += z_values[i][j]; + n++; + } + } + } + mean = sum / n; + // + // Now do the sumation of the squares of difference from mean + // + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + if (!isnan(z_values[i][j])) { + difference = (z_values[i][j] - mean); + sum_of_diff_squared += difference * difference; + } + } + } + SERIAL_ECHOLNPAIR("# of samples: ", n); + SERIAL_ECHOPGM("Mean Mesh Height: "); + SERIAL_PROTOCOL_F(mean, 6); + SERIAL_EOL; + + sigma = sqrt( sum_of_diff_squared / (n + 1)); + SERIAL_ECHOPGM("Standard Deviation: "); + SERIAL_PROTOCOL_F(sigma, 6); + SERIAL_EOL; + + if (C_Flag) + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) + if (!isnan(z_values[i][j])) + z_values[i][j] -= mean + Constant; + } + + void Shift_Mesh_Height( ) { + for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) + for (uint8_t j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) + if (!isnan(z_values[i][j])) + z_values[i][j] += Constant; + } + + // probe_entire_mesh(X_Pos, Y_Pos) probes all invalidated locations of the mesh that can be reached + // by the probe. It attempts to fill in locations closest to the nozzle's start location first. + + void probe_entire_mesh(float X_Pos, float Y_Pos, bool do_UBL_MESH_Map, bool stow_probe) { + mesh_index_pair location; + float xProbe, yProbe, measured_z; + + UBL_has_control_of_LCD_Panel++; + save_UBL_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe + DEPLOY_PROBE(); + + do { + if (G29_lcd_clicked()) { + SERIAL_PROTOCOLLNPGM("\nMesh only partially populated."); + lcd_quick_feedback(); + while (G29_lcd_clicked()) idle(); + UBL_has_control_of_LCD_Panel = 0; + STOW_PROBE(); + restore_UBL_active_state_and_leave(); + return; + } + location = find_closest_mesh_point_of_type( INVALID, X_Pos, Y_Pos, 1, NULL); // the '1' says we want the location to be relative to the probe + if (location.x_index>=0 && location.y_index>=0) { + xProbe = blm.map_x_index_to_bed_location(location.x_index); + yProbe = blm.map_y_index_to_bed_location(location.y_index); + if (xProbe < MIN_PROBE_X || xProbe > MAX_PROBE_X || yProbe < MIN_PROBE_Y || yProbe > MAX_PROBE_Y) { + SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed."); + UBL_has_control_of_LCD_Panel = 0; + goto LEAVE; + } + measured_z = probe_pt(xProbe, yProbe, stow_probe, G29_Verbose_Level); + z_values[location.x_index][location.y_index] = measured_z + Z_PROBE_OFFSET_FROM_EXTRUDER; + } + + if (do_UBL_MESH_Map) blm.display_map(1); + } while (location.x_index >= 0 && location.y_index >= 0); + + LEAVE: + STOW_PROBE(); + restore_UBL_active_state_and_leave(); + + X_Pos = constrain( X_Pos-X_PROBE_OFFSET_FROM_EXTRUDER, X_MIN_POS, X_MAX_POS); + Y_Pos = constrain( Y_Pos-Y_PROBE_OFFSET_FROM_EXTRUDER, Y_MIN_POS, Y_MAX_POS); + + do_blocking_move_to_xy(X_Pos, Y_Pos); + } + + struct vector tilt_mesh_based_on_3pts(float pt1, float pt2, float pt3) { + struct vector v1, v2, normal; + float c, d, t; + int i, j; + + v1.dx = (ubl_3_point_1_X - ubl_3_point_2_X); + v1.dy = (ubl_3_point_1_Y - ubl_3_point_2_Y); + v1.dz = (pt1 - pt2); + + v2.dx = (ubl_3_point_3_X - ubl_3_point_2_X); + v2.dy = (ubl_3_point_3_Y - ubl_3_point_2_Y); + v2.dz = (pt3 - pt2); + + // do cross product + + normal.dx = v1.dy * v2.dz - v1.dz * v2.dy; + normal.dy = v1.dz * v2.dx - v1.dx * v2.dz; + normal.dz = v1.dx * v2.dy - v1.dy * v2.dx; + + // printf("[%f,%f,%f] ", normal.dx, normal.dy, normal.dz); + + normal.dx /= normal.dz; // This code does two things. This vector is normal to the tilted plane. + normal.dy /= normal.dz; // However, we don't know its direction. We need it to point up. So if + normal.dz /= normal.dz; // Z is negative, we need to invert the sign of all components of the vector + // We also need Z to be unity because we are going to be treating this triangle + // as the sin() and cos() of the bed's tilt + + // + // All of 3 of these points should give us the same d constant + // + t = normal.dx * ubl_3_point_1_X + normal.dy * ubl_3_point_1_Y; + d = t + normal.dz * pt1; + c = d - t; + SERIAL_ECHOPGM("d from 1st point: "); + SERIAL_PROTOCOL_F(d, 6); + SERIAL_ECHOPGM(" c: "); + SERIAL_PROTOCOL_F(c, 6); + SERIAL_EOL; + t = normal.dx * ubl_3_point_2_X + normal.dy * ubl_3_point_2_Y; + d = t + normal.dz * pt2; + c = d - t; + SERIAL_ECHOPGM("d from 2nd point: "); + SERIAL_PROTOCOL_F(d, 6); + SERIAL_ECHOPGM(" c: "); + SERIAL_PROTOCOL_F(c, 6); + SERIAL_EOL; + t = normal.dx * ubl_3_point_3_X + normal.dy * ubl_3_point_3_Y; + d = t + normal.dz * pt3; + c = d - t; + SERIAL_ECHOPGM("d from 3rd point: "); + SERIAL_PROTOCOL_F(d, 6); + SERIAL_ECHOPGM(" c: "); + SERIAL_PROTOCOL_F(c, 6); + SERIAL_EOL; + + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + c = -((normal.dx * (UBL_MESH_MIN_X + i * (MESH_X_DIST)) + normal.dy * (UBL_MESH_MIN_Y + j * (MESH_Y_DIST))) - d); + z_values[i][j] += c; + } + } + return normal; + } + + float use_encoder_wheel_to_measure_point() { + UBL_has_control_of_LCD_Panel++; + while (!G29_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! + idle(); + if (G29_encoderDiff != 0) { + float new_z; + // We define a new variable so we can know ahead of time where we are trying to go. + // The reason is we want G29_encoderDiff cleared so an interrupt can update it even before the move + // is complete. (So the dial feels responsive to user) + new_z = current_position[Z_AXIS] + 0.01 * float(G29_encoderDiff); + G29_encoderDiff = 0; + do_blocking_move_to_z(new_z); + } + } + while (G29_lcd_clicked()) idle(); // debounce and wait + UBL_has_control_of_LCD_Panel--; + return current_position[Z_AXIS]; + } + + float measure_business_card_thickness(float Height_Value) { + float Z1, Z2; + + UBL_has_control_of_LCD_Panel++; + save_UBL_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe + + SERIAL_PROTOCOLLNPGM("Place Shim Under Nozzle and Perform Measurement."); + do_blocking_move_to_z(Height_Value); + do_blocking_move_to_xy((float(X_MAX_POS) - float(X_MIN_POS)) / 2.0, (float(Y_MAX_POS) - float(Y_MIN_POS)) / 2.0); + //, min( planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])/2.0); + + Z1 = use_encoder_wheel_to_measure_point(); + do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); + UBL_has_control_of_LCD_Panel = 0; + + SERIAL_PROTOCOLLNPGM("Remove Shim and Measure Bed Height."); + Z2 = use_encoder_wheel_to_measure_point(); + do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); + + if (G29_Verbose_Level > 1) { + SERIAL_ECHOPGM("Business Card is: "); + SERIAL_PROTOCOL_F(abs(Z1 - Z2), 6); + SERIAL_PROTOCOLLNPGM("mm thick."); + } + restore_UBL_active_state_and_leave(); + return abs(Z1 - Z2); + } + + void manually_probe_remaining_mesh(float X_Pos, float Y_Pos, float z_clearance, float card_thickness, bool do_UBL_MESH_Map) { + mesh_index_pair location; + float last_x, last_y, dx, dy, + xProbe, yProbe; + unsigned long cnt; + + UBL_has_control_of_LCD_Panel++; + last_x = last_y = -9999.99; + save_UBL_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe + do_blocking_move_to_z(z_clearance); + do_blocking_move_to_xy(X_Pos, Y_Pos); + + do { + if (do_UBL_MESH_Map) blm.display_map(1); + + location = find_closest_mesh_point_of_type(INVALID, X_Pos, Y_Pos, 0, NULL); // The '0' says we want to use the nozzle's position + // It doesn't matter if the probe can not reach the + // NAN location. This is a manual probe. + if (location.x_index < 0 && location.y_index < 0) continue; + + xProbe = blm.map_x_index_to_bed_location(location.x_index); + yProbe = blm.map_y_index_to_bed_location(location.y_index); + if (xProbe < (X_MIN_POS) || xProbe > (X_MAX_POS) || yProbe < (Y_MIN_POS) || yProbe > (Y_MAX_POS)) { + SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed."); + UBL_has_control_of_LCD_Panel = 0; + goto LEAVE; + } + + dx = xProbe - last_x; + dy = yProbe - last_y; + + if (HYPOT(dx, dy) < BIG_RAISE_NOT_NEEDED) + do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); + else + do_blocking_move_to_z(z_clearance); + + last_x = xProbe; + last_y = yProbe; + do_blocking_move_to_xy(xProbe, yProbe); + + while (!G29_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! + idle(); + if (G29_encoderDiff) { + float new_z; + // We define a new variable so we can know ahead of time where we are trying to go. + // The reason is we want G29_encoderDiff cleared so an interrupt can update it even before the move + // is complete. (So the dial feels responsive to user) + new_z = current_position[Z_AXIS] + float(G29_encoderDiff) / 100.0; + G29_encoderDiff = 0; + do_blocking_move_to_z(new_z); + } + } + + cnt = millis(); + while (G29_lcd_clicked()) { // debounce and watch for abort + idle(); + if (millis() - cnt > 1500L) { + SERIAL_PROTOCOLLNPGM("\nMesh only partially populated."); + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); + lcd_quick_feedback(); + while (G29_lcd_clicked()) idle(); + UBL_has_control_of_LCD_Panel = 0; + restore_UBL_active_state_and_leave(); + return; + } + } + + z_values[location.x_index][location.y_index] = current_position[Z_AXIS] - card_thickness; + if (G29_Verbose_Level > 2) { + SERIAL_PROTOCOL("Mesh Point Measured at: "); + SERIAL_PROTOCOL_F(z_values[location.x_index][location.y_index], 6); + SERIAL_EOL; + } + } while (location.x_index >= 0 && location.y_index >= 0); + + if (do_UBL_MESH_Map) blm.display_map(1); + + LEAVE: + restore_UBL_active_state_and_leave(); + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); + do_blocking_move_to_xy(X_Pos, Y_Pos); + } + + bool G29_Parameter_Parsing() { + + #if ENABLED(ULTRA_LCD) + lcd_setstatus("Doing G29 UBL !", true); + lcd_quick_feedback(); + #endif + + X_Pos = current_position[X_AXIS]; + Y_Pos = current_position[Y_AXIS]; + X_Flag = Y_Flag = Repeat_Flag = UBL_OK; + Constant = 0.0; + Repetition_Cnt = 1; + + if ((X_Flag = code_seen('X'))) { + X_Pos = code_value_float(); + if (X_Pos < X_MIN_POS || X_Pos > X_MAX_POS) { + SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n"); + return UBL_ERR; + } + } + + if ((Y_Flag = code_seen('Y'))) { + Y_Pos = code_value_float(); + if (Y_Pos < Y_MIN_POS || Y_Pos > Y_MAX_POS) { + SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n"); + return UBL_ERR; + } + } + + if (X_Flag != Y_Flag) { + SERIAL_PROTOCOLLNPGM("Both X & Y locations must be specified.\n"); + return UBL_ERR; + } + + G29_Verbose_Level = 0; + if (code_seen('V')) { + G29_Verbose_Level = code_value_int(); + if (G29_Verbose_Level < 0 || G29_Verbose_Level > 4) { + SERIAL_PROTOCOLLNPGM("Invalid Verbose Level specified. (0-4)\n"); + return UBL_ERR; + } + } + + if (code_seen('A')) { // Activate the Unified Bed Leveling System + blm.state.active = 1; + SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System activated.\n"); + blm.store_state(); + } + + if ((C_Flag = code_seen('C')) && code_has_value()) + Constant = code_value_float(); + + if (code_seen('D')) { // Disable the Unified Bed Leveling System + blm.state.active = 0; + SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System de-activated.\n"); + blm.store_state(); + } + + if (code_seen('F')) { + blm.state.G29_Correction_Fade_Height = 10.00; + if (code_has_value()) { + blm.state.G29_Correction_Fade_Height = code_value_float(); + blm.state.G29_Fade_Height_Multiplier = 1.0 / blm.state.G29_Correction_Fade_Height; + } + if (blm.state.G29_Correction_Fade_Height<0.0 || blm.state.G29_Correction_Fade_Height>100.0) { + SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausable.\n"); + blm.state.G29_Correction_Fade_Height = 10.00; + blm.state.G29_Fade_Height_Multiplier = 1.0 / blm.state.G29_Correction_Fade_Height; + return UBL_ERR; + } + } + + if ((Repeat_Flag = code_seen('R'))) { + Repetition_Cnt = code_has_value() ? code_value_int() : 9999; + if (Repetition_Cnt < 1) { + SERIAL_PROTOCOLLNPGM("Invalid Repetition count.\n"); + return UBL_ERR; + } + } + return UBL_OK; + } + + /** + * This function goes away after G29 debug is complete. But for right now, it is a handy + * routine to dump binary data structures. + */ + void dump(char *str, float f) { + char *ptr; + + SERIAL_PROTOCOL(str); + SERIAL_PROTOCOL_F(f, 8); + SERIAL_PROTOCOL(" "); + ptr = (char *)&f; + for (uint8_t i = 0; i < 4; i++) { + SERIAL_PROTOCOL(" "); + prt_hex_byte(*ptr++); + } + SERIAL_PROTOCOL(" isnan()="); + SERIAL_PROTOCOL(isnan(f)); + SERIAL_PROTOCOL(" isinf()="); + SERIAL_PROTOCOL(isinf(f)); + + constexpr float g = INFINITY; + if (f == -g) + SERIAL_PROTOCOL(" Minus Infinity detected."); + + SERIAL_EOL; + } + + static int UBL_state_at_invokation = 0, + UBL_state_recursion_chk = 0; + + void save_UBL_active_state_and_disable() { + UBL_state_recursion_chk++; + if (UBL_state_recursion_chk != 1) { + SERIAL_ECHOLNPGM("save_UBL_active_state_and_disabled() called multiple times in a row."); + lcd_setstatus("save_UBL_active() error", true); + lcd_quick_feedback(); + return; + } + UBL_state_at_invokation = blm.state.active; + blm.state.active = 0; + return; + } + + void restore_UBL_active_state_and_leave() { + if (--UBL_state_recursion_chk) { + SERIAL_ECHOLNPGM("restore_UBL_active_state_and_leave() called too many times."); + lcd_setstatus("restore_UBL_active() error", true); + lcd_quick_feedback(); + return; + } + blm.state.active = UBL_state_at_invokation; + } + + /** + * Much of the 'What?' command can be eliminated. But until we are fully debugged, it is + * good to have the extra information. Soon... we prune this to just a few items + */ + void G29_What_Command() { + int k, i; + k = E2END - Unified_Bed_Leveling_EEPROM_start; + Statistics_Flag++; + + SERIAL_PROTOCOLPGM("Version #4: 10/30/2016 branch \n"); + SERIAL_PROTOCOLPGM("Unified Bed Leveling System "); + if (blm.state.active) + SERIAL_PROTOCOLPGM("Active."); + else + SERIAL_PROTOCOLPGM("Inactive."); + SERIAL_PROTOCOLLNPGM(" ------------------------------------- <----<<<"); // These arrows are just to help me + + if (blm.state.EEPROM_storage_slot == 0xFFFF) { + SERIAL_PROTOCOLPGM("No Mesh Loaded."); + SERIAL_PROTOCOLLNPGM(" ------------------------------------- <----<<<"); // These arrows are just to help me + // find this info buried in the clutter + } + else { + SERIAL_PROTOCOLPGM("Mesh: "); + prt_hex_word(blm.state.EEPROM_storage_slot); + SERIAL_PROTOCOLPGM(" Loaded. "); + SERIAL_PROTOCOLLNPGM(" -------------------------------------------------------- <----<<<"); // These arrows are just to help me + // find this info buried in the clutter + } + + SERIAL_ECHOPAIR("\nG29_Correction_Fade_Height : ", blm.state.G29_Correction_Fade_Height ); + SERIAL_PROTOCOLPGM(" ------------------------------------- <----<<< \n"); // These arrows are just to help me + // find this info buried in the clutter + idle(); + + SERIAL_ECHOPGM("z_offset: "); + SERIAL_PROTOCOL_F(blm.state.z_offset, 6); + SERIAL_PROTOCOLLNPGM(" ------------------------------------------------------------ <----<<<"); + + SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: "); + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + SERIAL_PROTOCOL_F( blm.map_x_index_to_bed_location(i), 1); + SERIAL_PROTOCOLPGM(" "); + } + SERIAL_EOL; + SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: "); + for (i = 0; i < UBL_MESH_NUM_Y_POINTS; i++) { + SERIAL_PROTOCOL_F( blm.map_y_index_to_bed_location(i), 1); + SERIAL_PROTOCOLPGM(" "); + } + SERIAL_EOL; + + #if HAS_KILL + SERIAL_ECHOPAIR("Kill pin on :", KILL_PIN); + SERIAL_ECHOLNPAIR(" state:", READ(KILL_PIN)); + #endif + + SERIAL_ECHOLNPAIR("UBL_state_at_invokation :", UBL_state_at_invokation); + SERIAL_ECHOLNPAIR("UBL_state_recursion_chk :", UBL_state_recursion_chk); + + SERIAL_EOL; + SERIAL_PROTOCOLPGM("Free EEPROM space starts at: 0x"); + prt_hex_word(Unified_Bed_Leveling_EEPROM_start); + SERIAL_EOL; + idle(); + + SERIAL_PROTOCOLPGM("end of EEPROM : "); + prt_hex_word(E2END); + SERIAL_EOL; + idle(); + + SERIAL_PROTOCOLLNPAIR("sizeof(blm) : ", (int)sizeof(blm)); + SERIAL_EOL; + SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values)); + SERIAL_EOL; + + SERIAL_PROTOCOLPGM("EEPROM free for UBL: 0x"); + prt_hex_word(k); + SERIAL_EOL; + idle(); + + SERIAL_PROTOCOLPGM("EEPROM can hold 0x"); + prt_hex_word(k / sizeof(z_values)); + SERIAL_PROTOCOLPGM(" meshes. \n"); + + SERIAL_PROTOCOLPGM("sizeof(stat) :"); + prt_hex_word(sizeof(blm.state)); + SERIAL_EOL; + idle(); + + SERIAL_ECHOPAIR("\nUBL_MESH_NUM_X_POINTS ", UBL_MESH_NUM_X_POINTS); + SERIAL_ECHOPAIR("\nUBL_MESH_NUM_Y_POINTS ", UBL_MESH_NUM_Y_POINTS); + SERIAL_ECHOPAIR("\nUBL_MESH_MIN_X ", UBL_MESH_MIN_X); + SERIAL_ECHOPAIR("\nUBL_MESH_MIN_Y ", UBL_MESH_MIN_Y); + SERIAL_ECHOPAIR("\nUBL_MESH_MAX_X ", UBL_MESH_MAX_X); + SERIAL_ECHOPAIR("\nUBL_MESH_MAX_Y ", UBL_MESH_MAX_Y); + SERIAL_ECHOPGM("\nMESH_X_DIST "); + SERIAL_PROTOCOL_F(MESH_X_DIST, 6); + SERIAL_ECHOPGM("\nMESH_Y_DIST "); + SERIAL_PROTOCOL_F(MESH_Y_DIST, 6); + SERIAL_EOL; + idle(); + + SERIAL_ECHOPAIR("\nsizeof(block_t): ", (int)sizeof(block_t)); + SERIAL_ECHOPAIR("\nsizeof(planner.block_buffer): ", (int)sizeof(planner.block_buffer)); + SERIAL_ECHOPAIR("\nsizeof(char): ", (int)sizeof(char)); + SERIAL_ECHOPAIR(" sizeof(unsigned char): ", (int)sizeof(unsigned char)); + SERIAL_ECHOPAIR("\nsizeof(int): ", (int)sizeof(int)); + SERIAL_ECHOPAIR(" sizeof(unsigned int): ", (int)sizeof(unsigned int)); + SERIAL_ECHOPAIR("\nsizeof(long): ", (int)sizeof(long)); + SERIAL_ECHOPAIR(" sizeof(unsigned long int): ", (int)sizeof(unsigned long int)); + SERIAL_ECHOPAIR("\nsizeof(float): ", (int)sizeof(float)); + SERIAL_ECHOPAIR(" sizeof(double): ", (int)sizeof(double)); + SERIAL_ECHOPAIR("\nsizeof(void *): ", (int)sizeof(void *)); + struct pf { void *p_f(); } ptr_func; + SERIAL_ECHOPAIR(" sizeof(struct pf): ", (int)sizeof(pf)); + SERIAL_ECHOPAIR(" sizeof(void *()): ", (int)sizeof(ptr_func)); + SERIAL_EOL; + + idle(); + + if (!blm.sanity_check()) + SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed."); + } + + /** + * When we are fully debugged, the EEPROM dump command will get deleted also. But + * right now, it is good to have the extra information. Soon... we prune this. + */ + void G29_EEPROM_Dump() { + unsigned char cccc; + int i, j, kkkk; + + SERIAL_ECHO_START; + SERIAL_ECHOPGM("EEPROM Dump:\n"); + for (i = 0; i < E2END + 1; i += 16) { + if (i & 0x3 == 0) idle(); + prt_hex_word(i); + SERIAL_ECHOPGM(": "); + for (j = 0; j < 16; j++) { + kkkk = i + j; + eeprom_read_block(&cccc, (void *)kkkk, 1); + prt_hex_byte(cccc); + SERIAL_ECHO(' '); + } + SERIAL_EOL; + } + SERIAL_EOL; + return; + } + + /** + * When we are fully debugged, this may go away. But there are some valid + * use cases for the users. So we can wait and see what to do with it. + */ + void G29_Kompare_Current_Mesh_to_Stored_Mesh() { + float tmp_z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS]; + int i, j, k; + + if (!code_has_value()) { + SERIAL_PROTOCOLLNPGM("?Mesh # required.\n"); + return; + } + Storage_Slot = code_value_int(); + + k = E2END - sizeof(blm.state); + j = (k - Unified_Bed_Leveling_EEPROM_start) / sizeof(tmp_z_values); + + if (Storage_Slot < 0 || Storage_Slot > j || Unified_Bed_Leveling_EEPROM_start <= 0) { + SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); + return; + } + + j = k - (Storage_Slot + 1) * sizeof(tmp_z_values); + eeprom_read_block((void *)&tmp_z_values, (void *)j, sizeof(tmp_z_values)); + + SERIAL_ECHOPAIR("Subtracting Mesh ", Storage_Slot); + SERIAL_PROTOCOLPGM(" loaded from EEPROM address "); // Soon, we can remove the extra clutter of printing + prt_hex_word(j); // the address in the EEPROM where the Mesh is stored. + SERIAL_EOL; + + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) + z_values[i][j] = z_values[i][j] - tmp_z_values[i][j]; + } + + mesh_index_pair find_closest_mesh_point_of_type(Mesh_Point_Type type, float X, float Y, bool probe_as_reference, unsigned int bits[16]) { + int i, j; + float f, px, py, mx, my, dx, dy, closest = 99999.99; + float current_x, current_y, distance; + mesh_index_pair return_val; + + return_val.x_index = return_val.y_index = -1; + + current_x = current_position[X_AXIS]; + current_y = current_position[Y_AXIS]; + + px = X; // Get our reference position. Either the nozzle or + py = Y; // the probe location. + if (probe_as_reference) { + px -= X_PROBE_OFFSET_FROM_EXTRUDER; + py -= Y_PROBE_OFFSET_FROM_EXTRUDER; + } + + for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { + for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { + + if ( (type == INVALID && isnan(z_values[i][j])) // Check to see if this location holds the right thing + || (type == REAL && !isnan(z_values[i][j])) + || (type == SET_IN_BITMAP && is_bit_set(bits, i, j)) + ) { + + // We only get here if we found a Mesh Point of the specified type + + mx = blm.map_x_index_to_bed_location(i); // Check if we can probe this mesh location + my = blm.map_y_index_to_bed_location(j); + + // If we are using the probe as the reference + // there are some locations we can't get to. + // We prune these out of the list and ignore + // them until the next Phase where we do the + // manual nozzle probing. + if (probe_as_reference + && (mx < (MIN_PROBE_X) || mx > (MAX_PROBE_X)) + && (my < (MIN_PROBE_Y) || my > (MAX_PROBE_Y)) + ) continue; + + dx = px - mx; // We can get to it. Let's see if it is the + dy = py - my; // closest location to the nozzle. + distance = HYPOT(dx, dy); + + dx = current_x - mx; // We are going to add in a weighting factor that considers + dy = current_y - my; // the current location of the nozzle. If two locations are equal + distance += HYPOT(dx, dy) * 0.01; // distance from the measurement location, we are going to give + + if (distance < closest) { + closest = distance; // We found a closer location with + return_val.x_index = i; // the specified type of mesh value. + return_val.y_index = j; + return_val.distance = closest; + } + } + } + } + return return_val; + } + + void fine_tune_mesh(float X_Pos, float Y_Pos, float Height_Value, bool do_UBL_MESH_Map) { + mesh_index_pair location; + float xProbe, yProbe, new_z; + uint16_t i, not_done[16]; + long round_off; + + save_UBL_active_state_and_disable(); + memset(not_done, 0xFF, sizeof(not_done)); + + #if ENABLED(ULTRA_LCD) + lcd_setstatus("Fine Tuning Mesh.", true); + #endif + + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); + do_blocking_move_to_xy(X_Pos, Y_Pos); + do { + if (do_UBL_MESH_Map) blm.display_map(1); + + location = find_closest_mesh_point_of_type( SET_IN_BITMAP, X_Pos, Y_Pos, 0, not_done); // The '0' says we want to use the nozzle's position + // It doesn't matter if the probe can not reach this + // location. This is a manual edit of the Mesh Point. + if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points. + + bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a + // different location the next time through the loop + + xProbe = blm.map_x_index_to_bed_location(location.x_index); + yProbe = blm.map_y_index_to_bed_location(location.y_index); + if (xProbe < X_MIN_POS || xProbe > X_MAX_POS || yProbe < Y_MIN_POS || yProbe > Y_MAX_POS) { // In theory, we don't need this check. + SERIAL_PROTOCOLLNPGM("?Error: Attempt to edit off the bed."); // This really can't happen, but for now, + UBL_has_control_of_LCD_Panel = 0; // Let's do the check. + goto FINE_TUNE_EXIT; + } + + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); // Move the nozzle to where we are going to edit + do_blocking_move_to_xy(xProbe, yProbe); + new_z = z_values[location.x_index][location.y_index] + 0.001; + + round_off = (int32_t)(new_z * 1000.0 + 2.5); // we chop off the last digits just to be clean. We are rounding to the + round_off -= (round_off % 5L); // closest 0 or 5 at the 3rd decimal place. + new_z = ((float)(round_off)) / 1000.0; + + //SERIAL_ECHOPGM("Mesh Point Currently At: "); + //SERIAL_PROTOCOL_F(new_z, 6); + //SERIAL_EOL; + + lcd_implementation_clear(); + lcd_mesh_edit_setup(new_z); + UBL_has_control_of_LCD_Panel++; + do { + new_z = lcd_mesh_edit(); + idle(); + } while (!G29_lcd_clicked()); + + UBL_has_control_of_LCD_Panel = 1; // There is a race condition for the Encoder Wheel getting clicked. + // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune( ) + // or here. + millis_t nxt = millis() + 1500UL; + lcd_return_to_status(); + while (G29_lcd_clicked()) { // debounce and watch for abort + idle(); + if (ELAPSED(millis(), nxt)) { + lcd_return_to_status(); + SERIAL_PROTOCOLLNPGM("\nFine Tuning of Mesh Stopped."); + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); + lcd_setstatus("Mesh Editing Stopped", true); + + while (G29_lcd_clicked()) idle(); + + UBL_has_control_of_LCD_Panel = 0; + goto FINE_TUNE_EXIT; + } + } + //UBL_has_control_of_LCD_Panel = 0; + delay(20); // We don't want any switch noise. + + z_values[location.x_index][location.y_index] = new_z; + + lcd_implementation_clear(); + + } while (location.x_index >= 0 && location.y_index >= 0 && --Repetition_Cnt); + + FINE_TUNE_EXIT: + + if (do_UBL_MESH_Map) blm.display_map(1); + restore_UBL_active_state_and_leave(); + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); + + do_blocking_move_to_xy(X_Pos, Y_Pos); + + UBL_has_control_of_LCD_Panel = 0; + + #if ENABLED(ULTRA_LCD) + lcd_setstatus("Done Editing Mesh", true); + #endif + SERIAL_ECHOLNPGM("Done Editing Mesh."); + } + +#endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/UBL_line_to_destination.cpp b/Marlin/UBL_line_to_destination.cpp new file mode 100644 index 0000000000..bb3956dc38 --- /dev/null +++ b/Marlin/UBL_line_to_destination.cpp @@ -0,0 +1,553 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "Marlin.h" + +#if ENABLED(AUTO_BED_LEVELING_UBL) + + #include "UBL.h" + #include "planner.h" + #include + #include + + extern void set_current_to_destination(); + extern bool G26_Debug_flag; + void debug_current_and_destination(char *title); + + void wait_for_button_press(); + + void UBL_line_to_destination(const float &x_end, const float &y_end, const float &z_end, const float &e_end, const float &feed_rate, uint8_t extruder) { + + int cell_start_xi, cell_start_yi, cell_dest_xi, cell_dest_yi; + int left_flag, down_flag; + int current_xi, current_yi; + int dxi, dyi, xi_cnt, yi_cnt; + bool use_X_dist, inf_normalized_flag, inf_m_flag; + float x_start, y_start; + float x, y, z1, z2, z0 /*, z_optimized */; + float next_mesh_line_x, next_mesh_line_y, a0ma1diva2ma1; + float on_axis_distance, e_normalized_dist, e_position, e_start, z_normalized_dist, z_position, z_start; + float dx, dy, adx, ady, m, c; + + // + // Much of the nozzle movement will be within the same cell. So we will do as little computation + // as possible to determine if this is the case. If this move is within the same cell, we will + // just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave + // + + x_start = current_position[X_AXIS]; + y_start = current_position[Y_AXIS]; + z_start = current_position[Z_AXIS]; + e_start = current_position[E_AXIS]; + + cell_start_xi = blm.get_cell_index_x(x_start); + cell_start_yi = blm.get_cell_index_y(y_start); + cell_dest_xi = blm.get_cell_index_x(x_end); + cell_dest_yi = blm.get_cell_index_y(y_end); + + if (G26_Debug_flag!=0) { + SERIAL_ECHOPGM(" UBL_line_to_destination(xe="); + SERIAL_ECHO(x_end); + SERIAL_ECHOPGM(",ye="); + SERIAL_ECHO(y_end); + SERIAL_ECHOPGM(",ze="); + SERIAL_ECHO(z_end); + SERIAL_ECHOPGM(",ee="); + SERIAL_ECHO(e_end); + SERIAL_ECHOPGM(")\n"); + debug_current_and_destination( (char *) "Start of UBL_line_to_destination()"); + } + + if ((cell_start_xi == cell_dest_xi) && (cell_start_yi == cell_dest_yi)) { // if the whole move is within the same cell, + // we don't need to break up the move + // + // If we are moving off the print bed, we are going to allow the move at this level. + // But we detect it and isolate it. For now, we just pass along the request. + // + + if (cell_dest_xi<0 || cell_dest_yi<0 || cell_dest_xi >= UBL_MESH_NUM_X_POINTS || cell_dest_yi >= UBL_MESH_NUM_Y_POINTS) { + + // Note: There is no Z Correction in this case. We are off the grid and don't know what + // a reasonable correction would be. + + planner.buffer_line(x_end, y_end, z_end + blm.state.z_offset, e_end, feed_rate, extruder); + set_current_to_destination(); + if (G26_Debug_flag!=0) { + debug_current_and_destination( (char *) "out of bounds in UBL_line_to_destination()"); + } + return; + } + + // we can optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to + // generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function. + // We are going to only calculate the amount we are from the first mesh line towards the second mesh line once. + // We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And, + // instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor + // to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide. + + FINAL_MOVE: + a0ma1diva2ma1 = (x_end - mesh_index_to_X_location[cell_dest_xi]) * (float) (1.0 / MESH_X_DIST); + + z1 = z_values[cell_dest_xi][cell_dest_yi] + + (z_values[cell_dest_xi + 1][cell_dest_yi] - z_values[cell_dest_xi][cell_dest_yi]) * a0ma1diva2ma1; + + z2 = z_values[cell_dest_xi][cell_dest_yi+1] + + (z_values[cell_dest_xi+1][cell_dest_yi+1] - z_values[cell_dest_xi][cell_dest_yi+1]) * a0ma1diva2ma1; + + // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we + // are going to apply the Y-Distance into the cell to interpolate the final Z correction. + + a0ma1diva2ma1 = (y_end - mesh_index_to_Y_location[cell_dest_yi]) * (float) (1.0 / MESH_Y_DIST); + + z0 = z1 + (z2 - z1) * a0ma1diva2ma1; + + // debug code to use non-optimized get_z_correction() and to do a sanity check + // that the correct value is being passed to planner.buffer_line() + // + /* + z_optimized = z0; + z0 = blm.get_z_correction( x_end, y_end); + if ( fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized) ) { + debug_current_and_destination( (char *) "FINAL_MOVE: z_correction()"); + if ( isnan(z0) ) SERIAL_ECHO(" z0==NAN "); + if ( isnan(z_optimized) ) SERIAL_ECHO(" z_optimized==NAN "); + SERIAL_ECHOPAIR(" x_end=", x_end); + SERIAL_ECHOPAIR(" y_end=", y_end); + SERIAL_ECHOPAIR(" z0=", z0); + SERIAL_ECHOPAIR(" z_optimized=", z_optimized); + SERIAL_ECHOPAIR(" err=",fabs(z_optimized - z0)); + SERIAL_EOL; + } + */ + z0 = z0 * blm.fade_scaling_factor_for_Z( z_end ); + + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + } + + planner.buffer_line(x_end, y_end, z_end + z0 + blm.state.z_offset, e_end, feed_rate, extruder); + if (G26_Debug_flag!=0) { + debug_current_and_destination( (char *) "FINAL_MOVE in UBL_line_to_destination()"); + } + set_current_to_destination(); + return; + } + + // + // If we get here, we are processing a move that crosses at least one Mesh Line. We will check + // for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details + // of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less + // computation and in fact most lines are of this nature. We will check for that in the following + // blocks of code: + + left_flag = 0; + down_flag = 0; + inf_m_flag = false; + inf_normalized_flag = false; + + dx = x_end - x_start; + dy = y_end - y_start; + + if (dx<0.0) { // figure out which way we need to move to get to the next cell + dxi = -1; + adx = -dx; // absolute value of dx. We already need to check if dx and dy are negative. + } + else { // We may as well generate the appropriate values for adx and ady right now + dxi = 1; // to save setting up the abs() function call and actually doing the call. + adx = dx; + } + if (dy<0.0) { + dyi = -1; + ady = -dy; // absolute value of dy + } + else { + dyi = 1; + ady = dy; + } + + if (dx<0.0) left_flag = 1; + if (dy<0.0) down_flag = 1; + if (cell_start_xi == cell_dest_xi) dxi = 0; + if (cell_start_yi == cell_dest_yi) dyi = 0; + + // + // Compute the scaling factor for the extruder for each partial move. + // We need to watch out for zero length moves because it will cause us to + // have an infinate scaling factor. We are stuck doing a floating point + // divide to get our scaling factor, but after that, we just multiply by this + // number. We also pick our scaling factor based on whether the X or Y + // component is larger. We use the biggest of the two to preserve precision. + // + if ( adx > ady ) { + use_X_dist = true; + on_axis_distance = x_end-x_start; + } + else { + use_X_dist = false; + on_axis_distance = y_end-y_start; + } + e_position = e_end - e_start; + e_normalized_dist = e_position / on_axis_distance; + + z_position = z_end - z_start; + z_normalized_dist = z_position / on_axis_distance; + + if (e_normalized_dist==INFINITY || e_normalized_dist==-INFINITY) { + inf_normalized_flag = true; + } + current_xi = cell_start_xi; + current_yi = cell_start_yi; + + m = dy / dx; + c = y_start - m*x_start; + if (m == INFINITY || m == -INFINITY) { + inf_m_flag = true; + } + // + // This block handles vertical lines. These are lines that stay within the same + // X Cell column. They do not need to be perfectly vertical. They just can + // not cross into another X Cell column. + // + if (dxi == 0) { // Check for a vertical line + current_yi += down_flag; // Line is heading down, we just want to go to the bottom + while (current_yi != cell_dest_yi + down_flag) { + current_yi += dyi; + next_mesh_line_y = mesh_index_to_Y_location[current_yi]; + if (inf_m_flag) { + x = x_start; // if the slope of the line is infinite, we won't do the calculations + } + // we know the next X is the same so we can recover and continue! + else { + x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line + } + + z0 = blm.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi); + + // + // debug code to use non-optimized get_z_correction() and to do a sanity check + // that the correct value is being passed to planner.buffer_line() + // + /* + z_optimized = z0; + z0 = blm.get_z_correction( x, next_mesh_line_y); + if ( fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized) ) { + debug_current_and_destination( (char *) "VERTICAL z_correction()"); + if ( isnan(z0) ) SERIAL_ECHO(" z0==NAN "); + if ( isnan(z_optimized) ) SERIAL_ECHO(" z_optimized==NAN "); + SERIAL_ECHOPAIR(" x=", x); + SERIAL_ECHOPAIR(" next_mesh_line_y=", next_mesh_line_y); + SERIAL_ECHOPAIR(" z0=", z0); + SERIAL_ECHOPAIR(" z_optimized=", z_optimized); + SERIAL_ECHOPAIR(" err=",fabs(z_optimized-z0)); + SERIAL_ECHO("\n"); + } + */ + + z0 = z0 * blm.fade_scaling_factor_for_Z( z_end ); + + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + } + y = mesh_index_to_Y_location[current_yi]; + + // Without this check, it is possible for the algorythm to generate a zero length move in the case + // where the line is heading down and it is starting right on a Mesh Line boundary. For how often that + // happens, it might be best to remove the check and always 'schedule' the move because + // the planner.buffer_line() routine will filter it if that happens. + if ( y!=y_start) { + if ( inf_normalized_flag == false ) { + on_axis_distance = y - y_start; // we don't need to check if the extruder position + e_position = e_start + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a vertical move + z_position = z_start + on_axis_distance * z_normalized_dist; + } + else { + e_position = e_start; + z_position = z_start; + } + + planner.buffer_line(x, y, z_position + z0 + blm.state.z_offset, e_position, feed_rate, extruder); + } //else printf("FIRST MOVE PRUNED "); + } + // + // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done. + // + if (G26_Debug_flag!=0) { + debug_current_and_destination( (char *) "vertical move done in UBL_line_to_destination()"); + } + if (current_position[X_AXIS] != x_end || current_position[Y_AXIS] != y_end) { + goto FINAL_MOVE; + } + set_current_to_destination(); + return; + } + + // + // This block handles horizontal lines. These are lines that stay within the same + // Y Cell row. They do not need to be perfectly horizontal. They just can + // not cross into another Y Cell row. + // + + if (dyi == 0) { // Check for a horiziontal line + current_xi += left_flag; // Line is heading left, we just want to go to the left + // edge of this cell for the first move. + while (current_xi != cell_dest_xi + left_flag) { + current_xi += dxi; + next_mesh_line_x = mesh_index_to_X_location[current_xi]; + y = m * next_mesh_line_x + c; // Calculate X at the next Y mesh line + + z0 = blm.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi); + + // + // debug code to use non-optimized get_z_correction() and to do a sanity check + // that the correct value is being passed to planner.buffer_line() + // + /* + z_optimized = z0; + z0 = blm.get_z_correction( next_mesh_line_x, y); + if ( fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized) ) { + debug_current_and_destination( (char *) "HORIZONTAL z_correction()"); + if ( isnan(z0) ) SERIAL_ECHO(" z0==NAN "); + if ( isnan(z_optimized) ) SERIAL_ECHO(" z_optimized==NAN "); + SERIAL_ECHOPAIR(" next_mesh_line_x=", next_mesh_line_x); + SERIAL_ECHOPAIR(" y=", y); + SERIAL_ECHOPAIR(" z0=", z0); + SERIAL_ECHOPAIR(" z_optimized=", z_optimized); + SERIAL_ECHOPAIR(" err=",fabs(z_optimized-z0)); + SERIAL_ECHO("\n"); + } + */ + + z0 = z0 * blm.fade_scaling_factor_for_Z( z_end ); + + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + } + x = mesh_index_to_X_location[current_xi]; + + // Without this check, it is possible for the algorythm to generate a zero length move in the case + // where the line is heading left and it is starting right on a Mesh Line boundary. For how often + // that happens, it might be best to remove the check and always 'schedule' the move because + // the planner.buffer_line() routine will filter it if that happens. + if ( x!=x_start) { + if ( inf_normalized_flag == false ) { + on_axis_distance = x - x_start; // we don't need to check if the extruder position + e_position = e_start + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move + z_position = z_start + on_axis_distance * z_normalized_dist; + } + else { + e_position = e_start; + z_position = z_start; + } + + planner.buffer_line(x, y, z_position + z0 + blm.state.z_offset, e_position, feed_rate, extruder); + } //else printf("FIRST MOVE PRUNED "); + } + if (G26_Debug_flag!=0) { + debug_current_and_destination( (char *) "horizontal move done in UBL_line_to_destination()"); + } + if (current_position[X_AXIS] != x_end || current_position[Y_AXIS] != y_end) { + goto FINAL_MOVE; + } + set_current_to_destination(); + return; + } + + // + // + // + // + // This block handles the generic case of a line crossing both X and Y + // Mesh lines. + // + // + // + // + + xi_cnt = cell_start_xi - cell_dest_xi; + if ( xi_cnt < 0 ) { + xi_cnt = -xi_cnt; + } + + yi_cnt = cell_start_yi - cell_dest_yi; + if ( yi_cnt < 0 ) { + yi_cnt = -yi_cnt; + } + + current_xi += left_flag; + current_yi += down_flag; + + while ( xi_cnt>0 || yi_cnt>0 ) { + + next_mesh_line_x = mesh_index_to_X_location[current_xi + dxi]; + next_mesh_line_y = mesh_index_to_Y_location[current_yi + dyi]; + + y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line + x = (next_mesh_line_y-c) / m; // Calculate X at the next Y mesh line (we don't have to worry + // about m being equal to 0.0 If this was the case, we would have + // detected this as a vertical line move up above and we wouldn't + // be down here doing a generic type of move. + + if ((left_flag && (x>next_mesh_line_x)) || (!left_flag && (x .01 || isnan(z0) || isnan(z_optimized) ) { + debug_current_and_destination( (char *) "General_1: z_correction()"); + if ( isnan(z0) ) SERIAL_ECHO(" z0==NAN "); + if ( isnan(z_optimized) ) SERIAL_ECHO(" z_optimized==NAN "); { + SERIAL_ECHOPAIR(" x=", x); + } + SERIAL_ECHOPAIR(" next_mesh_line_y=", next_mesh_line_y); + SERIAL_ECHOPAIR(" z0=", z0); + SERIAL_ECHOPAIR(" z_optimized=", z_optimized); + SERIAL_ECHOPAIR(" err=",fabs(z_optimized-z0)); + SERIAL_ECHO("\n"); + } + */ + + z0 = z0 * blm.fade_scaling_factor_for_Z( z_end ); + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + } + + if ( inf_normalized_flag == false ) { + if ( use_X_dist ) { + on_axis_distance = x - x_start; + } + else { + on_axis_distance = next_mesh_line_y - y_start; + } + e_position = e_start + on_axis_distance * e_normalized_dist; + z_position = z_start + on_axis_distance * z_normalized_dist; + } + else { + e_position = e_start; + z_position = z_start; + } + planner.buffer_line(x, next_mesh_line_y, z_position + z0 + blm.state.z_offset, e_position, feed_rate, extruder); + current_yi += dyi; + yi_cnt--; + } + else { + // + // Yes! Crossing a X Mesh Line next + // + z0 = blm.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi+dxi, current_yi-down_flag); + + + // + // debug code to use non-optimized get_z_correction() and to do a sanity check + // that the correct value is being passed to planner.buffer_line() + // + /* + z_optimized = z0; + z0 = blm.get_z_correction( next_mesh_line_x, y); + if ( fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized) ) { + debug_current_and_destination( (char *) "General_2: z_correction()"); + if ( isnan(z0) ) SERIAL_ECHO(" z0==NAN "); + if ( isnan(z_optimized) ) SERIAL_ECHO(" z_optimized==NAN "); + SERIAL_ECHOPAIR(" next_mesh_line_x=", next_mesh_line_x); + SERIAL_ECHOPAIR(" y=", y); + SERIAL_ECHOPAIR(" z0=", z0); + SERIAL_ECHOPAIR(" z_optimized=", z_optimized); + SERIAL_ECHOPAIR(" err=",fabs(z_optimized-z0)); + SERIAL_ECHO("\n"); + } + */ + + z0 = z0 * blm.fade_scaling_factor_for_Z( z_end ); + + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + } + if ( inf_normalized_flag == false ) { + if ( use_X_dist ) { + on_axis_distance = next_mesh_line_x - x_start; + } + else { + on_axis_distance = y - y_start; + } + e_position = e_start + on_axis_distance * e_normalized_dist; + z_position = z_start + on_axis_distance * z_normalized_dist; + } + else { + e_position = e_start; + z_position = z_start; + } + + planner.buffer_line(next_mesh_line_x, y, z_position + z0 + blm.state.z_offset, e_position, feed_rate, extruder); + current_xi += dxi; + xi_cnt--; + } + } + if (G26_Debug_flag) { + debug_current_and_destination( (char *) "generic move done in UBL_line_to_destination()"); + } + if (current_position[0] != x_end || current_position[1] != y_end) { + goto FINAL_MOVE; + } + set_current_to_destination(); + return; + } + + void wait_for_button_press() { + // if ( !been_to_2_6 ) + //return; // bob - I think this should be commented out + + SET_INPUT_PULLUP(66); // Roxy's Left Switch is on pin 66. Right Switch is on pin 65 + SET_OUTPUT(64); + while (READ(66) & 0x01) idle(); + + delay(50); + while (!(READ(66) & 0x01)) idle(); + delay(50); + } + +#endif + + diff --git a/Marlin/hex_print_routines.cpp b/Marlin/hex_print_routines.cpp new file mode 100644 index 0000000000..19563ccb88 --- /dev/null +++ b/Marlin/hex_print_routines.cpp @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + + +#include "Marlin.h" +#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(M100_FREE_MEMORY_WATCHER) + +#include "hex_print_routines.h" + +void prt_hex_nibble(uint8_t n) { + if (n <= 9) + SERIAL_ECHO(n); + else + SERIAL_ECHO((char)('A' + n - 10)); + delay(3); +} + +void prt_hex_byte(uint8_t b) { + prt_hex_nibble((b & 0xF0) >> 4); + prt_hex_nibble(b & 0x0F); +} + +void prt_hex_word(uint16_t w) { + prt_hex_byte((w & 0xFF00) >> 8); + prt_hex_byte(w & 0x0FF); +} + +#endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER diff --git a/Marlin/hex_print_routines.h b/Marlin/hex_print_routines.h new file mode 100644 index 0000000000..f6b7b28e2c --- /dev/null +++ b/Marlin/hex_print_routines.h @@ -0,0 +1,33 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef HEX_PRINT_ROUTINES_H +#define HEX_PRINT_ROUTINES_H + +// +// 3 support routines to print hex numbers. We can print a nibble, byte and word +// +void prt_hex_nibble(uint8_t n); +void prt_hex_byte(uint8_t b); +void prt_hex_word(uint16_t w); + +#endif // HEX_PRINT_ROUTINES_H \ No newline at end of file