API tools faq
paste
Advertisement
Guest User

BedLeveling.cpp

a guest
Oct 30th, 2020
99
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 51.65 KB | None | 0 0
raw download clone embed print report
  1.  
  2. /*
  3. More and more printers now have automatic bed leveling using an ever increasing
  4. variety of methods. This makes the leveling routine one of the most complex
  5. parts of the firmware and there is not one way to level but hundreds of
  6. combinations.
  7.  
  8. First you should decide on the correction method. Once we know how our bed is
  9. tilted we want to remove that. This correction is defined by
  10. BED_CORRECTION_METHOD and allows the following values: BED_CORRECTION_METHOD 0
  11. Use a rotation matrix. This will make z axis go up/down while moving in x/y
  12. direction to compensate the tilt. For multiple extruders make sure the height
  13. match the tilt of the bed or one will scratch.
  14.  
  15. BED_CORRECTION_METHOD 1
  16. Motorized correction. This method needs a bed that is fixed on 3 points from
  17. which 2 have a motor to change the height. The positions are defined by
  18. BED_MOTOR_1_X, BED_MOTOR_1_Y, BED_MOTOR_2_X, BED_MOTOR_2_Y, BED_MOTOR_3_X,
  19. BED_MOTOR_3_Y Motor 2 and 3 are the one driven by motor driver 0 and 1. These
  20. can be extra motors like Felix Pro 1 uses them or a system with 3 z axis where
  21. motors can be controlled individually like the Sparkcube does.
  22.  
  23. Next we have to distinguish several methods of z probing sensors. Each have
  24. their own advantages and disadvantages. First the z probe has a position when
  25. activated and that position is defined by #define Z_PROBE_X_OFFSET 0 #define
  26. Z_PROBE_Y_OFFSET 0 This is needed since we need to know where we measure the
  27. height when the z probe triggers. When probing is activated you will see a move
  28. to make probe go over current extruder position. The position can be changed in
  29. eeprom later on.
  30.  
  31. Some probes need to be activated/deactivated so we can use them. This is defined
  32. in the scripts #define Z_PROBE_START_SCRIPT "" #define Z_PROBE_FINISHED_SCRIPT
  33. ""
  34.  
  35. Now when we probe we want to know the distance of the extruder to the bed. This
  36. is defined by #define Z_PROBE_HEIGHT 4 The 4 means that when we trigger the
  37. distance of the nozzle tip is 4mm. If your switch tends to return different
  38. points you might repeat a measured point and use the average height: #define
  39. Z_PROBE_SWITCHING_DISTANCE 1 #define Z_PROBE_REPETITIONS 5 Switching distance is
  40. the z raise needed to turn back a signal reliably to off. Inductive sensors need
  41. only a bit while mechanical switches may require a bit more.
  42.  
  43. Next thing to consider is the force for switching. Some beds use a cantilever
  44. design and pushing on the outside easily bends the bed. If your sensor needs
  45. some force to trigger you add the error of bending. For this reason you might
  46. add a bending correction. Currently you define #define BENDING_CORRECTION_A 0
  47. #define BENDING_CORRECTION_B 0
  48. #define BENDING_CORRECTION_C 0
  49. which are the deflections at the 3 z probe points. For all other possible
  50. measurements these values get interpolated. You can modify the values later on
  51. in eeprom. For force less sensors set them to 0.
  52.  
  53. Next thing is endstop handling. Without bed leveling you normally home to
  54. minimum position for x,y and z. With bed leveling this is not that easy any
  55. more. Since we do bed leveling we already assume the bed is not leveled for x/y
  56. moves. So without correction we would hit the bed for different x/y positions at
  57. different heights. As a result we have no real minimum position. That makes a z
  58. min endstop quite useless. There is an exception to this. If your nozzle
  59. triggers z min or if a inductive sensor would trigger at a given position we
  60. could use that signal. With nozzle triggers you need to be careful as a drop of
  61. filament would change the height. The other problem is that while homing the
  62. auto leveling is not used. So the only position would be if the z min sensor is
  63. directly over the 0,0 coordinate which is the rotation point if we have matrix
  64. based correction. For motor based correction this will work everywhere
  65. correctly.
  66.  
  67. So the only useful position for a z endstop is z max position. Apart from not
  68. having the bed tilt problem it also allows homing with a full bed so you can
  69. continue an aborted print with some gcode tweaking. With z max homing we adjust
  70. the error by simply changing the max. z height. One thing you need to remember
  71. is setting #define ENDSTOP_Z_BACK_ON_HOME 4 so we release the z max endstop.
  72. This is very important if we move xy at z max. Auto leveling might want to
  73. increase z and the endstop might prevent it causing wrong position and a head
  74. crash if we later go down. The value should be larger then the maximum expected
  75. tilt.
  76.  
  77. Now it is time to define how we measure the bed rotation. Here again we have
  78. several methods to choose. All methods need at least 3 points to define the bed
  79. rotation correctly. The quality we get comes from the selection of the right
  80. points and method.
  81.  
  82. BED_LEVELING_METHOD 0
  83. This method measures at the 3 probe points and creates a plane through these
  84. points. If you have a really planar bed this gives the optimum result. The 3
  85. points must not be in one line and have a long distance to increase numerical
  86. stability.
  87.  
  88. BED_LEVELING_METHOD 1
  89. This measures a grid. Probe point 1 is the origin and points 2 and 3 span a
  90. grid. We measure BED_LEVELING_GRID_SIZE points in each direction and compute a
  91. regression plane through all points. This gives a good overall plane if you have
  92. small bumps measuring inaccuracies.
  93.  
  94. BED_LEVELING_METHOD 2
  95. Bending correcting 4 point measurement. This is for cantilevered beds that have
  96. the rotation axis not at the side but inside the bed. Here we can assume no
  97. bending on the axis and a symmetric bending to both sides of the axis. So probe
  98. points 2 and 3 build the symmetric axis and point 1 is mirrored to 1m across the
  99. axis. Using the symmetry we then remove the bending from 1 and use that as
  100. plane.
  101.  
  102. By now the leveling process is finished. All errors that remain are measuring
  103. errors and bumps on the bed it self. For deltas you can enable distortion
  104. correction to follow the bumps.
  105.  
  106. There are 2 ways to consider a changing bed coating, which are defined by
  107. Z_PROBE_Z_OFFSET_MODE. Z_PROBE_Z_OFFSET_MODE = 0 means we measure the surface of
  108. the bed below any coating. This is e.g. the case with inductive sensors where we
  109. put BuildTak on top. In that case we can set Z_PROBE_Z_OFFSET to the thickness
  110. of BuildTak to compensate. If we later change the coating, we only change
  111. Z_PROBE_Z_OFFSET to new coating thickness.
  112.  
  113. Z_PROBE_Z_OFFSET_MODE = 1 means we measure the surface of the coating, e.g.
  114. because we have a mechanical switch. In that case we add Z_PROBE_Z_OFFSET for
  115. the measured height to compensate for correct distance to bed surface.
  116.  
  117. In homing to max we reduce z length by Z_PROBE_Z_OFFSET to get a correct height.
  118. In homing to z min we assume z endstop is bed level so we move up
  119. Z_PROBE_Z_OFFSET after endstop is hit. This requires the extruder to bend the
  120. coating thickness without harm!
  121. */
  122.  
  123. #include "Repetier.h"
  124.  
  125. #ifndef BED_LEVELING_METHOD
  126. #define BED_LEVELING_METHOD 0
  127. #endif
  128.  
  129. #ifndef BED_CORRECTION_METHOD
  130. #define BED_CORRECTION_METHOD 0
  131. #endif
  132.  
  133. #ifndef BED_LEVELING_GRID_SIZE
  134. #define BED_LEVELING_GRID_SIZE 5
  135. #endif
  136.  
  137. #ifndef BED_LEVELING_REPETITIONS
  138. #define BED_LEVELING_REPETITIONS 1
  139. #endif
  140.  
  141. #if FEATURE_Z_PROBE
  142. void Printer::prepareForProbing() {
  143. #ifndef SKIP_PROBE_PREPARE
  144.     // 1. Ensure we are homed so positions make sense
  145.     if (!Printer::isHomedAll()) {
  146.         Printer::homeAxis(true, true, true);
  147.     }
  148.     // 2. Go to z probe bed distance for probing
  149.     Printer::moveToReal(
  150.         IGNORE_COORDINATE, IGNORE_COORDINATE,
  151.         RMath::max(EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0),
  152.                    static_cast<float>(ZHOME_HEAT_HEIGHT)),
  153.         IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
  154.     // 3. Ensure we can activate z probe at current xy position
  155. #ifndef NO_SAVE_DEPLOY
  156.     // Printer::moveToCenter(); // safe position for deploying probe
  157. #endif
  158.     // Delta is at center already so does not need special testing here!
  159. #if EXTRUDER_IS_Z_PROBE == 0
  160.     float ZPOffsetX = EEPROM::zProbeXOffset();
  161.     float ZPOffsetY = EEPROM::zProbeYOffset();
  162. #if DRIVE_SYSTEM != DELTA
  163.     float targetX = Printer::currentPosition[X_AXIS];
  164.     float targetY = Printer::currentPosition[Y_AXIS];
  165.     if (ZPOffsetX > 0 && targetX - ZPOffsetX < Printer::xMin) {
  166.         targetX = Printer::xMin + ZPOffsetX + 1;
  167.     }
  168.     if (ZPOffsetY > 0 && targetY - ZPOffsetY < Printer::yMin) {
  169.         targetY = Printer::yMin + ZPOffsetY + 1;
  170.     }
  171.     if (ZPOffsetX < 0 && targetX - ZPOffsetX > Printer::xMin + Printer::xLength) {
  172.         targetX = Printer::xMin + Printer::xLength + ZPOffsetX - 1;
  173.     }
  174.     if (ZPOffsetY < 0 && targetY - ZPOffsetY > Printer::yMin + Printer::yLength) {
  175.         targetY = Printer::yMin + Printer::yLength + ZPOffsetY - 1;
  176.     }
  177.     Printer::moveToReal(targetX, targetY, IGNORE_COORDINATE, IGNORE_COORDINATE,
  178.                         EXTRUDER_SWITCH_XY_SPEED);
  179.     Printer::updateCurrentPosition(true);
  180.     Commands::waitUntilEndOfAllMoves();
  181. #endif
  182. #endif
  183. #endif
  184. }
  185. #endif
  186.  
  187. #if FEATURE_AUTOLEVEL && FEATURE_Z_PROBE
  188.  
  189. bool measureAutolevelPlane(Plane& plane) {
  190.     PlaneBuilder builder;
  191.     builder.reset();
  192. #if BED_LEVELING_METHOD == 0 // 3 point
  193.     float h;
  194.     Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE,
  195.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  196.     h = Printer::runZProbe(false, false);
  197.     if (h == ILLEGAL_Z_PROBE)
  198.         return false;
  199.     builder.addPoint(EEPROM::zProbeX1(), EEPROM::zProbeY1(), h);
  200.     Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE,
  201.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  202.     h = Printer::runZProbe(false, false);
  203.     if (h == ILLEGAL_Z_PROBE)
  204.         return false;
  205.     builder.addPoint(EEPROM::zProbeX2(), EEPROM::zProbeY2(), h);
  206.     Printer::moveTo(EEPROM::zProbeX3(), EEPROM::zProbeY3(), IGNORE_COORDINATE,
  207.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  208.     h = Printer::runZProbe(false, false);
  209.     if (h == ILLEGAL_Z_PROBE)
  210.         return false;
  211.     builder.addPoint(EEPROM::zProbeX3(), EEPROM::zProbeY3(), h);
  212. #elif BED_LEVELING_METHOD == 1 // linear regression
  213.     float delta = 1.0 / (BED_LEVELING_GRID_SIZE - 1);
  214.     float ox = EEPROM::zProbeX1();
  215.     float oy = EEPROM::zProbeY1();
  216.     float ax = delta * (EEPROM::zProbeX2() - EEPROM::zProbeX1());
  217.     float ay = delta * (EEPROM::zProbeY2() - EEPROM::zProbeY1());
  218.     float bx = delta * (EEPROM::zProbeX3() - EEPROM::zProbeX1());
  219.     float by = delta * (EEPROM::zProbeY3() - EEPROM::zProbeY1());
  220.     for (int ix = 0; ix < BED_LEVELING_GRID_SIZE; ix++) {
  221.         for (int iy = 0; iy < BED_LEVELING_GRID_SIZE; iy++) {
  222.             float px = ox + static_cast<float>(ix) * ax + static_cast<float>(iy) * bx;
  223.             float py = oy + static_cast<float>(ix) * ay + static_cast<float>(iy) * by;
  224.             Printer::moveTo(px, py, IGNORE_COORDINATE, IGNORE_COORDINATE,
  225.                             EEPROM::zProbeXYSpeed());
  226.             float h = Printer::runZProbe(false, false);
  227.             if (h == ILLEGAL_Z_PROBE)
  228.                 return false;
  229.             builder.addPoint(px, py, h);
  230.         }
  231.     }
  232.  
  233. #elif BED_LEVELING_METHOD == 2 // 4 point symmetric
  234.     float h1, h2, h3, h4;
  235.     float apx = EEPROM::zProbeX1() - EEPROM::zProbeX2();
  236.     float apy = EEPROM::zProbeY1() - EEPROM::zProbeY2();
  237.     float abx = EEPROM::zProbeX3() - EEPROM::zProbeX2();
  238.     float aby = EEPROM::zProbeY3() - EEPROM::zProbeY2();
  239.     float ab2 = abx * abx + aby * aby;
  240.     float abap = apx * abx + apy * aby;
  241.     float t = abap / ab2;
  242.     float xx = EEPROM::zProbeX2() + t * abx;
  243.     float xy = EEPROM::zProbeY2() + t * aby;
  244.     float x1Mirror = EEPROM::zProbeX1() + 2.0 * (xx - EEPROM::zProbeX1());
  245.     float y1Mirror = EEPROM::zProbeY1() + 2.0 * (xy - EEPROM::zProbeY1());
  246.     Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE,
  247.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  248.     h1 = Printer::runZProbe(false, false);
  249.     if (h1 == ILLEGAL_Z_PROBE)
  250.         return false;
  251.     Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE,
  252.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  253.     h2 = Printer::runZProbe(false, false);
  254.     if (h2 == ILLEGAL_Z_PROBE)
  255.         return false;
  256.     Printer::moveTo(EEPROM::zProbeX3(), EEPROM::zProbeY3(), IGNORE_COORDINATE,
  257.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  258.     h3 = Printer::runZProbe(false, false);
  259.     if (h3 == ILLEGAL_Z_PROBE)
  260.         return false;
  261.     Printer::moveTo(x1Mirror, y1Mirror, IGNORE_COORDINATE, IGNORE_COORDINATE,
  262.                     EEPROM::zProbeXYSpeed());
  263.     h4 = Printer::runZProbe(false, false);
  264.     if (h4 == ILLEGAL_Z_PROBE)
  265.         return false;
  266.     t = h2 + (h3 - h2) * t;   // theoretical height for crossing point for symmetric axis
  267.     h1 = t - (h4 - h1) * 0.5; // remove bending part
  268.     builder.addPoint(EEPROM::zProbeX1(), EEPROM::zProbeY1(), h1);
  269.     builder.addPoint(EEPROM::zProbeX2(), EEPROM::zProbeY2(), h2);
  270.     builder.addPoint(EEPROM::zProbeX3(), EEPROM::zProbeY3(), h3);
  271. #else
  272. #error Unknown bed leveling method
  273. #endif
  274.     builder.createPlane(plane, false);
  275.     return true;
  276. }
  277.  
  278. float correctAutolevel(Plane& plane) {
  279. #if BED_CORRECTION_METHOD == 0 // rotation matrix
  280.     // Printer::buildTransformationMatrix(plane.z(EEPROM::zProbeX1(),EEPROM::zProbeY1()),plane.z(EEPROM::zProbeX2(),EEPROM::zProbeY2()),plane.z(EEPROM::zProbeX3(),EEPROM::zProbeY3()));
  281.     // Printer::buildTransformationMatrix(plane);
  282. #elif BED_CORRECTION_METHOD == 1 // motorized correction
  283. #if !defined(NUM_MOTOR_DRIVERS) || NUM_MOTOR_DRIVERS < 2
  284. #error You need to define 2 motors for motorized bed correction
  285. #endif
  286. Commands::waitUntilEndOfAllMoves(); // move steppers might be leveling steppers as well !
  287.     //move to h1
  288.     Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  289.     //probe h1
  290.     float h1 = Printer::runZProbe(true, false, Z_PROBE_REPETITIONS, true);
  291.     //move to h2
  292.     Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY2(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  293.     //probe h2
  294.     float h2 = Printer::runZProbe(false, false, Z_PROBE_REPETITIONS, true);
  295.     //move to h3
  296.     Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  297.     //probe h3
  298.     float h3 = Printer::runZProbe(false, false, Z_PROBE_REPETITIONS, true);
  299.     //move to h4
  300.     Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  301.     //probe h4
  302.     float h4 = Printer::runZProbe(false, true, Z_PROBE_REPETITIONS, true);
  303.     // h1 is reference heights, h2 => motor 0, h3 => motor 1, h4 => motor 2
  304.     Com::printF(PSTR("h1 = "),h1,4);
  305.     h2 -= h1;
  306.     Com::printF(PSTR("  h2 = "),h2,4);
  307.     h3 -= h1;
  308.     Com::printF(PSTR("  h3 = "),h3,4);
  309.     h4 -= h1;
  310.     Com::printFLN(PSTR("  h4 = "),h4,4);
  311.    
  312. #if defined(LIMIT_MOTORIZED_CORRECTION)
  313. Com::printF(PSTR("LIMIT_MOTORIZED_CORRECTION = "),LIMIT_MOTORIZED_CORRECTION,4);
  314. if(h2 < -LIMIT_MOTORIZED_CORRECTION) h2 = -LIMIT_MOTORIZED_CORRECTION;
  315. if(h2 > LIMIT_MOTORIZED_CORRECTION) h2 = LIMIT_MOTORIZED_CORRECTION;
  316. if(h3 < -LIMIT_MOTORIZED_CORRECTION) h3 = -LIMIT_MOTORIZED_CORRECTION;
  317. if(h3 > LIMIT_MOTORIZED_CORRECTION) h3 = LIMIT_MOTORIZED_CORRECTION;
  318. if(h4 < -LIMIT_MOTORIZED_CORRECTION) h4 = -LIMIT_MOTORIZED_CORRECTION;
  319. if(h4 > LIMIT_MOTORIZED_CORRECTION) h4 = LIMIT_MOTORIZED_CORRECTION;
  320. #endif
  321.     MotorDriverInterface *motor2 = getMotorDriver(0);
  322.     MotorDriverInterface *motor3 = getMotorDriver(1);
  323.     MotorDriverInterface *motor4 = getMotorDriver(2);
  324.     motor2->setCurrentAs(0);
  325.     motor3->setCurrentAs(0);
  326.     motor4->setCurrentAs(0);
  327.     motor2->gotoPosition(-h2);
  328.     motor3->gotoPosition(-h3);
  329.     motor4->gotoPosition(-h4);
  330.     motor2->disable();
  331.     motor3->disable();
  332.     motor4->disable(); // now bed is even
  333.     Printer::currentPositionSteps[Z_AXIS] = h1 * Printer::axisStepsPerMM[Z_AXIS];
  334. #if NONLINEAR_SYSTEM
  335.     transformCartesianStepsToDeltaSteps(Printer::currentPositionSteps,
  336.                                         Printer::currentNonlinearPositionSteps);
  337. #endif
  338. #else
  339. #error Unknown bed correction method set
  340. #endif
  341.   if(fabsf(h3) > fabsf(h2)){
  342.     if(fabsf(h4) > fabsf(h3)){
  343.       return fabsf(h4);
  344.     }
  345.       return fabsf(h3);
  346.     }
  347.   if(fabsf(h4) > fabsf(h2)){
  348.     return fabsf(h4);
  349.   }else{
  350.     return fabsf(h2);
  351.   }
  352. }
  353.  
  354. /**
  355. Implementation of the G32 command
  356. G32 S<0..2> - Autolevel print bed. S = 1 measure zLength, S = 2 Measure and
  357. store new zLength S = 0 : Do not update length - use this if you have not homed
  358. before or you mess up zLength! S = 1 : Measure zLength so homing works S = 2 :
  359. Like s = 1 plus store results in EEPROM for next connection.
  360. */
  361. bool runBedLeveling(int s) {
  362.   Com::printFLN(PSTR("Autolevel initiated")); // Let user know autolevel is beginning
  363.     bool success = true;
  364. #if DISTORTION_CORRECTION
  365.     bool distEnabled = Printer::distortion.isEnabled();
  366.     Printer::distortion.disable(
  367.         false); // if level has changed, distortion is also invalid
  368. #endif
  369.     Printer::prepareForProbing();
  370. #if defined(Z_PROBE_MIN_TEMPERATURE) && Z_PROBE_MIN_TEMPERATURE && Z_PROBE_REQUIRES_HEATING
  371.     float actTemp[NUM_EXTRUDER];
  372.     for (int i = 0; i < NUM_EXTRUDER; i++) {
  373.         actTemp[i] = extruder[i].tempControl.targetTemperatureC;
  374.     }
  375.     Printer::moveToReal(
  376.         IGNORE_COORDINATE, IGNORE_COORDINATE,
  377.         RMath::max(EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0),
  378.                    static_cast<float>(ZHOME_HEAT_HEIGHT)),
  379.         IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
  380.     Commands::waitUntilEndOfAllMoves();
  381. #if ZHOME_HEAT_ALL
  382.     for (int i = 0; i < NUM_EXTRUDER; i++) {
  383.         Extruder::setTemperatureForExtruder(
  384.             RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i,
  385.             false, false);
  386.     }
  387.     for (int i = 0; i < NUM_EXTRUDER; i++) {
  388.         if (extruder[i].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
  389.             Extruder::setTemperatureForExtruder(
  390.                 RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i,
  391.                 false, true);
  392.     }
  393. #else
  394.     if (extruder[Extruder::current->id].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
  395.         Extruder::setTemperatureForExtruder(
  396.             RMath::max(actTemp[Extruder::current->id],
  397.                        static_cast<float>(ZPROBE_MIN_TEMPERATURE)),
  398.             Extruder::current->id, false, true);
  399. #endif
  400. #endif //  defined(Z_PROBE_MIN_TEMPERATURE) ...
  401.  
  402.     float h1, h2, h3, hc, oldFeedrate = Printer::feedrate;
  403.     Printer::setAutolevelActive(false); // iterate
  404.     Printer::resetTransformationMatrix(
  405.         true); // in case we switch from matrix to motorized!
  406. #if DRIVE_SYSTEM == DELTA
  407.     // It is not possible to go to the edges at the top, also users try
  408.     // it often and wonder why the coordinate system is then wrong.
  409.     // For that reason we ensure a correct behavior by code.
  410.     if (!Printer::isHomedAll()) {
  411.         Printer::homeAxis(true, true, true);
  412.     }
  413.     Printer::moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE,
  414.                     EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0),
  415.                     IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
  416. #else
  417.     if (!Printer::isXHomed() || !Printer::isYHomed())
  418.         Printer::homeAxis(true, true, false);
  419.     Printer::updateCurrentPosition(true);
  420.     // Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE,
  421.     // IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  422.     Printer::moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, IGNORE_COORDINATE,
  423.                     IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
  424. #endif
  425.     Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0;
  426.     if (!Printer::startProbing(true)) {
  427.         return false;
  428.     }
  429.     // GCode::executeFString(Com::tZProbeStartScript);
  430.     Plane plane;
  431. #if BED_CORRECTION_METHOD == 1
  432.     success = false;
  433.     for (int r = 0; r < BED_LEVELING_REPETITIONS; r++) {
  434. #if DRIVE_SYSTEM == DELTA
  435.         if (r > 0) {
  436.             Printer::finishProbing();
  437.             Printer::homeAxis(true, true, true);
  438.             Printer::moveTo(
  439.                 IGNORE_COORDINATE, IGNORE_COORDINATE,
  440.                 EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0),
  441.                 IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
  442.             if (!Printer::startProbing(true)) {
  443.                 return false;
  444.             }
  445.         }
  446. #endif // DELTA
  447. #endif // BED_CORRECTION_METHOD == 1
  448. //        if (!measureAutolevelPlane(plane)) {
  449. //            Com::printErrorFLN(
  450. //                PSTR("Probing had returned errors - autoleveling canceled."));
  451. //            UI_MESSAGE(1);
  452. //            return false;
  453. //        }
  454.         float h = correctAutolevel(plane);
  455.  
  456.         // Leveling is finished now update own positions and store leveling data if
  457.         // needed
  458.         // float currentZ = plane.z((float)Printer::currentPositionSteps[X_AXIS] *
  459.         // Printer::invAxisStepsPerMM[X_AXIS],(float)Printer::currentPositionSteps[Y_AXIS]
  460.         // * Printer::invAxisStepsPerMM[Y_AXIS]);
  461.         float currentZ = plane.z(
  462.             0.0, 0.0); // we rotated around this point, so that is now z height
  463.         // With max z end stop we adjust z length so after next homing we have also
  464.         // a calibrated printer
  465.         Printer::zMin = 0;
  466. #if MAX_HARDWARE_ENDSTOP_Z
  467.         // float xRot,yRot,zRot;
  468.         // Printer::transformFromPrinter(Printer::currentPosition[X_AXIS],Printer::currentPosition[Y_AXIS],Printer::currentPosition[Z_AXIS],xRot,yRot,zRot);
  469.         // Com::printFLN(PSTR("Z after rotation:"),zRot);
  470.         // With max z end stop we adjust z length so after next homing we have also
  471.         // a calibrated printer
  472.         if (s != 0) {
  473.             // at origin rotations have no influence so use values there to update
  474.             Printer::zLength += currentZ - Printer::currentPosition[Z_AXIS];
  475.             // Printer::zLength += /*currentZ*/
  476.             // plane.z((float)Printer::currentPositionSteps[X_AXIS] *
  477.             // Printer::invAxisStepsPerMM[X_AXIS],(float)Printer::currentPositionSteps[Y_AXIS]
  478.             // * Printer::invAxisStepsPerMM[Y_AXIS]) - zRot;
  479.             Com::printFLN(Com::tZProbePrinterHeight, Printer::zLength);
  480.         }
  481. #endif
  482. #if Z_PROBE_Z_OFFSET_MODE == 1
  483.         currentZ -= EEPROM::zProbeZOffset();
  484. #endif
  485.         Com::printF(PSTR("CurrentZ:"), currentZ); // this is at x=0, y=0!
  486.         Com::printFLN(PSTR(" atZ:"), Printer::currentPosition[Z_AXIS]);
  487.         currentZ = plane.z(Printer::currentPosition[X_AXIS], Printer::currentPosition[Y_AXIS]); // we are not at 0,0 in general so update for our position!
  488.         Printer::currentPositionSteps[Z_AXIS] = currentZ * Printer::axisStepsPerMM[Z_AXIS];
  489.         Printer::updateCurrentPosition(
  490.             true); // set position based on steps position
  491. #if BED_CORRECTION_METHOD == 1
  492.         if (fabsf(plane.a) < 0.00001 && fabsf(plane.b) < 0.00001) {
  493.             success = true;
  494.             Com::printFLN(PSTR("Acceptable precision reached"));
  495.             break; // we reached achievable precision so we can stop
  496.         }
  497.  
  498.         if(fabsf(h) < 0.01){
  499.           success = true;
  500.           Com::printFLN(PSTR("Acceptable precision reached"));
  501.           break; // we reached achievable precision so we can stop
  502.         }
  503.       if(r == BED_LEVELING_REPETITIONS - 1){
  504.         Com::printFLN(PSTR("Autolevel Completed")); // Let user know autolevel is completed
  505.       }
  506.     } // for BED_LEVELING_REPETITIONS
  507. #if Z_HOME_DIR > 0 && MAX_HARDWARE_ENDSTOP_Z
  508.     float zall = Printer::runZProbe(false, false, 1, false);
  509.     if (zall == ILLEGAL_Z_PROBE)
  510.         return false;
  511.     Printer::currentPosition[Z_AXIS] = zall;
  512.     Printer::currentPositionSteps[Z_AXIS] = zall * Printer::axisStepsPerMM[Z_AXIS];
  513. #if NONLINEAR_SYSTEM
  514.     transformCartesianStepsToDeltaSteps(Printer::currentPositionSteps,
  515.                                         Printer::currentNonlinearPositionSteps);
  516. #endif
  517.     if (s >= 1) {
  518.         float zMax = Printer::runZMaxProbe();
  519.         if (zMax == ILLEGAL_Z_PROBE)
  520.             return false;
  521.         zall += zMax - ENDSTOP_Z_BACK_ON_HOME;
  522.         Printer::zLength = zall;
  523.     }
  524. #endif
  525. #endif // BED_CORRECTION_METHOD == 1
  526.     Printer::updateDerivedParameter();
  527.     Printer::finishProbing();
  528. #if BED_CORRECTION_METHOD != 1
  529.     Printer::setAutolevelActive(
  530.         true); // only for software correction or we can spare the comp. time
  531. #endif
  532.     if (s >= 2) {
  533.         EEPROM::storeDataIntoEEPROM();
  534.     }
  535. #if DISTORTION_CORRECTION
  536.     if (distEnabled)
  537.         Printer::distortion.enable(
  538.             false); // if level has changed, distortion is also invalid
  539. #endif
  540.     Printer::updateCurrentPosition(true);
  541.     Commands::printCurrentPosition();
  542. #if DRIVE_SYSTEM == DELTA
  543.     Printer::homeAxis(
  544.         true, true,
  545.         true); // shifting z makes positioning invalid, need to recalibrate
  546. #endif
  547.     Printer::feedrate = oldFeedrate;
  548.  
  549. #if defined(Z_PROBE_MIN_TEMPERATURE) && Z_PROBE_MIN_TEMPERATURE && Z_PROBE_REQUIRES_HEATING
  550. #if ZHOME_HEAT_ALL
  551.     for (int i = 0; i < NUM_EXTRUDER; i++) {
  552.         Extruder::setTemperatureForExtruder(
  553.             RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i,
  554.             false, false);
  555.     }
  556.     for (int i = 0; i < NUM_EXTRUDER; i++) {
  557.         if (extruder[i].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
  558.             Extruder::setTemperatureForExtruder(
  559.                 RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i,
  560.                 false, true);
  561.     }
  562. #else
  563.     if (extruder[Extruder::current->id].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
  564.         Extruder::setTemperatureForExtruder(
  565.             RMath::max(actTemp[Extruder::current->id],
  566.                        static_cast<float>(ZPROBE_MIN_TEMPERATURE)),
  567.             Extruder::current->id, false, true);
  568. #endif
  569. #endif
  570.  
  571.     return success;
  572. }
  573.  
  574. #endif
  575.  
  576. /** \brief Activate or deactivate rotation correction.
  577.  
  578. \param on True if Rotation correction should be enabled.
  579. */
  580. void Printer::setAutolevelActive(bool on) {
  581. #if FEATURE_AUTOLEVEL
  582.     if (on == isAutolevelActive()) {
  583.         return;
  584.     }
  585.     flag0 = (on ? flag0 | PRINTER_FLAG0_AUTOLEVEL_ACTIVE
  586.                 : flag0 & ~PRINTER_FLAG0_AUTOLEVEL_ACTIVE);
  587.     if (on) {
  588.         Com::printInfoFLN(Com::tAutolevelEnabled);
  589.     } else {
  590.         Com::printInfoFLN(Com::tAutolevelDisabled);
  591.     }
  592.     updateCurrentPosition(false);
  593. #endif // FEATURE_AUTOLEVEL
  594. }
  595. #if MAX_HARDWARE_ENDSTOP_Z
  596. /** \brief Measure distance from current position until triggering z max
  597. endstop.
  598.  
  599. \return Distance until triggering in mm. */
  600. float Printer::runZMaxProbe() {
  601. #if NONLINEAR_SYSTEM
  602.     long startZ = realDeltaPositionSteps[Z_AXIS] = currentNonlinearPositionSteps[Z_AXIS]; // update real
  603. #endif
  604.     Commands::waitUntilEndOfAllMoves();
  605.     long probeDepth = 2 * (Printer::zMaxSteps - Printer::zMinSteps);
  606.     stepsRemainingAtZHit = -1;
  607.     setZProbingActive(true);
  608.     PrintLine::moveRelativeDistanceInSteps(
  609.         0, 0, probeDepth, 0,
  610.         homingFeedrate[Z_AXIS] / ENDSTOP_Z_RETEST_REDUCTION_FACTOR, true, true);
  611.     if (stepsRemainingAtZHit < 0) {
  612.         Com::printErrorFLN(PSTR("z-max homing failed"));
  613.         return ILLEGAL_Z_PROBE;
  614.     }
  615.     setZProbingActive(false);
  616.     currentPositionSteps[Z_AXIS] -= stepsRemainingAtZHit;
  617. #if NONLINEAR_SYSTEM
  618.     transformCartesianStepsToDeltaSteps(Printer::currentPositionSteps,
  619.                                         Printer::currentNonlinearPositionSteps);
  620.     probeDepth = (realDeltaPositionSteps[Z_AXIS] - startZ);
  621. #else
  622.     probeDepth -= stepsRemainingAtZHit;
  623. #endif
  624.     float distance = (float)probeDepth * invAxisStepsPerMM[Z_AXIS];
  625.     Com::printF(Com::tZProbeMax, distance);
  626.     Com::printF(Com::tSpaceXColon, realXPosition());
  627.     Com::printFLN(Com::tSpaceYColon, realYPosition());
  628.     PrintLine::moveRelativeDistanceInSteps(0, 0, -probeDepth, 0,
  629.                                            homingFeedrate[Z_AXIS], true, true);
  630.     return distance;
  631. }
  632. #endif
  633.  
  634. #if FEATURE_Z_PROBE
  635. /** \brief Activate z-probe
  636.  
  637. Tests if switching from active tool to z-probe is possible at current position.
  638. If not the operation is aborted. If ok, it runs start script, checks z position
  639. and applies the z-probe offset.
  640.  
  641. \param runScript Run start z-probe script from configuration.
  642. \param enforceStartHeight If true moves z to EEPROM::zProbeBedDistance() +
  643. (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0) + 0.1 if current
  644. position is higher. \return True if activation was successful. */
  645. bool Printer::startProbing(bool runScript, bool enforceStartHeight) {
  646.     float cx, cy, cz;
  647.     realPosition(cx, cy, cz);
  648.     // Fix position to be inside print area when probe is enabled
  649. #if EXTRUDER_IS_Z_PROBE == 0
  650.     float ZPOffsetX = EEPROM::zProbeXOffset();
  651.     float ZPOffsetY = EEPROM::zProbeYOffset();
  652. #if DRIVE_SYSTEM == DELTA
  653.     float rad = EEPROM::deltaMaxRadius();
  654.     float dx = Printer::currentPosition[X_AXIS] - ZPOffsetX;
  655.     float dy = Printer::currentPosition[Y_AXIS] - ZPOffsetY;
  656.     if (sqrt(dx * dx + dy * dy) > rad)
  657. #else
  658.     if ((ZPOffsetX > 0 && Printer::currentPosition[X_AXIS] - ZPOffsetX < Printer::xMin) || (ZPOffsetY > 0 && Printer::currentPosition[Y_AXIS] - ZPOffsetY < Printer::yMin) || (ZPOffsetX < 0 && Printer::currentPosition[X_AXIS] - ZPOffsetX > Printer::xMin + Printer::xLength) || (ZPOffsetY < 0 && Printer::currentPosition[Y_AXIS] - ZPOffsetY > Printer::yMin + Printer::yLength))
  659. #endif
  660.     {
  661.         Com::printErrorF(
  662.             PSTR("Activating z-probe would lead to forbidden xy position: "));
  663.         Com::print(Printer::currentPosition[X_AXIS] - ZPOffsetX);
  664.         Com::printFLN(PSTR(", "), Printer::currentPosition[Y_AXIS] - ZPOffsetY);
  665.         GCode::fatalError(PSTR("Could not activate z-probe offset due to "
  666.                                "coordinate constraints - result is inaccurate!"));
  667.         return false;
  668.     } else {
  669.         if (runScript) {
  670.             GCode::executeFString(Com::tZProbeStartScript);
  671.         }
  672.         float maxStartHeight = EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0) + 0.1;
  673.         if (currentPosition[Z_AXIS] > maxStartHeight && enforceStartHeight) {
  674.             cz = maxStartHeight;
  675.             moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, maxStartHeight,
  676.                    IGNORE_COORDINATE, homingFeedrate[Z_AXIS]);
  677.         }
  678.  
  679.         // Update position
  680.         Printer::offsetX = -ZPOffsetX;
  681.         Printer::offsetY = -ZPOffsetY;
  682.         Printer::offsetZ = 0;
  683. #if FEATURE_AUTOLEVEL
  684.         // we must not change z for the probe offset even if we are rotated, so add
  685.         // a correction for z
  686.         float dx, dy;
  687.         transformToPrinter(EEPROM::zProbeXOffset(), EEPROM::zProbeYOffset(), 0, dx,
  688.                            dy, offsetZ2);
  689.         // Com::printFLN(PSTR("ZPOffset2:"),offsetZ2,3);
  690. #endif
  691.     }
  692. #else
  693.     if (runScript) {
  694.         GCode::executeFString(Com::tZProbeStartScript);
  695.     }
  696. #endif
  697.     Printer::moveToReal(cx, cy, cz, IGNORE_COORDINATE, EXTRUDER_SWITCH_XY_SPEED);
  698.     updateCurrentPosition(false);
  699.     return true;
  700. }
  701.  
  702. /** \brief Deactivate z-probe. */
  703. void Printer::finishProbing() {
  704.     float cx, cy, cz;
  705.     realPosition(cx, cy, cz);
  706.     GCode::executeFString(Com::tZProbeEndScript);
  707.     if (Extruder::current) {
  708. #if DUAL_X_AXIS
  709.         offsetX = 0; // offsets are parking positions for dual x axis!
  710. #else
  711.         offsetX = -Extruder::current->xOffset * invAxisStepsPerMM[X_AXIS];
  712. #endif
  713.         offsetY = -Extruder::current->yOffset * invAxisStepsPerMM[Y_AXIS];
  714.         offsetZ = -Extruder::current->zOffset * invAxisStepsPerMM[Z_AXIS];
  715.     } else {
  716.         offsetX = offsetY = offsetZ = 0;
  717.     }
  718.     offsetZ2 = 0;
  719.     Printer::moveToReal(cx, cy, cz, IGNORE_COORDINATE, EXTRUDER_SWITCH_XY_SPEED);
  720. }
  721.  
  722. /** \brief Measure distance to bottom at current position.
  723.  
  724. This is the most important function for bed leveling. It does
  725. 1. Run probe start script if first = true and runStartScript = true
  726. 2. Position zProbe at current position if first = true. If we are more then
  727. maxStartHeight away from bed we also go down to that distance.
  728. 3. Measure the the steps until probe hits the bed.
  729. 4. Undo positioning to z probe and run finish script if last = true.
  730.  
  731. Now we compute the nozzle height as follows:
  732. a) Compute average height from repeated measurements
  733. b) Add zProbeHeight to correct difference between triggering point and nozzle
  734. height above bed c) If Z_PROBE_Z_OFFSET_MODE == 1 we add zProbeZOffset() that is
  735. coating thickness if we measure below coating with indictive sensor. d) Add
  736. distortion correction. e) Add bending correction
  737.  
  738. Then we return the measured and corrected z distance.
  739.  
  740. \param first If true, Printer::startProbing is called.
  741. \param last If true, Printer::finishProbing is called at the end.
  742. \param repeat Number of repetitions to average measurement errors.
  743. \param runStartScript If true tells startProbing to run start script.
  744. \param enforceStartHeight Tells start script to enforce a maximum distance to
  745. bed. \return ILLEGAL_Z_PROBE on errors or measured distance.
  746. */
  747. float Printer::runZProbe(bool first, bool last, uint8_t repeat,
  748.                          bool runStartScript, bool enforceStartHeight) {
  749.     float oldOffX = Printer::offsetX;
  750.     float oldOffY = Printer::offsetY;
  751.     float oldOffZ = Printer::offsetZ;
  752.     if (first) {
  753.         if (!startProbing(runStartScript, enforceStartHeight))
  754.             return ILLEGAL_Z_PROBE;
  755.     }
  756.     Commands::waitUntilEndOfAllMoves();
  757.     float zStart = currentPosition[Z_AXIS];
  758. #if defined(Z_PROBE_USE_MEDIAN) && Z_PROBE_USE_MEDIAN
  759.     int32_t measurements[Z_PROBE_REPETITIONS];
  760.     repeat = RMath::min(repeat, Z_PROBE_REPETITIONS);
  761. #else
  762.     int32_t sum = 0;
  763. #endif
  764.     int32_t probeDepth;
  765.     int32_t shortMove = static_cast<int32_t>(
  766.         (float)Z_PROBE_SWITCHING_DISTANCE * axisStepsPerMM[Z_AXIS]); // distance to go up for repeated moves
  767.     int32_t lastCorrection = currentPositionSteps[Z_AXIS];           // starting position
  768. #if NONLINEAR_SYSTEM
  769.     realDeltaPositionSteps[Z_AXIS] = currentNonlinearPositionSteps[Z_AXIS]; // update real
  770. #endif
  771. #if DISTORTION_CORRECTION
  772.     bool distOn = Printer::distortion.isEnabled();
  773. #if DRIVE_SYSTEM != DELTA
  774.     int32_t zStepsIncluded = zCorrectionStepsIncluded;
  775.     zCorrectionStepsIncluded = 0; // prevent computing with this included distortion
  776. #endif
  777.     Printer::distortion.disable(false, true);
  778. #endif
  779.     // int32_t updateZ = 0;
  780.     waitForZProbeStart();
  781. #if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
  782.     HAL::delayMilliseconds(Z_PROBE_DELAY);
  783. #endif
  784.     Endstops::update();
  785.     Endstops::update(); // need to call twice for full update!
  786.     if (Endstops::zProbe()) {
  787.         Com::printErrorFLN(PSTR("z-probe triggered before starting probing."));
  788.         return ILLEGAL_Z_PROBE;
  789.     }
  790. #if Z_PROBE_DISABLE_HEATERS
  791.     Extruder::pauseExtruders(true);
  792.     HAL::delayMilliseconds(70);
  793. #endif
  794.     for (int8_t r = 0; r < repeat; r++) {
  795.         probeDepth = 2 * (Printer::zMaxSteps - Printer::zMinSteps); // probe should always hit within this distance
  796.         stepsRemainingAtZHit = -1;                                  // Marker that we did not hit z probe
  797.         setZProbingActive(true);                                    // prevents also including distortion
  798. #if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
  799.         HAL::delayMilliseconds(Z_PROBE_DELAY);
  800. #endif
  801.         PrintLine::moveRelativeDistanceInSteps(0, 0, -probeDepth, 0,
  802.                                                EEPROM::zProbeSpeed(), true, true);
  803.         setZProbingActive(false);
  804.         if (stepsRemainingAtZHit < 0) {
  805.             Com::printErrorFLN(Com::tZProbeFailed);
  806.             return ILLEGAL_Z_PROBE;
  807.         }
  808. #if NONLINEAR_SYSTEM
  809.         stepsRemainingAtZHit = realDeltaPositionSteps[C_TOWER] - currentNonlinearPositionSteps[C_TOWER]; // nonlinear moves may split z so stepsRemainingAtZHit is
  810.                                                                                                          // only what is left from last segment not total move.
  811.                                                                                                          // This corrects the problem.
  812. #endif
  813. #if DRIVE_SYSTEM == DELTA
  814.         currentNonlinearPositionSteps[A_TOWER] += stepsRemainingAtZHit; // Update difference
  815.         currentNonlinearPositionSteps[B_TOWER] += stepsRemainingAtZHit;
  816.         currentNonlinearPositionSteps[C_TOWER] += stepsRemainingAtZHit;
  817. #elif NONLINEAR_SYSTEM
  818.         currentNonlinearPositionSteps[Z_AXIS] += stepsRemainingAtZHit;
  819. #endif
  820.         currentPositionSteps[Z_AXIS] += stepsRemainingAtZHit; // now current position is correct
  821. #if defined(Z_PROBE_USE_MEDIAN) && Z_PROBE_USE_MEDIAN
  822.         measurements[r] = lastCorrection - currentPositionSteps[Z_AXIS];
  823. #else
  824.         sum += lastCorrection - currentPositionSteps[Z_AXIS];
  825. #endif
  826.  
  827.         // Com::printFLN(PSTR("ZHSteps:"),lastCorrection -
  828.         // currentPositionSteps[Z_AXIS]);
  829.         if (r + 1 < repeat) {
  830.             // go only shortest possible move up for repetitions
  831.             PrintLine::moveRelativeDistanceInSteps(
  832.                 0, 0, shortMove, 0, Printer::homingFeedrate[Z_AXIS], true, false);
  833. #if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
  834.             HAL::delayMilliseconds(Z_PROBE_DELAY);
  835. #endif
  836.             Endstops::update();
  837.             Endstops::update(); // need to call twice for full update!
  838.             if (Endstops::zProbe()) {
  839.                 Com::printErrorFLN(
  840.                     PSTR("z-probe did not untrigger on repetitive measurement - maybe "
  841.                          "you need to increase distance!"));
  842.                 UI_MESSAGE(1);
  843.                 return ILLEGAL_Z_PROBE;
  844.             }
  845.         }
  846. #ifdef Z_PROBE_RUN_AFTER_EVERY_PROBE
  847.         GCode::executeFString(PSTR(Z_PROBE_RUN_AFTER_EVERY_PROBE));
  848. #endif
  849.     }
  850. #if Z_PROBE_DISABLE_HEATERS
  851.     Extruder::unpauseExtruders(false);
  852. #endif
  853.  
  854.     // Go back to start position
  855.     PrintLine::moveRelativeDistanceInSteps(
  856.         0, 0, lastCorrection - currentPositionSteps[Z_AXIS], 0,
  857.         Printer::homingFeedrate[Z_AXIS], true, false);
  858.  
  859. #if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
  860.     HAL::delayMilliseconds(Z_PROBE_DELAY);
  861. #endif
  862.     Endstops::update();
  863.     Endstops::update();       // need to call twice for full update!
  864.     if (Endstops::zProbe()) { // did we untrigger? If not don't trust result!
  865.         Com::printErrorFLN(PSTR(
  866.             "z-probe did not untrigger - maybe you need to increase distance!"));
  867.         UI_MESSAGE(1);
  868.         return ILLEGAL_Z_PROBE;
  869.     }
  870.     // Com::printFLN(PSTR("after probe"));
  871.     // Commands::printCurrentPosition();
  872. #if defined(Z_PROBE_USE_MEDIAN) && Z_PROBE_USE_MEDIAN
  873.     // bubble sort the measurements
  874.     int32_t tmp;
  875.     for (fast8_t i = 0; i < repeat - 1;
  876.          i++) {                                          // n numbers require at most n-1 rounds of swapping
  877.         for (fast8_t j = 0; j < repeat - i - 1; j++) {   //
  878.             if (measurements[j] > measurements[j + 1]) { // out of order?
  879.                 tmp = measurements[j];                   // swap them:
  880.                 measurements[j] = measurements[j + 1];
  881.                 measurements[j + 1] = tmp;
  882.             }
  883.         }
  884.     }
  885.     // process result
  886.     float distance = static_cast<float>(measurements[repeat >> 1]) * invAxisStepsPerMM[Z_AXIS] + EEPROM::zProbeHeight();
  887. #else
  888.     float distance = static_cast<float>(sum) * invAxisStepsPerMM[Z_AXIS] / static_cast<float>(repeat) + EEPROM::zProbeHeight();
  889. #endif
  890.  
  891. #if DISTORTION_CORRECTION
  892. #if DRIVE_SYSTEM != DELTA
  893.     zCorrectionStepsIncluded = zStepsIncluded;
  894. #endif
  895.     if (distOn) {
  896.         Printer::distortion.enable(false, true);
  897.     }
  898.     float zCorr = 0;
  899.     if (Printer::distortion.isEnabled()) {
  900.         zCorr = distortion.correct(
  901.                     currentPositionSteps[X_AXIS] /* + EEPROM::zProbeXOffset() *
  902.                                                 axisStepsPerMM[X_AXIS]*/
  903.                     ,
  904.                     currentPositionSteps[Y_AXIS]
  905.                     /* + EEPROM::zProbeYOffset() * axisStepsPerMM[Y_AXIS]*/,
  906.                     zStart * axisStepsPerMM[Z_AXIS] /* currentPositionSteps[Z_AXIS] */ /* zMinSteps */)
  907.             * invAxisStepsPerMM[Z_AXIS];
  908.     }
  909. #endif
  910.  
  911.     updateCurrentPosition(false);
  912.  
  913. #if FEATURE_AUTOLEVEL
  914.     // we must change z for the z change from moving in rotated coordinates away
  915.     // from real position
  916.     float dx, dy, dz;
  917.     transformToPrinter(0, 0, currentPosition[Z_AXIS], dx, dy,
  918.                        dz); // what is our x,y offset from z position
  919.     dz -= currentPosition[Z_AXIS];
  920.     // Com::printF(PSTR("ZXO:"),dx,3);Com::printF(PSTR(" ZYO:"),dy,3);
  921.     // transformToPrinter(dx,dy,0,dx,dy,dz); // how much changes z from x,y
  922.     // offset? Com::printFLN(PSTR(" Z from xy off:"), dz,7);
  923.     distance += dz;
  924. #endif
  925.     // Com::printFLN(PSTR("OrigDistance:"),distance);
  926. #if Z_PROBE_Z_OFFSET_MODE == 1
  927.     distance += EEPROM::zProbeZOffset(); // We measured including coating, so we
  928.                                          // need to add coating thickness!
  929. #endif
  930.  
  931.     distance += bendingCorrectionAt(currentPosition[X_AXIS], currentPosition[Y_AXIS]);
  932.  
  933.     Com::printF(Com::tZProbe, distance, 3);
  934.     Com::printF(Com::tSpaceXColon, realXPosition());
  935. #if DISTORTION_CORRECTION
  936.     if (Printer::distortion.isEnabled()) {
  937.         Com::printF(Com::tSpaceYColon, realYPosition());
  938.         Com::printFLN(PSTR(" zCorr:"), zCorr, 3);
  939.     } else {
  940.         Com::printFLN(Com::tSpaceYColon, realYPosition());
  941.     }
  942. #else
  943.     Com::printFLN(Com::tSpaceYColon, realYPosition());
  944. #endif
  945. #if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
  946.     HAL::delayMilliseconds(Z_PROBE_DELAY);
  947. #endif
  948.     Endstops::update();
  949.     Endstops::update(); // need to call twice for full update!
  950.     if (Endstops::zProbe()) {
  951.         Com::printErrorFLN(
  952.             PSTR("z-probe did not untrigger after going back to start position."));
  953.         UI_MESSAGE(1);
  954.         return ILLEGAL_Z_PROBE;
  955.     }
  956.     if (last) {
  957.         finishProbing();
  958.     }
  959.  
  960.     return distance;
  961. }
  962.  
  963. /**
  964.  * Having printer's height set properly (i.e. after calibration of Z=0), one can
  965.  * use this procedure to measure Z-probe height. It deploys the sensor, takes
  966.  * several probes at center, then updates Z-probe height with average.
  967.  */
  968. void Printer::measureZProbeHeight(float curHeight) {
  969. #if FEATURE_Z_PROBE
  970.     currentPositionSteps[Z_AXIS] = curHeight * axisStepsPerMM[Z_AXIS];
  971.     updateCurrentPosition(true);
  972. #if NONLINEAR_SYSTEM
  973.     transformCartesianStepsToDeltaSteps(currentPositionSteps,
  974.                                         currentNonlinearPositionSteps);
  975. #endif
  976.     float startHeight = EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0);
  977.     moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, startHeight, IGNORE_COORDINATE,
  978.            homingFeedrate[Z_AXIS]);
  979.     float zheight = Printer::runZProbe(true, true, Z_PROBE_REPETITIONS, true);
  980.     if (zheight == ILLEGAL_Z_PROBE) {
  981.         return;
  982.     }
  983.     float zProbeHeight = EEPROM::zProbeHeight() + startHeight - zheight;
  984.  
  985. #if EEPROM_MODE != 0                       // Com::tZProbeHeight is not declared when EEPROM_MODE is 0
  986.     EEPROM::setZProbeHeight(zProbeHeight); // will also report on output
  987. #else
  988.     Com::printFLN(PSTR("Z-probe height [mm]:"), zProbeHeight);
  989. #endif
  990. #endif
  991. }
  992.  
  993. float Printer::bendingCorrectionAt(float x, float y) {
  994.     PlaneBuilder builder;
  995.     builder.addPoint(EEPROM::zProbeX1(), EEPROM::zProbeY1(),
  996.                      EEPROM::bendingCorrectionA());
  997.     builder.addPoint(EEPROM::zProbeX2(), EEPROM::zProbeY2(),
  998.                      EEPROM::bendingCorrectionB());
  999.     builder.addPoint(EEPROM::zProbeX3(), EEPROM::zProbeY3(),
  1000.                      EEPROM::bendingCorrectionC());
  1001.     Plane plane;
  1002.     builder.createPlane(plane, true);
  1003.     return plane.z(x, y);
  1004. }
  1005.  
  1006. void Printer::waitForZProbeStart() {
  1007. #if Z_PROBE_WAIT_BEFORE_TEST
  1008.     Endstops::update();
  1009.     Endstops::update(); // double test to get right signal. Needed for crosstalk
  1010.                         // protection.
  1011.     if (Endstops::zProbe())
  1012.         return;
  1013. #if UI_DISPLAY_TYPE != NO_DISPLAY
  1014.     uid.setStatusP(Com::tHitZProbe);
  1015.     uid.refreshPage();
  1016. #endif
  1017. #ifdef DEBUG_PRINT
  1018.     debugWaitLoop = 3;
  1019. #endif
  1020.     while (!Endstops::zProbe()) {
  1021.         defaultLoopActions();
  1022.         Endstops::update();
  1023.         Endstops::update(); // double test to get right signal. Needed for crosstalk
  1024.                             // protection.
  1025.     }
  1026. #ifdef DEBUG_PRINT
  1027.     debugWaitLoop = 4;
  1028. #endif
  1029.     HAL::delayMilliseconds(30);
  1030.     while (Endstops::zProbe()) {
  1031.         defaultLoopActions();
  1032.         Endstops::update();
  1033.         Endstops::update(); // double test to get right signal. Needed for crosstalk
  1034.                             // protection.
  1035.     }
  1036.     HAL::delayMilliseconds(30);
  1037.     UI_CLEAR_STATUS;
  1038. #endif
  1039. }
  1040. #endif
  1041.  
  1042. /*
  1043.  Transforms theoretical correct coordinates to corrected coordinates resulting
  1044.  from bed rotation and shear transformations.
  1045.  
  1046.  We have 2 coordinate systems. The printer step position where we want to be.
  1047.  These are the positions we send to printers, the theoretical coordinates. In
  1048.  contrast we have the printer coordinates that we need to be at to get the
  1049.  desired result, the real coordinates.
  1050. */
  1051. void Printer::transformToPrinter(float x, float y, float z, float& transX,
  1052.                                  float& transY, float& transZ) {
  1053. #if FEATURE_AXISCOMP
  1054.     // Axis compensation:
  1055.     x = x + y * EEPROM::axisCompTanXY() + z * EEPROM::axisCompTanXZ();
  1056.     y = y + z * EEPROM::axisCompTanYZ();
  1057. #endif
  1058. #if BED_CORRECTION_METHOD != 1 && FEATURE_AUTOLEVEL
  1059.     if (isAutolevelActive()) {
  1060.         transX = x * autolevelTransformation[0] + y * autolevelTransformation[3] + z * autolevelTransformation[6];
  1061.         transY = x * autolevelTransformation[1] + y * autolevelTransformation[4] + z * autolevelTransformation[7];
  1062.         transZ = x * autolevelTransformation[2] + y * autolevelTransformation[5] + z * autolevelTransformation[8];
  1063.     } else {
  1064.         transX = x;
  1065.         transY = y;
  1066.         transZ = z;
  1067.     }
  1068. #else
  1069.     transX = x;
  1070.     transY = y;
  1071.     transZ = z;
  1072. #endif
  1073. }
  1074.  
  1075. /* Transform back to real printer coordinates. */
  1076. void Printer::transformFromPrinter(float x, float y, float z, float& transX,
  1077.                                    float& transY, float& transZ) {
  1078. #if BED_CORRECTION_METHOD != 1 && FEATURE_AUTOLEVEL
  1079.     if (isAutolevelActive()) {
  1080.         transX = x * autolevelTransformation[0] + y * autolevelTransformation[1] + z * autolevelTransformation[2];
  1081.         transY = x * autolevelTransformation[3] + y * autolevelTransformation[4] + z * autolevelTransformation[5];
  1082.         transZ = x * autolevelTransformation[6] + y * autolevelTransformation[7] + z * autolevelTransformation[8];
  1083.     } else {
  1084.         transX = x;
  1085.         transY = y;
  1086.         transZ = z;
  1087.     }
  1088. #else
  1089.     transX = x;
  1090.     transY = y;
  1091.     transZ = z;
  1092. #endif
  1093. #if FEATURE_AXISCOMP
  1094.     // Axis compensation:
  1095.     transY = transY - transZ * EEPROM::axisCompTanYZ();
  1096.     transX = transX - transY * EEPROM::axisCompTanXY() - transZ * EEPROM::axisCompTanXZ();
  1097. #endif
  1098. }
  1099. #if FEATURE_AUTOLEVEL
  1100. void Printer::resetTransformationMatrix(bool silent) {
  1101.     autolevelTransformation[0] = autolevelTransformation[4] = autolevelTransformation[8] = 1;
  1102.     autolevelTransformation[1] = autolevelTransformation[2] = autolevelTransformation[3] = autolevelTransformation[5] = autolevelTransformation[6] = autolevelTransformation[7] = 0;
  1103.     if (!silent)
  1104.         Com::printInfoFLN(Com::tAutolevelReset);
  1105. }
  1106.  
  1107. void Printer::buildTransformationMatrix(Plane& plane) {
  1108.     float z0 = plane.z(0, 0);
  1109.     float az = z0 - plane.z(1, 0); // ax = 1, ay = 0
  1110.     float bz = z0 - plane.z(0, 1); // bx = 0, by = 1
  1111.     // First z direction
  1112.     autolevelTransformation[6] = -az;
  1113.     autolevelTransformation[7] = -bz;
  1114.     autolevelTransformation[8] = 1;
  1115.     float len = sqrt(az * az + bz * bz + 1);
  1116.     autolevelTransformation[6] /= len;
  1117.     autolevelTransformation[7] /= len;
  1118.     autolevelTransformation[8] /= len;
  1119.     autolevelTransformation[0] = 1;
  1120.     autolevelTransformation[1] = 0;
  1121.     autolevelTransformation[2] = -autolevelTransformation[6] / autolevelTransformation[8];
  1122.     len = sqrt(autolevelTransformation[0] * autolevelTransformation[0] + autolevelTransformation[1] * autolevelTransformation[1] + autolevelTransformation[2] * autolevelTransformation[2]);
  1123.     autolevelTransformation[0] /= len;
  1124.     autolevelTransformation[1] /= len;
  1125.     autolevelTransformation[2] /= len;
  1126.     // cross(z,x) y,z)
  1127.     autolevelTransformation[3] = autolevelTransformation[7] * autolevelTransformation[2] - autolevelTransformation[8] * autolevelTransformation[1];
  1128.     autolevelTransformation[4] = autolevelTransformation[8] * autolevelTransformation[0] - autolevelTransformation[6] * autolevelTransformation[2];
  1129.     autolevelTransformation[5] = autolevelTransformation[6] * autolevelTransformation[1] - autolevelTransformation[7] * autolevelTransformation[0];
  1130.     len = sqrt(autolevelTransformation[3] * autolevelTransformation[3] + autolevelTransformation[4] * autolevelTransformation[4] + autolevelTransformation[5] * autolevelTransformation[5]);
  1131.     autolevelTransformation[3] /= len;
  1132.     autolevelTransformation[4] /= len;
  1133.     autolevelTransformation[5] /= len;
  1134.  
  1135.     Com::printArrayFLN(Com::tTransformationMatrix, autolevelTransformation, 9, 6);
  1136. }
  1137. /*
  1138. void Printer::buildTransformationMatrix(float h1,float h2,float h3) {
  1139.     float ax = EEPROM::zProbeX2() - EEPROM::zProbeX1();
  1140.     float ay = EEPROM::zProbeY2() - EEPROM::zProbeY1();
  1141.     float az = h1 - h2;
  1142.     float bx = EEPROM::zProbeX3() - EEPROM::zProbeX1();
  1143.     float by = EEPROM::zProbeY3() - EEPROM::zProbeY1();
  1144.     float bz = h1 - h3;
  1145.     // First z direction
  1146.     autolevelTransformation[6] = ay * bz - az * by;
  1147.     autolevelTransformation[7] = az * bx - ax * bz;
  1148.     autolevelTransformation[8] = ax * by - ay * bx;
  1149.     float len = sqrt(autolevelTransformation[6] * autolevelTransformation[6] +
  1150. autolevelTransformation[7] * autolevelTransformation[7] +
  1151. autolevelTransformation[8] * autolevelTransformation[8]);
  1152.     if(autolevelTransformation[8] < 0) len = -len;
  1153.     autolevelTransformation[6] /= len;
  1154.     autolevelTransformation[7] /= len;
  1155.     autolevelTransformation[8] /= len;
  1156.     autolevelTransformation[3] = 0;
  1157.     autolevelTransformation[4] = autolevelTransformation[8];
  1158.     autolevelTransformation[5] = -autolevelTransformation[7];
  1159.     // cross(y,z)
  1160.     autolevelTransformation[0] = autolevelTransformation[4] *
  1161. autolevelTransformation[8] - autolevelTransformation[5] *
  1162. autolevelTransformation[7]; autolevelTransformation[1] =
  1163. autolevelTransformation[5] * autolevelTransformation[6];// -
  1164. autolevelTransformation[3] * autolevelTransformation[8];
  1165.     autolevelTransformation[2] = autolevelTransformation[3] *
  1166. autolevelTransformation[7] - autolevelTransformation[4] *
  1167. autolevelTransformation[6]; len = sqrt(autolevelTransformation[0] *
  1168. autolevelTransformation[0] + autolevelTransformation[1] *
  1169. autolevelTransformation[1] + autolevelTransformation[2] *
  1170. autolevelTransformation[2]); autolevelTransformation[0] /= len;
  1171.     autolevelTransformation[1] /= len;
  1172.     autolevelTransformation[2] /= len;
  1173.     len = sqrt(autolevelTransformation[4] * autolevelTransformation[4] +
  1174. autolevelTransformation[5] * autolevelTransformation[5]);
  1175.     autolevelTransformation[4] /= len;
  1176.     autolevelTransformation[5] /= len;
  1177.     Com::printArrayFLN(Com::tTransformationMatrix,autolevelTransformation, 9,
  1178. 6);
  1179. }
  1180. */
  1181. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!