12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754 |
-
-
- #include "MarlinConfig.h"
-
- #if ENABLED(AUTO_BED_LEVELING_UBL)
-
- #include "ubl.h"
- #include "Marlin.h"
- #include "hex_print_routines.h"
- #include "configuration_store.h"
- #include "ultralcd.h"
- #include "stepper.h"
- #include "gcode.h"
-
- #include <math.h>
- #include "least_squares_fit.h"
-
- #define UBL_G29_P31
-
- extern float destination[XYZE], current_position[XYZE];
-
- void lcd_return_to_status();
- void lcd_mesh_edit_setup(float initial);
- 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 probe_pt(const float &x, const float &y, bool, int);
- extern bool set_probe_deployed(bool);
-
- #define SIZE_OF_LITTLE_RAISE 1
- #define BIG_RAISE_NOT_NEEDED 0
-
- int unified_bed_leveling::g29_verbose_level,
- unified_bed_leveling::g29_phase_value,
- unified_bed_leveling::g29_repetition_cnt,
- unified_bed_leveling::g29_storage_slot = 0,
- unified_bed_leveling::g29_map_type,
- unified_bed_leveling::g29_grid_size;
- bool unified_bed_leveling::g29_c_flag,
- unified_bed_leveling::g29_x_flag,
- unified_bed_leveling::g29_y_flag;
- float unified_bed_leveling::g29_x_pos,
- unified_bed_leveling::g29_y_pos,
- unified_bed_leveling::g29_card_thickness = 0.0,
- unified_bed_leveling::g29_constant = 0.0;
-
-
-
-
- void unified_bed_leveling::G29() {
-
- if (!settings.calc_num_meshes()) {
- SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it");
- SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n");
- return;
- }
-
-
- if (axis_unhomed_error()) {
- const int8_t p_val = parser.seen('P') && parser.has_value() ? parser.value_int() : -1;
- if (p_val == 1 || p_val == 2 || p_val == 4 || parser.seen('J'))
- home_all_axes();
- }
-
- if (g29_parameter_parsing()) return;
-
-
-
- if (parser.seen('I')) {
- uint8_t cnt = 0;
- g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
- while (g29_repetition_cnt--) {
- if (cnt > 20) { cnt = 0; idle(); }
- const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
- if (location.x_index < 0) {
- SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
- break;
- }
- z_values[location.x_index][location.y_index] = NAN;
- cnt++;
- }
- SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
- }
-
- if (parser.seen('Q')) {
- const int test_pattern = parser.has_value() ? parser.value_int() : -99;
- if (!WITHIN(test_pattern, -1, 2)) {
- SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (-1 to 2)\n");
- return;
- }
- SERIAL_PROTOCOLLNPGM("Loading test_pattern values.\n");
- switch (test_pattern) {
- case -1:
- g29_eeprom_dump();
- break;
- case 0:
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
- const float p1 = 0.5 * (GRID_MAX_POINTS_X) - x,
- p2 = 0.5 * (GRID_MAX_POINTS_Y) - y;
- z_values[x][y] += 2.0 * HYPOT(p1, p2);
- }
- }
- break;
- case 1:
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
- z_values[x][x] += 9.999;
- z_values[x][x + (x < GRID_MAX_POINTS_Y - 1) ? 1 : -1] += 9.999;
- }
- break;
- case 2:
-
- for (uint8_t x = (GRID_MAX_POINTS_X) / 3; x < 2 * (GRID_MAX_POINTS_X) / 3; x++)
- for (uint8_t y = (GRID_MAX_POINTS_Y) / 3; y < 2 * (GRID_MAX_POINTS_Y) / 3; y++)
- z_values[x][y] += parser.seen('C') ? g29_constant : 9.99;
- break;
- }
- }
-
- if (parser.seen('J')) {
- if (g29_grid_size) {
- save_ubl_active_state_and_disable();
- tilt_mesh_based_on_probed_grid(parser.seen('T'));
- restore_ubl_active_state_and_leave();
- }
- else {
- float z3, z2, z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level);
- if (!isnan(z1)) {
- z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level);
- if (!isnan(z2))
- z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
- }
-
- if (isnan(z1) || isnan(z2) || isnan(z3)) {
- SERIAL_ERROR_START;
- SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
- goto LEAVE;
- }
-
-
-
-
-
- save_ubl_active_state_and_disable();
- z1 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) ;
- z2 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) ;
- z3 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) ;
-
- do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
- tilt_mesh_based_on_3pts(z1, z2, z3);
- restore_ubl_active_state_and_leave();
- }
- }
-
- if (parser.seen('P')) {
- if (WITHIN(g29_phase_value, 0, 1) && state.storage_slot == -1) {
- state.storage_slot = 0;
- SERIAL_PROTOCOLLNPGM("Default storage slot 0 selected.");
- }
-
- switch (g29_phase_value) {
- case 0:
-
-
-
- reset();
- SERIAL_PROTOCOLLNPGM("Mesh zeroed.");
- break;
-
- case 1:
-
-
-
- if (!parser.seen('C')) {
- invalidate();
- SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.");
- }
- if (g29_verbose_level > 1) {
- SERIAL_PROTOCOLPAIR("Probing Mesh Points Closest to (", g29_x_pos);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL(g29_y_pos);
- SERIAL_PROTOCOLLNPGM(").\n");
- }
- probe_entire_mesh(g29_x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, g29_y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
- parser.seen('T'), parser.seen('E'), parser.seen('U'));
- break;
-
- case 2: {
-
-
-
- SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.");
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
- if (!g29_x_flag && !g29_y_flag) {
-
-
- #if IS_KINEMATIC
- g29_x_pos = X_HOME_POS;
- g29_y_pos = Y_HOME_POS;
- #else
- g29_x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? X_MAX_POS : X_MIN_POS;
- g29_y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? Y_MAX_POS : Y_MIN_POS;
- #endif
- }
-
- if (parser.seen('C')) {
- g29_x_pos = current_position[X_AXIS];
- g29_y_pos = current_position[Y_AXIS];
- }
-
- float height = Z_CLEARANCE_BETWEEN_PROBES;
-
- if (parser.seen('B')) {
- g29_card_thickness = parser.has_value() ? parser.value_float() : measure_business_card_thickness(height);
- if (fabs(g29_card_thickness) > 1.5) {
- SERIAL_PROTOCOLLNPGM("?Error in Business Card measurement.");
- return;
- }
- }
-
- if (parser.seen('H') && parser.has_value()) height = parser.value_float();
-
- if (!position_is_reachable_xy(g29_x_pos, g29_y_pos)) {
- SERIAL_PROTOCOLLNPGM("XY outside printable radius.");
- return;
- }
-
- manually_probe_remaining_mesh(g29_x_pos, g29_y_pos, height, g29_card_thickness, parser.seen('T'));
- SERIAL_PROTOCOLLNPGM("G29 P2 finished.");
- } break;
-
- case 3: {
-
-
- if (g29_c_flag) {
- if (g29_repetition_cnt >= GRID_MAX_POINTS) {
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
- z_values[x][y] = g29_constant;
- }
- }
- }
- else {
- while (g29_repetition_cnt--) {
- const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
- if (location.x_index < 0) break;
- z_values[location.x_index][location.y_index] = g29_constant;
- }
- }
- } else {
- const float cvf = parser.value_float();
- switch((int)truncf(cvf * 10.0) - 30) {
- #if ENABLED(UBL_G29_P31)
- case 1: {
-
-
-
-
-
-
-
- const float weight_power = (cvf - 3.10) * 100.0,
- weight_factor = weight_power ? pow(10.0, weight_power) : 0;
- smart_fill_wlsf(weight_factor);
- }
- break;
- #endif
- case 0:
- default:
- smart_fill_mesh();
- break;
- }
- }
- break;
- }
-
- case 4:
-
-
-
- fine_tune_mesh(g29_x_pos, g29_y_pos, parser.seen('T'));
- break;
-
- case 5: find_mean_mesh_height(); break;
-
- case 6: shift_mesh_height(); break;
- }
- }
-
-
-
-
-
- if (parser.seen('W')) g29_what_command();
-
-
-
-
-
-
- if (parser.seen('K'))
- g29_compare_current_mesh_to_stored_mesh();
-
-
-
-
-
- if (parser.seen('L')) {
- g29_storage_slot = parser.has_value() ? parser.value_int() : state.storage_slot;
-
- int16_t a = settings.calc_num_meshes();
-
- if (!a) {
- SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
- return;
- }
-
- if (!WITHIN(g29_storage_slot, 0, a - 1)) {
- SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
- SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
- return;
- }
-
- settings.load_mesh(g29_storage_slot);
- state.storage_slot = g29_storage_slot;
-
- SERIAL_PROTOCOLLNPGM("Done.");
- }
-
-
-
-
-
- if (parser.seen('S')) {
- g29_storage_slot = parser.has_value() ? parser.value_int() : state.storage_slot;
-
- if (g29_storage_slot == -1) {
- SERIAL_ECHOLNPGM("G29 I 999");
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
- if (!isnan(z_values[x][y])) {
- SERIAL_ECHOPAIR("M421 I ", x);
- SERIAL_ECHOPAIR(" J ", y);
- SERIAL_ECHOPGM(" Z ");
- SERIAL_ECHO_F(z_values[x][y], 6);
- SERIAL_ECHOPAIR(" ; X ", LOGICAL_X_POSITION(mesh_index_to_xpos(x)));
- SERIAL_ECHOPAIR(", Y ", LOGICAL_Y_POSITION(mesh_index_to_ypos(y)));
- SERIAL_EOL;
- }
- return;
- }
-
- int16_t a = settings.calc_num_meshes();
-
- if (!a) {
- SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
- goto LEAVE;
- }
-
- if (!WITHIN(g29_storage_slot, 0, a - 1)) {
- SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
- SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
- goto LEAVE;
- }
-
- settings.store_mesh(g29_storage_slot);
- state.storage_slot = g29_storage_slot;
-
- SERIAL_PROTOCOLLNPGM("Done.");
- }
-
- if (parser.seen('T'))
- display_map(parser.has_value() ? parser.value_int() : 0);
-
-
-
- #if 0
- if (parser.seen('Z')) {
- if (parser.has_value())
- state.z_offset = parser.value_float();
- else {
- save_ubl_active_state_and_disable();
-
-
- has_control_of_lcd_panel = true;
- float measured_z = 1.5;
- do_blocking_move_to_z(measured_z);
-
-
- lcd_refresh();
- lcd_z_offset_edit_setup(measured_z);
-
- KEEPALIVE_STATE(PAUSED_FOR_USER);
-
- do {
- measured_z = lcd_z_offset_edit();
- idle();
- do_blocking_move_to_z(measured_z);
- } while (!ubl_lcd_clicked());
-
- has_control_of_lcd_panel = true;
-
-
-
-
- KEEPALIVE_STATE(IN_HANDLER);
-
- lcd_return_to_status();
-
- const millis_t nxt = millis() + 1500UL;
- while (ubl_lcd_clicked()) {
- idle();
- if (ELAPSED(millis(), nxt)) {
- SERIAL_PROTOCOLLNPGM("\nZ-Offset Adjustment Stopped.");
- do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
- LCD_MESSAGEPGM("Z-Offset Stopped");
- restore_ubl_active_state_and_leave();
- goto LEAVE;
- }
- }
- has_control_of_lcd_panel = false;
- safe_delay(20);
-
- state.z_offset = measured_z;
-
- lcd_refresh();
- restore_ubl_active_state_and_leave();
- }
- }
- #endif
-
- LEAVE:
-
- lcd_reset_alert_level();
- LCD_MESSAGEPGM("");
- lcd_quick_feedback();
-
- has_control_of_lcd_panel = false;
- }
-
- void unified_bed_leveling::find_mean_mesh_height() {
- float sum = 0.0;
- int n = 0;
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
- if (!isnan(z_values[x][y])) {
- sum += z_values[x][y];
- n++;
- }
-
- const float mean = sum / n;
-
-
-
-
- float sum_of_diff_squared = 0.0;
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
- if (!isnan(z_values[x][y]))
- sum_of_diff_squared += sq(z_values[x][y] - mean);
-
- SERIAL_ECHOLNPAIR("# of samples: ", n);
- SERIAL_ECHOPGM("Mean Mesh Height: ");
- SERIAL_ECHO_F(mean, 6);
- SERIAL_EOL;
-
- const float sigma = sqrt(sum_of_diff_squared / (n + 1));
- SERIAL_ECHOPGM("Standard Deviation: ");
- SERIAL_ECHO_F(sigma, 6);
- SERIAL_EOL;
-
- if (g29_c_flag)
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
- if (!isnan(z_values[x][y]))
- z_values[x][y] -= mean + g29_constant;
- }
-
- void unified_bed_leveling::shift_mesh_height() {
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
- if (!isnan(z_values[x][y]))
- z_values[x][y] += g29_constant;
- }
-
-
-
- void unified_bed_leveling::probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool close_or_far) {
- mesh_index_pair location;
-
- has_control_of_lcd_panel = true;
- save_ubl_active_state_and_disable();
- DEPLOY_PROBE();
-
- uint16_t max_iterations = GRID_MAX_POINTS;
-
- do {
- if (ubl_lcd_clicked()) {
- SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
- lcd_quick_feedback();
- STOW_PROBE();
- while (ubl_lcd_clicked()) idle();
- has_control_of_lcd_panel = false;
- restore_ubl_active_state_and_leave();
- safe_delay(50);
- return;
- }
-
- location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, close_or_far);
-
- if (location.x_index >= 0) {
- const float rawx = mesh_index_to_xpos(location.x_index),
- rawy = mesh_index_to_ypos(location.y_index);
-
- const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level);
- z_values[location.x_index][location.y_index] = measured_z;
- }
-
- if (do_ubl_mesh_map) display_map(g29_map_type);
-
- } while (location.x_index >= 0 && --max_iterations);
-
- STOW_PROBE();
- restore_ubl_active_state_and_leave();
-
- do_blocking_move_to_xy(
- constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_X, UBL_MESH_MAX_X),
- constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)
- );
- }
-
- void unified_bed_leveling::tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
- matrix_3x3 rotation;
- vector_3 v1 = vector_3( (UBL_PROBE_PT_1_X - UBL_PROBE_PT_2_X),
- (UBL_PROBE_PT_1_Y - UBL_PROBE_PT_2_Y),
- (z1 - z2) ),
-
- v2 = vector_3( (UBL_PROBE_PT_3_X - UBL_PROBE_PT_2_X),
- (UBL_PROBE_PT_3_Y - UBL_PROBE_PT_2_Y),
- (z3 - z2) ),
-
- normal = vector_3::cross(v1, v2);
-
- normal = normal.get_normal();
-
-
-
- if (normal.z < 0.0) {
- normal.x = -normal.x;
- normal.y = -normal.y;
- normal.z = -normal.z;
- }
-
- rotation = matrix_3x3::create_look_at(vector_3(normal.x, normal.y, 1));
-
- if (g29_verbose_level > 2) {
- SERIAL_ECHOPGM("bed plane normal = [");
- SERIAL_PROTOCOL_F(normal.x, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(normal.y, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(normal.z, 7);
- SERIAL_ECHOLNPGM("]");
- rotation.debug(PSTR("rotation matrix:"));
- }
-
-
-
-
-
- float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y),
- d = t + normal.z * z1;
-
- if (g29_verbose_level>2) {
- SERIAL_ECHOPGM("D constant: ");
- SERIAL_PROTOCOL_F(d, 7);
- SERIAL_ECHOLNPGM(" ");
- }
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPGM("d from 1st point: ");
- SERIAL_ECHO_F(d, 6);
- SERIAL_EOL;
- t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y);
- d = t + normal.z * z2;
- SERIAL_ECHOPGM("d from 2nd point: ");
- SERIAL_ECHO_F(d, 6);
- SERIAL_EOL;
- t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y);
- d = t + normal.z * z3;
- SERIAL_ECHOPGM("d from 3rd point: ");
- SERIAL_ECHO_F(d, 6);
- SERIAL_EOL;
- }
- #endif
-
- for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
- for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
- float x_tmp = mesh_index_to_xpos(i),
- y_tmp = mesh_index_to_ypos(j),
- z_tmp = z_values[i][j];
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPGM("before rotation = [");
- SERIAL_PROTOCOL_F(x_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(y_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(z_tmp, 7);
- SERIAL_ECHOPGM("] ---> ");
- safe_delay(20);
- }
- #endif
- apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPGM("after rotation = [");
- SERIAL_PROTOCOL_F(x_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(y_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(z_tmp, 7);
- SERIAL_ECHOLNPGM("]");
- safe_delay(55);
- }
- #endif
- z_values[i][j] += z_tmp - d;
- }
- }
- }
-
- float unified_bed_leveling::measure_point_with_encoder() {
-
- while (ubl_lcd_clicked()) delay(50);
- delay(50);
-
- KEEPALIVE_STATE(PAUSED_FOR_USER);
- while (!ubl_lcd_clicked()) {
- idle();
- if (encoder_diff) {
- do_blocking_move_to_z(current_position[Z_AXIS] + 0.01 * float(encoder_diff));
- encoder_diff = 0;
- }
- }
- KEEPALIVE_STATE(IN_HANDLER);
- return current_position[Z_AXIS];
- }
-
- static void echo_and_take_a_measurement() { SERIAL_PROTOCOLLNPGM(" and take a measurement."); }
-
- float unified_bed_leveling::measure_business_card_thickness(float &in_height) {
- has_control_of_lcd_panel = true;
- save_ubl_active_state_and_disable();
-
- do_blocking_move_to_z(in_height);
- do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
-
- stepper.synchronize();
-
- SERIAL_PROTOCOLPGM("Place shim under nozzle");
- LCD_MESSAGEPGM("Place shim & measure");
- lcd_return_to_status();
- echo_and_take_a_measurement();
-
- const float z1 = measure_point_with_encoder();
- do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
- stepper.synchronize();
-
- SERIAL_PROTOCOLPGM("Remove shim");
- LCD_MESSAGEPGM("Remove & measure bed");
- echo_and_take_a_measurement();
-
- const float z2 = measure_point_with_encoder();
-
- do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES);
-
- const float thickness = abs(z1 - z2);
-
- if (g29_verbose_level > 1) {
- SERIAL_PROTOCOLPGM("Business Card is ");
- SERIAL_PROTOCOL_F(thickness, 4);
- SERIAL_PROTOCOLLNPGM("mm thick.");
- }
-
- in_height = current_position[Z_AXIS];
-
- has_control_of_lcd_panel = false;
-
- restore_ubl_active_state_and_leave();
-
- return thickness;
- }
-
- void unified_bed_leveling::manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) {
-
- has_control_of_lcd_panel = true;
- save_ubl_active_state_and_disable();
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
- do_blocking_move_to_xy(lx, ly);
-
- lcd_return_to_status();
- mesh_index_pair location;
- do {
- location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_NOZZLE_AS_REFERENCE, NULL, false);
-
- if (location.x_index < 0 && location.y_index < 0) continue;
-
- const float rawx = mesh_index_to_xpos(location.x_index),
- rawy = mesh_index_to_ypos(location.y_index),
- xProbe = LOGICAL_X_POSITION(rawx),
- yProbe = LOGICAL_Y_POSITION(rawy);
-
- if (!position_is_reachable_raw_xy(rawx, rawy)) break;
-
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
-
- LCD_MESSAGEPGM("Moving to next");
-
- do_blocking_move_to_xy(xProbe, yProbe);
- do_blocking_move_to_z(z_clearance);
-
- KEEPALIVE_STATE(PAUSED_FOR_USER);
- has_control_of_lcd_panel = true;
-
- if (do_ubl_mesh_map) display_map(g29_map_type);
-
- serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure"));
-
- while (ubl_lcd_clicked()) delay(50);
- delay(50);
- while (!ubl_lcd_clicked()) {
- idle();
- if (encoder_diff) {
- do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) / 100.0);
- encoder_diff = 0;
- }
- }
-
-
-
-
- const millis_t nxt = millis() + 1500L;
- while (ubl_lcd_clicked()) {
- idle();
- if (ELAPSED(millis(), nxt)) {
- SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.");
- do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
- lcd_quick_feedback();
- while (ubl_lcd_clicked()) idle();
- has_control_of_lcd_panel = false;
- KEEPALIVE_STATE(IN_HANDLER);
- restore_ubl_active_state_and_leave();
- return;
- }
- }
-
- z_values[location.x_index][location.y_index] = current_position[Z_AXIS] - thick;
- if (g29_verbose_level > 2) {
- SERIAL_PROTOCOLPGM("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) display_map(g29_map_type);
-
- restore_ubl_active_state_and_leave();
- KEEPALIVE_STATE(IN_HANDLER);
- do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
- do_blocking_move_to_xy(lx, ly);
- }
-
- bool unified_bed_leveling::g29_parameter_parsing() {
- bool err_flag = false;
-
- LCD_MESSAGEPGM("Doing G29 UBL!");
- lcd_quick_feedback();
-
- g29_constant = 0.0;
- g29_repetition_cnt = 0;
-
- g29_x_flag = parser.seen('X') && parser.has_value();
- g29_x_pos = g29_x_flag ? parser.value_float() : current_position[X_AXIS];
- g29_y_flag = parser.seen('Y') && parser.has_value();
- g29_y_pos = g29_y_flag ? parser.value_float() : current_position[Y_AXIS];
-
- if (parser.seen('R')) {
- g29_repetition_cnt = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS;
- NOMORE(g29_repetition_cnt, GRID_MAX_POINTS);
- if (g29_repetition_cnt < 1) {
- SERIAL_PROTOCOLLNPGM("?(R)epetition count invalid (1+).\n");
- return UBL_ERR;
- }
- }
-
- g29_verbose_level = parser.seen('V') ? parser.value_int() : 0;
- if (!WITHIN(g29_verbose_level, 0, 4)) {
- SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-4).\n");
- err_flag = true;
- }
-
- if (parser.seen('P')) {
- g29_phase_value = parser.value_int();
- if (!WITHIN(g29_phase_value, 0, 6)) {
- SERIAL_PROTOCOLLNPGM("?(P)hase value invalid (0-6).\n");
- err_flag = true;
- }
- }
-
- if (parser.seen('J')) {
- g29_grid_size = parser.has_value() ? parser.value_int() : 0;
- if (g29_grid_size && !WITHIN(g29_grid_size, 2, 9)) {
- SERIAL_PROTOCOLLNPGM("?Invalid grid size (J) specified (2-9).\n");
- err_flag = true;
- }
- }
-
- if (g29_x_flag != g29_y_flag) {
- SERIAL_PROTOCOLLNPGM("Both X & Y locations must be specified.\n");
- err_flag = true;
- }
- if (!WITHIN(RAW_X_POSITION(g29_x_pos), X_MIN_POS, X_MAX_POS)) {
- SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n");
- err_flag = true;
- }
-
- if (!WITHIN(RAW_Y_POSITION(g29_y_pos), Y_MIN_POS, Y_MAX_POS)) {
- SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n");
- err_flag = true;
- }
-
- if (err_flag) return UBL_ERR;
-
-
-
- if (parser.seen('A')) {
- if (parser.seen('D')) {
- SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
- return UBL_ERR;
- }
- state.active = true;
- report_state();
- }
- else if (parser.seen('D')) {
- state.active = false;
- report_state();
- }
-
-
- if ((g29_c_flag = parser.seen('C')))
- g29_constant = parser.value_float();
-
- #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
- if (parser.seen('F') && parser.has_value()) {
- const float fh = parser.value_float();
- if (!WITHIN(fh, 0.0, 100.0)) {
- SERIAL_PROTOCOLLNPGM("?(F)ade height for Bed Level Correction not plausible.\n");
- return UBL_ERR;
- }
- set_z_fade_height(fh);
- }
- #endif
-
- g29_map_type = parser.seen('T') && parser.has_value() ? parser.value_int() : 0;
- if (!WITHIN(g29_map_type, 0, 1)) {
- SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
- return UBL_ERR;
- }
- return UBL_OK;
- }
-
- static int ubl_state_at_invocation = 0,
- ubl_state_recursion_chk = 0;
-
- void unified_bed_leveling::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_MESSAGEPGM("save_UBL_active() error");
- lcd_quick_feedback();
- return;
- }
- ubl_state_at_invocation = state.active;
- state.active = 0;
- }
-
- void unified_bed_leveling::restore_ubl_active_state_and_leave() {
- if (--ubl_state_recursion_chk) {
- SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
- LCD_MESSAGEPGM("restore_UBL_active() error");
- lcd_quick_feedback();
- return;
- }
- state.active = ubl_state_at_invocation;
- }
-
-
-
- void unified_bed_leveling::g29_what_command() {
- report_state();
-
- if (state.storage_slot == -1)
- SERIAL_PROTOCOLPGM("No Mesh Loaded.");
- else {
- SERIAL_PROTOCOLPAIR("Mesh ", state.storage_slot);
- SERIAL_PROTOCOLPGM(" Loaded.");
- }
- SERIAL_EOL;
- safe_delay(50);
-
- SERIAL_PROTOCOLLNPAIR("UBL object count: ", (int)ubl_cnt);
-
- #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
- SERIAL_PROTOCOL("planner.z_fade_height : ");
- SERIAL_PROTOCOL_F(planner.z_fade_height, 4);
- SERIAL_EOL;
- #endif
-
- #if HAS_BED_PROBE
- SERIAL_PROTOCOLPGM("zprobe_zoffset: ");
- SERIAL_PROTOCOL_F(zprobe_zoffset, 7);
- SERIAL_EOL;
- #endif
-
- SERIAL_ECHOLNPAIR("UBL_MESH_MIN_X " STRINGIFY(UBL_MESH_MIN_X) "=", UBL_MESH_MIN_X);
- SERIAL_ECHOLNPAIR("UBL_MESH_MIN_Y " STRINGIFY(UBL_MESH_MIN_Y) "=", UBL_MESH_MIN_Y);
- safe_delay(25);
- SERIAL_ECHOLNPAIR("UBL_MESH_MAX_X " STRINGIFY(UBL_MESH_MAX_X) "=", UBL_MESH_MAX_X);
- SERIAL_ECHOLNPAIR("UBL_MESH_MAX_Y " STRINGIFY(UBL_MESH_MAX_Y) "=", UBL_MESH_MAX_Y);
- safe_delay(25);
- SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_X ", GRID_MAX_POINTS_X);
- SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_Y ", GRID_MAX_POINTS_Y);
- safe_delay(25);
- SERIAL_ECHOLNPAIR("MESH_X_DIST ", MESH_X_DIST);
- SERIAL_ECHOLNPAIR("MESH_Y_DIST ", MESH_Y_DIST);
- safe_delay(25);
-
- SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: ");
- for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
- SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(mesh_index_to_xpos(i)), 3);
- SERIAL_PROTOCOLPGM(" ");
- safe_delay(25);
- }
- SERIAL_EOL;
-
- SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
- for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; i++) {
- SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(mesh_index_to_ypos(i)), 3);
- SERIAL_PROTOCOLPGM(" ");
- safe_delay(25);
- }
- SERIAL_EOL;
-
- #if HAS_KILL
- SERIAL_PROTOCOLPAIR("Kill pin on :", KILL_PIN);
- SERIAL_PROTOCOLLNPAIR(" state:", READ(KILL_PIN));
- #endif
- SERIAL_EOL;
- safe_delay(50);
-
- SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
- SERIAL_EOL;
- SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk);
- SERIAL_EOL;
- safe_delay(50);
-
- SERIAL_PROTOCOLPAIR("Meshes go from ", hex_address((void*)settings.get_start_of_meshes()));
- SERIAL_PROTOCOLLNPAIR(" to ", hex_address((void*)settings.get_end_of_meshes()));
- safe_delay(50);
-
- SERIAL_PROTOCOLLNPAIR("sizeof(ubl) : ", (int)sizeof(ubl));
- SERIAL_EOL;
- SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values));
- SERIAL_EOL;
- safe_delay(25);
-
- SERIAL_PROTOCOLLNPAIR("EEPROM free for UBL: ", hex_address((void*)(settings.get_end_of_meshes() - settings.get_start_of_meshes())));
- safe_delay(50);
-
- SERIAL_PROTOCOLPAIR("EEPROM can hold ", settings.calc_num_meshes());
- SERIAL_PROTOCOLLNPGM(" meshes.\n");
- safe_delay(25);
-
- if (!sanity_check()) {
- echo_name();
- SERIAL_PROTOCOLLNPGM(" sanity checks passed.");
- }
- }
-
-
-
- void unified_bed_leveling::g29_eeprom_dump() {
- unsigned char cccc;
- uint16_t kkkk;
-
- SERIAL_ECHO_START;
- SERIAL_ECHOLNPGM("EEPROM Dump:");
- for (uint16_t i = 0; i < E2END + 1; i += 16) {
- if (!(i & 0x3)) idle();
- print_hex_word(i);
- SERIAL_ECHOPGM(": ");
- for (uint16_t j = 0; j < 16; j++) {
- kkkk = i + j;
- eeprom_read_block(&cccc, (void *)kkkk, 1);
- print_hex_byte(cccc);
- SERIAL_ECHO(' ');
- }
- SERIAL_EOL;
- }
- SERIAL_EOL;
- }
-
-
-
- void unified_bed_leveling::g29_compare_current_mesh_to_stored_mesh() {
- int16_t a = settings.calc_num_meshes();
-
- if (!a) {
- SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
- return;
- }
-
- if (!parser.has_value()) {
- SERIAL_PROTOCOLLNPGM("?Storage slot # required.");
- SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
- return;
- }
-
- g29_storage_slot = parser.value_int();
-
- if (!WITHIN(g29_storage_slot, 0, a - 1)) {
- SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
- SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
- return;
- }
-
- float tmp_z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
- settings.load_mesh(g29_storage_slot, &tmp_z_values);
-
- SERIAL_PROTOCOLPAIR("Subtracting mesh in slot ", g29_storage_slot);
- SERIAL_PROTOCOLLNPGM(" from current mesh.");
-
- for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
- for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
- z_values[x][y] -= tmp_z_values[x][y];
- }
-
- mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], const bool far_flag) {
- mesh_index_pair out_mesh;
- out_mesh.x_index = out_mesh.y_index = -1;
-
-
- const float px = RAW_X_POSITION(lx) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
- py = RAW_Y_POSITION(ly) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
-
- float best_so_far = far_flag ? -99999.99 : 99999.99;
-
- for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
- for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
-
- if ( (type == INVALID && isnan(z_values[i][j]))
- || (type == REAL && !isnan(z_values[i][j]))
- || (type == SET_IN_BITMAP && is_bit_set(bits, i, j))
- ) {
-
-
- float raw_x = RAW_CURRENT_POSITION(X), raw_y = RAW_CURRENT_POSITION(Y);
- const float mx = mesh_index_to_xpos(i),
- my = mesh_index_to_ypos(j);
-
-
-
-
-
- if (probe_as_reference ? !position_is_reachable_by_probe_raw_xy(mx, my) : !position_is_reachable_raw_xy(mx, my))
- continue;
-
-
-
-
- float distance = HYPOT(px - mx, py - my);
-
-
-
- if (far_flag) {
- for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
- for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
- if (i != k && j != l && !isnan(z_values[k][l])) {
-
- distance += HYPOT(MESH_X_DIST, MESH_Y_DIST) / log(HYPOT((i - k) * (MESH_X_DIST) + .001, (j - l) * (MESH_Y_DIST)) + .001);
- }
- }
- }
- }
- else
-
-
- distance += HYPOT(raw_x - mx, raw_y - my) * 0.1;
-
-
- if (far_flag == (distance > best_so_far) && distance != best_so_far) {
- best_so_far = distance;
- out_mesh.x_index = i;
- out_mesh.y_index = j;
- out_mesh.distance = best_so_far;
- }
- }
- }
- }
-
- return out_mesh;
- }
-
- void unified_bed_leveling::fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) {
- if (!parser.seen('R'))
- g29_repetition_cnt = 1;
-
- mesh_index_pair location;
- uint16_t not_done[16];
-
- if (!position_is_reachable_xy(lx, ly)) {
- SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
- return;
- }
-
- save_ubl_active_state_and_disable();
-
- memset(not_done, 0xFF, sizeof(not_done));
-
- LCD_MESSAGEPGM("Fine Tuning Mesh");
-
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
- do_blocking_move_to_xy(lx, ly);
- do {
- location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false);
-
- if (location.x_index < 0) break;
-
- bit_clear(not_done, location.x_index, location.y_index);
-
-
- const float rawx = mesh_index_to_xpos(location.x_index),
- rawy = mesh_index_to_ypos(location.y_index);
-
- if (!position_is_reachable_raw_xy(rawx, rawy))
- break;
-
- float new_z = z_values[location.x_index][location.y_index];
-
- if (isnan(new_z))
- new_z = 0.0;
-
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
- do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy));
-
- new_z = floor(new_z * 1000.0) * 0.001;
-
- KEEPALIVE_STATE(PAUSED_FOR_USER);
- has_control_of_lcd_panel = true;
-
- if (do_ubl_mesh_map) display_map(g29_map_type);
-
- lcd_refresh();
-
- lcd_mesh_edit_setup(new_z);
-
- do {
- new_z = lcd_mesh_edit();
- #ifdef UBL_MESH_EDIT_MOVES_Z
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES + new_z);
- #endif
- idle();
- } while (!ubl_lcd_clicked());
-
- lcd_return_to_status();
-
-
-
-
- has_control_of_lcd_panel = true;
-
-
-
-
- const millis_t nxt = millis() + 1500UL;
- while (ubl_lcd_clicked()) {
- idle();
- if (ELAPSED(millis(), nxt)) {
- lcd_return_to_status();
-
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
- LCD_MESSAGEPGM("Mesh Editing Stopped");
-
- while (ubl_lcd_clicked()) idle();
-
- goto FINE_TUNE_EXIT;
- }
- }
-
- safe_delay(20);
-
- z_values[location.x_index][location.y_index] = new_z;
-
- lcd_refresh();
-
- } while (location.x_index >= 0 && --g29_repetition_cnt > 0);
-
- FINE_TUNE_EXIT:
-
- has_control_of_lcd_panel = false;
- KEEPALIVE_STATE(IN_HANDLER);
-
- if (do_ubl_mesh_map) display_map(g29_map_type);
- restore_ubl_active_state_and_leave();
- do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
-
- do_blocking_move_to_xy(lx, ly);
-
- LCD_MESSAGEPGM("Done Editing Mesh");
- SERIAL_ECHOLNPGM("Done Editing Mesh");
- }
-
-
-
-
- bool unified_bed_leveling::smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
- const int8_t x1 = x + xdir, x2 = x1 + xdir,
- y1 = y + ydir, y2 = y1 + ydir;
-
- if (isnan(z_values[x][y]) && !isnan(z_values[x1][y1]) && !isnan(z_values[x2][y2])) {
- if (z_values[x1][y1] < z_values[x2][y2])
- z_values[x][y] = z_values[x1][y1];
- else
- z_values[x][y] = 2.0 * z_values[x1][y1] - z_values[x2][y2];
- return true;
- }
- return false;
- }
-
- typedef struct { uint8_t sx, ex, sy, ey; bool yfirst; } smart_fill_info;
-
- void unified_bed_leveling::smart_fill_mesh() {
- const smart_fill_info info[] = {
- { 0, GRID_MAX_POINTS_X, 0, GRID_MAX_POINTS_Y - 2, false },
- { 0, GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y - 1, 0, false },
- { 0, GRID_MAX_POINTS_X - 2, 0, GRID_MAX_POINTS_Y, true },
- { GRID_MAX_POINTS_X - 1, 0, 0, GRID_MAX_POINTS_Y, true }
- };
- for (uint8_t i = 0; i < COUNT(info); ++i) {
- const smart_fill_info &f = info[i];
- if (f.yfirst) {
- const int8_t dir = f.ex > f.sx ? 1 : -1;
- for (uint8_t y = f.sy; y != f.ey; ++y)
- for (uint8_t x = f.sx; x != f.ex; x += dir)
- if (smart_fill_one(x, y, dir, 0)) break;
- }
- else {
- const int8_t dir = f.ey > f.sy ? 1 : -1;
- for (uint8_t x = f.sx; x != f.ex; ++x)
- for (uint8_t y = f.sy; y != f.ey; y += dir)
- if (smart_fill_one(x, y, 0, dir)) break;
- }
- }
- }
-
- void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) {
- constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X),
- x_max = min(MAX_PROBE_X, UBL_MESH_MAX_X),
- y_min = max(MIN_PROBE_Y, UBL_MESH_MIN_Y),
- y_max = min(MAX_PROBE_Y, UBL_MESH_MAX_Y);
-
- const float dx = float(x_max - x_min) / (g29_grid_size - 1.0),
- dy = float(y_max - y_min) / (g29_grid_size - 1.0);
-
- struct linear_fit_data lsf_results;
- incremental_LSF_reset(&lsf_results);
-
- bool zig_zag = false;
- for (uint8_t ix = 0; ix < g29_grid_size; ix++) {
- const float x = float(x_min) + ix * dx;
- for (int8_t iy = 0; iy < g29_grid_size; iy++) {
- const float y = float(y_min) + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
- float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), parser.seen('E'), g29_verbose_level);
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_CHAR('(');
- SERIAL_PROTOCOL_F(x, 7);
- SERIAL_CHAR(',');
- SERIAL_PROTOCOL_F(y, 7);
- SERIAL_ECHOPGM(") logical: ");
- SERIAL_CHAR('(');
- SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
- SERIAL_CHAR(',');
- SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7);
- SERIAL_ECHOPGM(") measured: ");
- SERIAL_PROTOCOL_F(measured_z, 7);
- SERIAL_ECHOPGM(" correction: ");
- SERIAL_PROTOCOL_F(get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
- }
- #endif
-
- measured_z -= get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) ;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPGM(" final >>>---> ");
- SERIAL_PROTOCOL_F(measured_z, 7);
- SERIAL_EOL;
- }
- #endif
-
- incremental_LSF(&lsf_results, x, y, measured_z);
- }
-
- zig_zag ^= true;
- }
-
- if (finish_incremental_LSF(&lsf_results)) {
- SERIAL_ECHOPGM("Could not complete LSF!");
- return;
- }
-
- if (g29_verbose_level > 3) {
- SERIAL_ECHOPGM("LSF Results A=");
- SERIAL_PROTOCOL_F(lsf_results.A, 7);
- SERIAL_ECHOPGM(" B=");
- SERIAL_PROTOCOL_F(lsf_results.B, 7);
- SERIAL_ECHOPGM(" D=");
- SERIAL_PROTOCOL_F(lsf_results.D, 7);
- SERIAL_EOL;
- }
-
- vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal();
-
- if (g29_verbose_level > 2) {
- SERIAL_ECHOPGM("bed plane normal = [");
- SERIAL_PROTOCOL_F(normal.x, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(normal.y, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(normal.z, 7);
- SERIAL_ECHOLNPGM("]");
- }
-
- matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
-
- for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
- for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
- float x_tmp = mesh_index_to_xpos(i),
- y_tmp = mesh_index_to_ypos(j),
- z_tmp = z_values[i][j];
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPGM("before rotation = [");
- SERIAL_PROTOCOL_F(x_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(y_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(z_tmp, 7);
- SERIAL_ECHOPGM("] ---> ");
- safe_delay(20);
- }
- #endif
-
- apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPGM("after rotation = [");
- SERIAL_PROTOCOL_F(x_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(y_tmp, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(z_tmp, 7);
- SERIAL_ECHOLNPGM("]");
- safe_delay(55);
- }
- #endif
-
- z_values[i][j] += z_tmp - lsf_results.D;
- }
- }
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- rotation.debug(PSTR("rotation matrix:"));
- SERIAL_ECHOPGM("LSF Results A=");
- SERIAL_PROTOCOL_F(lsf_results.A, 7);
- SERIAL_ECHOPGM(" B=");
- SERIAL_PROTOCOL_F(lsf_results.B, 7);
- SERIAL_ECHOPGM(" D=");
- SERIAL_PROTOCOL_F(lsf_results.D, 7);
- SERIAL_EOL;
- safe_delay(55);
-
- SERIAL_ECHOPGM("bed plane normal = [");
- SERIAL_PROTOCOL_F(normal.x, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(normal.y, 7);
- SERIAL_PROTOCOLCHAR(',');
- SERIAL_PROTOCOL_F(normal.z, 7);
- SERIAL_ECHOPGM("]\n");
- SERIAL_EOL;
- }
- #endif
- }
-
- #if ENABLED(UBL_G29_P31)
- void unified_bed_leveling::smart_fill_wlsf(const float &weight_factor) {
-
-
-
-
-
-
- static_assert(GRID_MAX_POINTS_Y <= 16, "GRID_MAX_POINTS_Y too big");
- uint16_t bitmap[GRID_MAX_POINTS_X] = { 0 };
- struct linear_fit_data lsf_results;
-
- SERIAL_ECHOPGM("Extrapolating mesh...");
-
- const float weight_scaled = weight_factor * max(MESH_X_DIST, MESH_Y_DIST);
-
- for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++)
- for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++)
- if (!isnan(z_values[jx][jy]))
- SBI(bitmap[jx], jy);
-
- for (uint8_t ix = 0; ix < GRID_MAX_POINTS_X; ix++) {
- const float px = mesh_index_to_xpos(ix);
- for (uint8_t iy = 0; iy < GRID_MAX_POINTS_Y; iy++) {
- const float py = mesh_index_to_ypos(iy);
- if (isnan(z_values[ix][iy])) {
-
- incremental_LSF_reset(&lsf_results);
- for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++) {
- const float rx = mesh_index_to_xpos(jx);
- for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++) {
- if (TEST(bitmap[jx], jy)) {
- const float ry = mesh_index_to_ypos(jy),
- rz = z_values[jx][jy],
- w = 1.0 + weight_scaled / HYPOT((rx - px), (ry - py));
- incremental_WLSF(&lsf_results, rx, ry, rz, w);
- }
- }
- }
- if (finish_incremental_LSF(&lsf_results)) {
- SERIAL_ECHOLNPGM("Insufficient data");
- return;
- }
- const float ez = -lsf_results.D - lsf_results.A * px - lsf_results.B * py;
- z_values[ix][iy] = ez;
- idle();
- }
- }
- }
-
- SERIAL_ECHOLNPGM("done");
- }
- #endif
-
- #endif
|