|
@@ -31,7 +31,7 @@
|
31
|
31
|
|
32
|
32
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
33
|
33
|
#include "vector_3.h"
|
34
|
|
- #ifdef ACCURATE_BED_LEVELING
|
|
34
|
+ #ifdef AUTO_BED_LEVELING_GRID
|
35
|
35
|
#include "qr_solve.h"
|
36
|
36
|
#endif
|
37
|
37
|
#endif // ENABLE_AUTO_BED_LEVELING
|
|
@@ -822,7 +822,7 @@ static void axis_is_at_home(int axis) {
|
822
|
822
|
}
|
823
|
823
|
|
824
|
824
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
825
|
|
-#ifdef ACCURATE_BED_LEVELING
|
|
825
|
+#ifdef AUTO_BED_LEVELING_GRID
|
826
|
826
|
static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
|
827
|
827
|
{
|
828
|
828
|
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
|
|
@@ -846,10 +846,9 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
|
846
|
846
|
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
847
|
847
|
}
|
848
|
848
|
|
849
|
|
-#else // not ACCURATE_BED_LEVELING
|
|
849
|
+#else // not AUTO_BED_LEVELING_GRID
|
850
|
850
|
|
851
|
|
- #ifdef AUTO_BED_LEVELING_ANY_POINTS
|
852
|
|
-static void set_bed_level_equation_any_pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
|
|
851
|
+static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
|
853
|
852
|
|
854
|
853
|
plan_bed_level_matrix.set_to_identity();
|
855
|
854
|
|
|
@@ -869,49 +868,14 @@ static void set_bed_level_equation_any_pts(float z_at_pt_1, float z_at_pt_2, flo
|
869
|
868
|
current_position[Y_AXIS] = corrected_position.y;
|
870
|
869
|
current_position[Z_AXIS] = corrected_position.z;
|
871
|
870
|
|
872
|
|
- // but the bed at 0 so we don't go below it.
|
|
871
|
+ // put the bed at 0 so we don't go below it.
|
873
|
872
|
current_position[Z_AXIS] = zprobe_zoffset;
|
874
|
873
|
|
875
|
874
|
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
876
|
875
|
|
877
|
876
|
}
|
878
|
|
- #else // not AUTO_BED_LEVELING_ANY_POINTS
|
879
|
|
-static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yFront, float z_at_xLeft_yBack) {
|
880
|
|
- plan_bed_level_matrix.set_to_identity();
|
881
|
|
-
|
882
|
|
- vector_3 xLeftyFront = vector_3(LEFT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, z_at_xLeft_yFront);
|
883
|
|
- vector_3 xLeftyBack = vector_3(LEFT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION, z_at_xLeft_yBack);
|
884
|
|
- vector_3 xRightyFront = vector_3(RIGHT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, z_at_xRight_yFront);
|
885
|
|
-
|
886
|
|
- vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
|
887
|
|
- vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
|
888
|
|
- vector_3 planeNormal = vector_3::cross(xPositive, yPositive).get_normal();
|
889
|
|
-
|
890
|
|
- //planeNormal.debug("planeNormal");
|
891
|
|
- //yPositive.debug("yPositive");
|
892
|
|
- plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
893
|
|
- //bedLevel.debug("bedLevel");
|
894
|
|
-
|
895
|
|
- //plan_bed_level_matrix.debug("bed level before");
|
896
|
|
- //vector_3 uncorrected_position = plan_get_position_mm();
|
897
|
|
- //uncorrected_position.debug("position before");
|
898
|
|
-
|
899
|
|
- // and set our bed level equation to do the right thing
|
900
|
|
- //plan_bed_level_matrix.debug("bed level after");
|
901
|
877
|
|
902
|
|
- vector_3 corrected_position = plan_get_position();
|
903
|
|
- //corrected_position.debug("position after");
|
904
|
|
- current_position[X_AXIS] = corrected_position.x;
|
905
|
|
- current_position[Y_AXIS] = corrected_position.y;
|
906
|
|
- current_position[Z_AXIS] = corrected_position.z;
|
907
|
|
-
|
908
|
|
- // but the bed at 0 so we don't go below it.
|
909
|
|
- current_position[Z_AXIS] = zprobe_zoffset;
|
910
|
|
-
|
911
|
|
- plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
912
|
|
-}
|
913
|
|
- #endif // AUTO_BED_LEVELING_ANY_POINTS
|
914
|
|
-#endif // ACCURATE_BED_LEVELING
|
|
878
|
+#endif // AUTO_BED_LEVELING_GRID
|
915
|
879
|
|
916
|
880
|
static void run_z_probe() {
|
917
|
881
|
plan_bed_level_matrix.set_to_identity();
|
|
@@ -1462,11 +1426,11 @@ void process_commands()
|
1462
|
1426
|
setup_for_endstop_move();
|
1463
|
1427
|
|
1464
|
1428
|
feedrate = homing_feedrate[Z_AXIS];
|
1465
|
|
-#ifdef ACCURATE_BED_LEVELING
|
|
1429
|
+#ifdef AUTO_BED_LEVELING_GRID
|
1466
|
1430
|
// probe at the points of a lattice grid
|
1467
|
1431
|
|
1468
|
|
- int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
|
1469
|
|
- int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
|
|
1432
|
+ int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
|
|
1433
|
+ int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
|
1470
|
1434
|
|
1471
|
1435
|
|
1472
|
1436
|
// solve the plane equation ax + by + d = z
|
|
@@ -1476,9 +1440,9 @@ void process_commands()
|
1476
|
1440
|
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
1477
|
1441
|
|
1478
|
1442
|
// "A" matrix of the linear system of equations
|
1479
|
|
- double eqnAMatrix[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS*3];
|
|
1443
|
+ double eqnAMatrix[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS*3];
|
1480
|
1444
|
// "B" vector of Z points
|
1481
|
|
- double eqnBVector[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS];
|
|
1445
|
+ double eqnBVector[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS];
|
1482
|
1446
|
|
1483
|
1447
|
|
1484
|
1448
|
int probePointCounter = 0;
|
|
@@ -1501,7 +1465,7 @@ void process_commands()
|
1501
|
1465
|
zig = true;
|
1502
|
1466
|
}
|
1503
|
1467
|
|
1504
|
|
- for (int xCount=0; xCount < ACCURATE_BED_LEVELING_POINTS; xCount++)
|
|
1468
|
+ for (int xCount=0; xCount < AUTO_BED_LEVELING_GRID_POINTS; xCount++)
|
1505
|
1469
|
{
|
1506
|
1470
|
float z_before;
|
1507
|
1471
|
if (probePointCounter == 0)
|
|
@@ -1518,9 +1482,9 @@ void process_commands()
|
1518
|
1482
|
|
1519
|
1483
|
eqnBVector[probePointCounter] = measured_z;
|
1520
|
1484
|
|
1521
|
|
- eqnAMatrix[probePointCounter + 0*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = xProbe;
|
1522
|
|
- eqnAMatrix[probePointCounter + 1*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = yProbe;
|
1523
|
|
- eqnAMatrix[probePointCounter + 2*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = 1;
|
|
1485
|
+ eqnAMatrix[probePointCounter + 0*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = xProbe;
|
|
1486
|
+ eqnAMatrix[probePointCounter + 1*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = yProbe;
|
|
1487
|
+ eqnAMatrix[probePointCounter + 2*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = 1;
|
1524
|
1488
|
probePointCounter++;
|
1525
|
1489
|
xProbe += xInc;
|
1526
|
1490
|
}
|
|
@@ -1528,7 +1492,7 @@ void process_commands()
|
1528
|
1492
|
clean_up_after_endstop_move();
|
1529
|
1493
|
|
1530
|
1494
|
// solve lsq problem
|
1531
|
|
- double *plane_equation_coefficients = qr_solve(ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS, 3, eqnAMatrix, eqnBVector);
|
|
1495
|
+ double *plane_equation_coefficients = qr_solve(AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS, 3, eqnAMatrix, eqnBVector);
|
1532
|
1496
|
|
1533
|
1497
|
SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
|
1534
|
1498
|
SERIAL_PROTOCOL(plane_equation_coefficients[0]);
|
|
@@ -1542,10 +1506,8 @@ void process_commands()
|
1542
|
1506
|
|
1543
|
1507
|
free(plane_equation_coefficients);
|
1544
|
1508
|
|
1545
|
|
-#else // ACCURATE_BED_LEVELING not defined
|
|
1509
|
+#else // AUTO_BED_LEVELING_GRID not defined
|
1546
|
1510
|
|
1547
|
|
-
|
1548
|
|
- #ifdef AUTO_BED_LEVELING_ANY_POINTS
|
1549
|
1511
|
// Probe at 3 arbitrary points
|
1550
|
1512
|
// probe 1
|
1551
|
1513
|
float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
|
|
@@ -1558,24 +1520,10 @@ void process_commands()
|
1558
|
1520
|
|
1559
|
1521
|
clean_up_after_endstop_move();
|
1560
|
1522
|
|
1561
|
|
- set_bed_level_equation_any_pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
1562
|
|
- #else // not AUTO_BED_LEVELING_ANY_POINTS
|
1563
|
|
- // probe at 3 corners of a rectangle
|
1564
|
|
- // probe 1
|
1565
|
|
- float z_at_xLeft_yBack = probe_pt(LEFT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION, Z_RAISE_BEFORE_PROBING);
|
|
1523
|
+ set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
1566
|
1524
|
|
1567
|
|
- // probe 2
|
1568
|
|
- float z_at_xLeft_yFront = probe_pt(LEFT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
1569
|
|
-
|
1570
|
|
- // probe 3
|
1571
|
|
- float z_at_xRight_yFront = probe_pt(RIGHT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
1572
|
|
-
|
1573
|
|
- clean_up_after_endstop_move();
|
1574
|
|
-
|
1575
|
|
- set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
|
1576
|
|
- #endif
|
1577
|
1525
|
|
1578
|
|
-#endif // ACCURATE_BED_LEVELING
|
|
1526
|
+#endif // AUTO_BED_LEVELING_GRID
|
1579
|
1527
|
st_synchronize();
|
1580
|
1528
|
|
1581
|
1529
|
// The following code correct the Z height difference from z-probe position and hotend tip position.
|