957 lines
45 KiB
C++
957 lines
45 KiB
C++
|
|
/*
|
|
More and more printers now have automatic bed leveling using an ever increasing variety of methods.
|
|
This makes the leveling routine one of the most complex parts of the firmware and there is not one
|
|
way to level but hundreds of combinations.
|
|
|
|
First you should decide on the correction method. Once we know how our bed is tilted we want to
|
|
remove that. This correction is defined by BED_CORRECTION_METHOD and allows the following values:
|
|
BED_CORRECTION_METHOD 0
|
|
Use a rotation matrix. This will make z axis go up/down while moving in x/y direction to compensate
|
|
the tilt. For multiple extruders make sure the height match the tilt of the bed or one will scratch.
|
|
|
|
BED_CORRECTION_METHOD 1
|
|
Motorized correction. This method needs a bed that is fixed on 3 points from which 2 have a motor
|
|
to change the height. The positions are defined by
|
|
BED_MOTOR_1_X, BED_MOTOR_1_Y, BED_MOTOR_2_X, BED_MOTOR_2_Y, BED_MOTOR_3_X, BED_MOTOR_3_Y
|
|
Motor 2 and 3 are the one driven by motor driver 0 and 1. These can be extra motors like Felix Pro 1
|
|
uses them or a system with 3 z axis where motors can be controlled individually like the Sparkcube
|
|
does.
|
|
|
|
Next we have to distinguish several methods of z probing sensors. Each have their own advantages and
|
|
disadvantages. First the z probe has a position when activated and that position is defined by
|
|
#define Z_PROBE_X_OFFSET 0
|
|
#define Z_PROBE_Y_OFFSET 0
|
|
This is needed since we need to know where we measure the height when the z probe triggers. When
|
|
probing is activated you will see a move to make probe go over current extruder position. The
|
|
position can be changed in eeprom later on.
|
|
|
|
Some probes need to be activated/deactivated so we can use them. This is defined in the scripts
|
|
#define Z_PROBE_START_SCRIPT ""
|
|
#define Z_PROBE_FINISHED_SCRIPT ""
|
|
|
|
Now when we probe we want to know the distance of the extruder to the bed. This is defined by
|
|
#define Z_PROBE_HEIGHT 4
|
|
The 4 means that when we trigger the distance of the nozzle tip is 4mm. If your switch tends
|
|
to return different points you might repeat a measured point and use the average height:
|
|
#define Z_PROBE_SWITCHING_DISTANCE 1
|
|
#define Z_PROBE_REPETITIONS 5
|
|
Switching distance is the z raise needed to turn back a signal reliably to off. Inductive sensors
|
|
need only a bit while mechanical switches may require a bit more.
|
|
|
|
Next thing to consider is the force for switching. Some beds use a cantilever design and pushing on
|
|
the outside easily bends the bed. If your sensor needs some force to trigger you add the error of
|
|
bending. For this reason you might add a bending correction. Currently you define
|
|
#define BENDING_CORRECTION_A 0
|
|
#define BENDING_CORRECTION_B 0
|
|
#define BENDING_CORRECTION_C 0
|
|
which are the deflections at the 3 z probe points. For all other possible measurements these values
|
|
get interpolated. You can modify the values later on in eeprom. For force less sensors set them to 0.
|
|
|
|
Next thing is endstop handling. Without bed leveling you normally home to minimum position for x,y and z.
|
|
With bed leveling this is not that easy any more. Since we do bed leveling we already assume the bed is
|
|
not leveled for x/y moves. So without correction we would hit the bed for different x/y positions at
|
|
different heights. As a result we have no real minimum position. That makes a z min endstop quite useless.
|
|
There is an exception to this. If your nozzle triggers z min or if a inductive sensor would trigger at a given
|
|
position we could use that signal. With nozzle triggers you need to be careful as a drop of filament
|
|
would change the height. The other problem is that while homing the auto leveling is not used. So
|
|
the only position would be if the z min sensor is directly over the 0,0 coordinate which is the rotation point
|
|
if we have matrix based correction. For motor based correction this will work everywhere correctly.
|
|
|
|
So the only useful position for a z endstop is z max position. Apart from not having the bed tilt problem it
|
|
also allows homing with a full bed so you can continue an aborted print with some gcode tweaking. With z max
|
|
homing we adjust the error by simply changing the max. z height. One thing you need to remember is setting
|
|
#define ENDSTOP_Z_BACK_ON_HOME 4
|
|
so we release the z max endstop. This is very important if we move xy at z max. Auto leveling might want to
|
|
increase z and the endstop might prevent it causing wrong position and a head crash if we later go down.
|
|
The value should be larger then the maximum expected tilt.
|
|
|
|
Now it is time to define how we measure the bed rotation. Here again we have several methods to choose.
|
|
All methods need at least 3 points to define the bed rotation correctly. The quality we get comes
|
|
from the selection of the right points and method.
|
|
|
|
BED_LEVELING_METHOD 0
|
|
This method measures at the 3 probe points and creates a plane through these points. If you have
|
|
a really planar bed this gives the optimum result. The 3 points must not be in one line and have
|
|
a long distance to increase numerical stability.
|
|
|
|
BED_LEVELING_METHOD 1
|
|
This measures a grid. Probe point 1 is the origin and points 2 and 3 span a grid. We measure
|
|
BED_LEVELING_GRID_SIZE points in each direction and compute a regression plane through all
|
|
points. This gives a good overall plane if you have small bumps measuring inaccuracies.
|
|
|
|
BED_LEVELING_METHOD 2
|
|
Bending correcting 4 point measurement. This is for cantilevered beds that have the rotation axis
|
|
not at the side but inside the bed. Here we can assume no bending on the axis and a symmetric
|
|
bending to both sides of the axis. So probe points 2 and 3 build the symmetric axis and
|
|
point 1 is mirrored to 1m across the axis. Using the symmetry we then remove the bending
|
|
from 1 and use that as plane.
|
|
|
|
By now the leveling process is finished. All errors that remain are measuring errors and bumps on
|
|
the bed it self. For deltas you can enable distortion correction to follow the bumps.
|
|
|
|
There are 2 ways to consider a changing bed coating, which are defined by Z_PROBE_Z_OFFSET_MODE.
|
|
Z_PROBE_Z_OFFSET_MODE = 0 means we measure the surface of the bed below any coating. This is e.g.
|
|
the case with inductive sensors where we put BuildTak on top. In that case we can set Z_PROBE_Z_OFFSET
|
|
to the thickness of BuildTak to compensate. If we later change the coating, we only change Z_PROBE_Z_OFFSET
|
|
to new coating thickness.
|
|
|
|
Z_PROBE_Z_OFFSET_MODE = 1 means we measure the surface of the coating, e.g. because we have a mechanical switch.
|
|
In that case we add Z_PROBE_Z_OFFSET for the measured height to compensate for correct distance to bed surface.
|
|
|
|
In homing to max we reduce z length by Z_PROBE_Z_OFFSET to get a correct height.
|
|
In homing to z min we assume z endstop is bed level so we move up Z_PROBE_Z_OFFSET after endstop is hit. This
|
|
requires the extruder to bend the coating thickness without harm!
|
|
*/
|
|
|
|
#include "Repetier.h"
|
|
|
|
#ifndef BED_LEVELING_METHOD
|
|
#define BED_LEVELING_METHOD 0
|
|
#endif
|
|
|
|
#ifndef BED_CORRECTION_METHOD
|
|
#define BED_CORRECTION_METHOD 0
|
|
#endif
|
|
|
|
#ifndef BED_LEVELING_GRID_SIZE
|
|
#define BED_LEVELING_GRID_SIZE 5
|
|
#endif
|
|
|
|
#ifndef BED_LEVELING_REPETITIONS
|
|
#define BED_LEVELING_REPETITIONS 1
|
|
#endif
|
|
|
|
#if FEATURE_Z_PROBE
|
|
void Printer::prepareForProbing() {
|
|
#ifndef SKIP_PROBE_PREPARE
|
|
// 1. Ensure we are homed so positions make sense
|
|
if(!Printer::isHomedAll()) {
|
|
Printer::homeAxis(true, true, true);
|
|
}
|
|
// 2. Go to z probe bed distance for probing
|
|
Printer::moveToReal(IGNORE_COORDINATE, IGNORE_COORDINATE, RMath::max(EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0), static_cast<float>(ZHOME_HEAT_HEIGHT)), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
|
|
// 3. Ensure we can activate z probe at current xy position
|
|
// Delta is at center already so does not need special testing here!
|
|
#if EXTRUDER_IS_Z_PROBE == 0
|
|
float ZPOffsetX = EEPROM::zProbeXOffset();
|
|
float ZPOffsetY = EEPROM::zProbeYOffset();
|
|
#if DRIVE_SYSTEM != DELTA
|
|
float targetX = Printer::currentPosition[X_AXIS];
|
|
float targetY = Printer::currentPosition[Y_AXIS];
|
|
if(ZPOffsetX > 0 && targetX - ZPOffsetX < Printer::xMin) {
|
|
targetX = Printer::xMin + ZPOffsetX;
|
|
}
|
|
if(ZPOffsetY > 0 && targetY - ZPOffsetY < Printer::yMin) {
|
|
targetY = Printer::yMin + ZPOffsetY;
|
|
}
|
|
if(ZPOffsetX < 0 && targetX - ZPOffsetX > Printer::xMin + Printer::xLength) {
|
|
targetX = Printer::xMin + Printer::xLength + ZPOffsetX;
|
|
}
|
|
if(ZPOffsetY < 0 && targetY - ZPOffsetY > Printer::yMin + Printer::yLength) {
|
|
targetY = Printer::yMin + Printer::yLength + ZPOffsetY;
|
|
}
|
|
Printer::moveToReal(targetX, targetY, IGNORE_COORDINATE, IGNORE_COORDINATE, EXTRUDER_SWITCH_XY_SPEED);
|
|
Printer::updateCurrentPosition(true);
|
|
Commands::waitUntilEndOfAllMoves();
|
|
#endif
|
|
#endif
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
#if FEATURE_AUTOLEVEL && FEATURE_Z_PROBE
|
|
|
|
bool measureAutolevelPlane(Plane &plane) {
|
|
PlaneBuilder builder;
|
|
builder.reset();
|
|
#if BED_LEVELING_METHOD == 0 // 3 point
|
|
float h;
|
|
Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h = Printer::runZProbe(false, false);
|
|
if(h == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
builder.addPoint(EEPROM::zProbeX1(), EEPROM::zProbeY1(), h);
|
|
Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h = Printer::runZProbe(false, false);
|
|
if(h == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
builder.addPoint(EEPROM::zProbeX2(), EEPROM::zProbeY2(), h);
|
|
Printer::moveTo(EEPROM::zProbeX3(), EEPROM::zProbeY3(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h = Printer::runZProbe(false, false);
|
|
if(h == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
builder.addPoint(EEPROM::zProbeX3(), EEPROM::zProbeY3(), h);
|
|
#elif BED_LEVELING_METHOD == 1 // linear regression
|
|
float delta = 1.0 / (BED_LEVELING_GRID_SIZE - 1);
|
|
float ox = EEPROM::zProbeX1();
|
|
float oy = EEPROM::zProbeY1();
|
|
float ax = delta * (EEPROM::zProbeX2() - EEPROM::zProbeX1());
|
|
float ay = delta * (EEPROM::zProbeY2() - EEPROM::zProbeY1());
|
|
float bx = delta * (EEPROM::zProbeX3() - EEPROM::zProbeX1());
|
|
float by = delta * (EEPROM::zProbeY3() - EEPROM::zProbeY1());
|
|
for(int ix = 0; ix < BED_LEVELING_GRID_SIZE; ix++) {
|
|
for(int iy = 0; iy < BED_LEVELING_GRID_SIZE; iy++) {
|
|
float px = ox + static_cast<float>(ix) * ax + static_cast<float>(iy) * bx;
|
|
float py = oy + static_cast<float>(ix) * ay + static_cast<float>(iy) * by;
|
|
Printer::moveTo(px, py, IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
float h = Printer::runZProbe(false, false);
|
|
if(h == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
builder.addPoint(px, py, h);
|
|
}
|
|
}
|
|
|
|
#elif BED_LEVELING_METHOD == 2 // 4 point symmetric
|
|
float h1, h2, h3, h4;
|
|
float apx = EEPROM::zProbeX1() - EEPROM::zProbeX2();
|
|
float apy = EEPROM::zProbeY1() - EEPROM::zProbeY2();
|
|
float abx = EEPROM::zProbeX3() - EEPROM::zProbeX2();
|
|
float aby = EEPROM::zProbeY3() - EEPROM::zProbeY2();
|
|
float ab2 = abx * abx + aby * aby;
|
|
float abap = apx * abx + apy * aby;
|
|
float t = abap / ab2;
|
|
float xx = EEPROM::zProbeX2() + t * abx;
|
|
float xy = EEPROM::zProbeY2() + t * aby;
|
|
float x1Mirror = EEPROM::zProbeX1() + 2.0 * (xx - EEPROM::zProbeX1());
|
|
float y1Mirror = EEPROM::zProbeY1() + 2.0 * (xy - EEPROM::zProbeY1());
|
|
Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h1 = Printer::runZProbe(false, false);
|
|
if(h1 == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h2 = Printer::runZProbe(false, false);
|
|
if(h2 == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
Printer::moveTo(EEPROM::zProbeX3(), EEPROM::zProbeY3(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h3 = Printer::runZProbe(false, false);
|
|
if(h3 == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
Printer::moveTo(x1Mirror, y1Mirror, IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
h4 = Printer::runZProbe(false, false);
|
|
if(h4 == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
t = h2 + (h3 - h2) * t; // theoretical height for crossing point for symmetric axis
|
|
h1 = t - (h4 - h1) * 0.5; // remove bending part
|
|
builder.addPoint(EEPROM::zProbeX1(), EEPROM::zProbeY1(), h1);
|
|
builder.addPoint(EEPROM::zProbeX2(), EEPROM::zProbeY2(), h2);
|
|
builder.addPoint(EEPROM::zProbeX3(), EEPROM::zProbeY3(), h3);
|
|
#else
|
|
#error Unknown bed leveling method
|
|
#endif
|
|
builder.createPlane(plane, false);
|
|
return true;
|
|
}
|
|
|
|
void correctAutolevel(Plane &plane) {
|
|
#if BED_CORRECTION_METHOD == 0 // rotation matrix
|
|
//Printer::buildTransformationMatrix(plane.z(EEPROM::zProbeX1(),EEPROM::zProbeY1()),plane.z(EEPROM::zProbeX2(),EEPROM::zProbeY2()),plane.z(EEPROM::zProbeX3(),EEPROM::zProbeY3()));
|
|
Printer::buildTransformationMatrix(plane);
|
|
#elif BED_CORRECTION_METHOD == 1 // motorized correction
|
|
#if !defined(NUM_MOTOR_DRIVERS) || NUM_MOTOR_DRIVERS < 2
|
|
#error You need to define 2 motors for motorized bed correction
|
|
#endif
|
|
Commands::waitUntilEndOfAllMoves(); // move steppers might be leveling steppers as well !
|
|
float h1 = plane.z(BED_MOTOR_1_X, BED_MOTOR_1_Y);
|
|
float h2 = plane.z(BED_MOTOR_2_X, BED_MOTOR_2_Y);
|
|
float h3 = plane.z(BED_MOTOR_3_X, BED_MOTOR_3_Y);
|
|
// h1 is reference heights, h2 => motor 0, h3 => motor 1
|
|
h2 -= h1;
|
|
h3 -= h1;
|
|
#if defined(LIMIT_MOTORIZED_CORRECTION)
|
|
if(h2 < -LIMIT_MOTORIZED_CORRECTION) h2 = -LIMIT_MOTORIZED_CORRECTION;
|
|
if(h2 > LIMIT_MOTORIZED_CORRECTION) h2 = LIMIT_MOTORIZED_CORRECTION;
|
|
if(h3 < -LIMIT_MOTORIZED_CORRECTION) h3 = -LIMIT_MOTORIZED_CORRECTION;
|
|
if(h3 > LIMIT_MOTORIZED_CORRECTION) h3 = LIMIT_MOTORIZED_CORRECTION;
|
|
#endif
|
|
MotorDriverInterface *motor2 = getMotorDriver(0);
|
|
MotorDriverInterface *motor3 = getMotorDriver(1);
|
|
motor2->setCurrentAs(0);
|
|
motor3->setCurrentAs(0);
|
|
motor2->gotoPosition(h2);
|
|
motor3->gotoPosition(h3);
|
|
motor2->disable();
|
|
motor3->disable(); // now bed is even
|
|
Printer::currentPositionSteps[Z_AXIS] = h1 * Printer::axisStepsPerMM[Z_AXIS];
|
|
#if NONLINEAR_SYSTEM
|
|
transformCartesianStepsToDeltaSteps(Printer::currentPositionSteps, Printer::currentNonlinearPositionSteps);
|
|
#endif
|
|
#else
|
|
#error Unknown bed correction method set
|
|
#endif
|
|
}
|
|
|
|
|
|
/**
|
|
Implementation of the G32 command
|
|
G32 S<0..2> - Autolevel print bed. S = 1 measure zLength, S = 2 Measure and store new zLength
|
|
S = 0 : Do not update length - use this if you have not homed before or you mess up zLength!
|
|
S = 1 : Measure zLength so homing works
|
|
S = 2 : Like s = 1 plus store results in EEPROM for next connection.
|
|
*/
|
|
bool runBedLeveling(int s) {
|
|
bool success = true;
|
|
Printer::prepareForProbing();
|
|
#if defined(Z_PROBE_MIN_TEMPERATURE) && Z_PROBE_MIN_TEMPERATURE && Z_PROBE_REQUIRES_HEATING
|
|
float actTemp[NUM_EXTRUDER];
|
|
for(int i = 0; i < NUM_EXTRUDER; i++)
|
|
actTemp[i] = extruder[i].tempControl.targetTemperatureC;
|
|
Printer::moveToReal(IGNORE_COORDINATE, IGNORE_COORDINATE, RMath::max(EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0), static_cast<float>(ZHOME_HEAT_HEIGHT)), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
|
|
Commands::waitUntilEndOfAllMoves();
|
|
#if ZHOME_HEAT_ALL
|
|
for(int i = 0; i < NUM_EXTRUDER; i++) {
|
|
Extruder::setTemperatureForExtruder(RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i, false, false);
|
|
}
|
|
for(int i = 0; i < NUM_EXTRUDER; i++) {
|
|
if(extruder[i].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
|
|
Extruder::setTemperatureForExtruder(RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i, false, true);
|
|
}
|
|
#else
|
|
if(extruder[Extruder::current->id].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
|
|
Extruder::setTemperatureForExtruder(RMath::max(actTemp[Extruder::current->id], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), Extruder::current->id, false, true);
|
|
#endif
|
|
#endif // defined(Z_PROBE_MIN_TEMPERATURE) && Z_PROBE_MIN_TEMPERATURE && Z_PROBE_REQUIRES_HEATING
|
|
|
|
|
|
float h1, h2, h3, hc, oldFeedrate = Printer::feedrate;
|
|
#if DISTORTION_CORRECTION
|
|
bool distEnabled = Printer::distortion.isEnabled();
|
|
Printer::distortion.disable(false); // if level has changed, distortion is also invalid
|
|
#endif
|
|
Printer::setAutolevelActive(false); // iterate
|
|
Printer::resetTransformationMatrix(true); // in case we switch from matrix to motorized!
|
|
#if DRIVE_SYSTEM == DELTA
|
|
// It is not possible to go to the edges at the top, also users try
|
|
// it often and wonder why the coordinate system is then wrong.
|
|
// For that reason we ensure a correct behavior by code.
|
|
Printer::homeAxis(true, true, true);
|
|
Printer::moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
|
|
#else
|
|
if(!Printer::isXHomed() || !Printer::isYHomed())
|
|
Printer::homeAxis(true, true, false);
|
|
Printer::updateCurrentPosition(true);
|
|
Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed());
|
|
#endif
|
|
Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0;
|
|
Printer::startProbing(true);
|
|
//GCode::executeFString(Com::tZProbeStartScript);
|
|
Plane plane;
|
|
#if BED_CORRECTION_METHOD == 1
|
|
success = false;
|
|
for(int r = 0; r < BED_LEVELING_REPETITIONS; r++) {
|
|
#if DRIVE_SYSTEM == DELTA
|
|
if(r > 0) {
|
|
Printer::finishProbing();
|
|
Printer::homeAxis(true, true, true);
|
|
Printer::moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]);
|
|
Printer::startProbing(true);
|
|
}
|
|
#endif // DELTA
|
|
#endif // BED_CORRECTION_METHOD == 1
|
|
if(!measureAutolevelPlane(plane)) {
|
|
Com::printErrorFLN(PSTR("Probing had returned errors - autoleveling canceled."));
|
|
UI_MESSAGE(1);
|
|
return false;
|
|
}
|
|
correctAutolevel(plane);
|
|
|
|
// Leveling is finished now update own positions and store leveling data if needed
|
|
//float currentZ = plane.z((float)Printer::currentPositionSteps[X_AXIS] * Printer::invAxisStepsPerMM[X_AXIS],(float)Printer::currentPositionSteps[Y_AXIS] * Printer::invAxisStepsPerMM[Y_AXIS]);
|
|
float currentZ = plane.z(0.0, 0.0); // we rotated around this point, so that is now z height
|
|
// With max z end stop we adjust z length so after next homing we have also a calibrated printer
|
|
Printer::zMin = 0;
|
|
#if MAX_HARDWARE_ENDSTOP_Z
|
|
//float xRot,yRot,zRot;
|
|
//Printer::transformFromPrinter(Printer::currentPosition[X_AXIS],Printer::currentPosition[Y_AXIS],Printer::currentPosition[Z_AXIS],xRot,yRot,zRot);
|
|
//Com::printFLN(PSTR("Z after rotation:"),zRot);
|
|
// With max z end stop we adjust z length so after next homing we have also a calibrated printer
|
|
if(s != 0) {
|
|
// at origin rotations have no influence so use values there to update
|
|
Printer::zLength += currentZ - Printer::currentPosition[Z_AXIS];
|
|
//Printer::zLength += /*currentZ*/ plane.z((float)Printer::currentPositionSteps[X_AXIS] * Printer::invAxisStepsPerMM[X_AXIS],(float)Printer::currentPositionSteps[Y_AXIS] * Printer::invAxisStepsPerMM[Y_AXIS]) - zRot;
|
|
Com::printFLN(Com::tZProbePrinterHeight, Printer::zLength);
|
|
}
|
|
#endif
|
|
#if Z_PROBE_Z_OFFSET_MODE == 1
|
|
currentZ -= EEPROM::zProbeZOffset();
|
|
#endif
|
|
Com::printF(PSTR("CurrentZ:"), currentZ);
|
|
Com::printFLN(PSTR(" atZ:"), Printer::currentPosition[Z_AXIS]);
|
|
Printer::currentPositionSteps[Z_AXIS] = currentZ * Printer::axisStepsPerMM[Z_AXIS];
|
|
Printer::updateCurrentPosition(true); // set position based on steps position
|
|
#if BED_CORRECTION_METHOD == 1
|
|
if(fabsf(plane.a) < 0.00025 && fabsf(plane.b) < 0.00025 ) {
|
|
success = true;
|
|
break; // we reached achievable precision so we can stop
|
|
}
|
|
} // for BED_LEVELING_REPETITIONS
|
|
#if Z_HOME_DIR > 0 && MAX_HARDWARE_ENDSTOP_Z
|
|
float zall = Printer::runZProbe(false, false, 1, false);
|
|
if(zall == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
Printer::currentPosition[Z_AXIS] = zall;
|
|
Printer::currentPositionSteps[Z_AXIS] = zall * Printer::axisStepsPerMM[Z_AXIS];
|
|
#if NONLINEAR_SYSTEM
|
|
transformCartesianStepsToDeltaSteps(Printer::currentPositionSteps, Printer::currentNonlinearPositionSteps);
|
|
#endif
|
|
if(s >= 1) {
|
|
float zMax = Printer::runZMaxProbe();
|
|
if(zMax == ILLEGAL_Z_PROBE)
|
|
return false;
|
|
zall += zMax - ENDSTOP_Z_BACK_ON_HOME;
|
|
Printer::zLength = zall;
|
|
}
|
|
#endif
|
|
#endif // BED_CORRECTION_METHOD == 1
|
|
Printer::updateDerivedParameter();
|
|
Printer::finishProbing();
|
|
#if BED_CORRECTION_METHOD != 1
|
|
Printer::setAutolevelActive(true); // only for software correction or we can spare the comp. time
|
|
#endif
|
|
if(s >= 2) {
|
|
EEPROM::storeDataIntoEEPROM();
|
|
}
|
|
Printer::updateCurrentPosition(true);
|
|
Commands::printCurrentPosition();
|
|
#if DISTORTION_CORRECTION
|
|
if(distEnabled)
|
|
Printer::distortion.enable(false); // if level has changed, distortion is also invalid
|
|
#endif
|
|
#if DRIVE_SYSTEM == DELTA
|
|
Printer::homeAxis(true, true, true); // shifting z makes positioning invalid, need to recalibrate
|
|
#endif
|
|
Printer::feedrate = oldFeedrate;
|
|
|
|
#if defined(Z_PROBE_MIN_TEMPERATURE) && Z_PROBE_MIN_TEMPERATURE && Z_PROBE_REQUIRES_HEATING
|
|
#if ZHOME_HEAT_ALL
|
|
for(int i = 0; i < NUM_EXTRUDER; i++) {
|
|
Extruder::setTemperatureForExtruder(RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i, false, false);
|
|
}
|
|
for(int i = 0; i < NUM_EXTRUDER; i++) {
|
|
if(extruder[i].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
|
|
Extruder::setTemperatureForExtruder(RMath::max(actTemp[i], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), i, false, true);
|
|
}
|
|
#else
|
|
if(extruder[Extruder::current->id].tempControl.currentTemperatureC < ZPROBE_MIN_TEMPERATURE)
|
|
Extruder::setTemperatureForExtruder(RMath::max(actTemp[Extruder::current->id], static_cast<float>(ZPROBE_MIN_TEMPERATURE)), Extruder::current->id, false, true);
|
|
#endif
|
|
#endif
|
|
|
|
return success;
|
|
}
|
|
|
|
#endif
|
|
|
|
/** \brief Activate or deactivate rotation correction.
|
|
|
|
\param on True if Rotation correction should be enabled.
|
|
*/
|
|
void Printer::setAutolevelActive(bool on) {
|
|
#if FEATURE_AUTOLEVEL
|
|
if(on == isAutolevelActive()) return;
|
|
flag0 = (on ? flag0 | PRINTER_FLAG0_AUTOLEVEL_ACTIVE : flag0 & ~PRINTER_FLAG0_AUTOLEVEL_ACTIVE);
|
|
if(on)
|
|
Com::printInfoFLN(Com::tAutolevelEnabled);
|
|
else
|
|
Com::printInfoFLN(Com::tAutolevelDisabled);
|
|
updateCurrentPosition(false);
|
|
#endif // FEATURE_AUTOLEVEL
|
|
}
|
|
#if MAX_HARDWARE_ENDSTOP_Z
|
|
/** \brief Measure distance from current position until triggering z max endstop.
|
|
|
|
\return Distance until triggering in mm. */
|
|
float Printer::runZMaxProbe() {
|
|
#if NONLINEAR_SYSTEM
|
|
long startZ = realDeltaPositionSteps[Z_AXIS] = currentNonlinearPositionSteps[Z_AXIS]; // update real
|
|
#endif
|
|
Commands::waitUntilEndOfAllMoves();
|
|
long probeDepth = 2 * (Printer::zMaxSteps - Printer::zMinSteps);
|
|
stepsRemainingAtZHit = -1;
|
|
setZProbingActive(true);
|
|
PrintLine::moveRelativeDistanceInSteps(0, 0, probeDepth, 0, homingFeedrate[Z_AXIS] / ENDSTOP_Z_RETEST_REDUCTION_FACTOR, true, true);
|
|
if(stepsRemainingAtZHit < 0) {
|
|
Com::printErrorFLN(PSTR("z-max homing failed"));
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
setZProbingActive(false);
|
|
currentPositionSteps[Z_AXIS] -= stepsRemainingAtZHit;
|
|
#if NONLINEAR_SYSTEM
|
|
transformCartesianStepsToDeltaSteps(Printer::currentPositionSteps, Printer::currentNonlinearPositionSteps);
|
|
probeDepth = (realDeltaPositionSteps[Z_AXIS] - startZ);
|
|
#else
|
|
probeDepth -= stepsRemainingAtZHit;
|
|
#endif
|
|
float distance = (float)probeDepth * invAxisStepsPerMM[Z_AXIS];
|
|
Com::printF(Com::tZProbeMax, distance);
|
|
Com::printF(Com::tSpaceXColon, realXPosition());
|
|
Com::printFLN(Com::tSpaceYColon, realYPosition());
|
|
PrintLine::moveRelativeDistanceInSteps(0, 0, -probeDepth, 0, homingFeedrate[Z_AXIS], true, true);
|
|
return distance;
|
|
}
|
|
#endif
|
|
|
|
#if FEATURE_Z_PROBE
|
|
/** \brief Activate z-probe
|
|
|
|
Tests if switching from active tool to z-probe is possible at current position. If not the operation is aborted.
|
|
If ok, it runs start script, checks z position and applies the z-probe offset.
|
|
|
|
\param runScript Run start z-probe script from configuration.
|
|
\param enforceStartHeight If true moves z to EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0) + 0.1 if current position is higher.
|
|
\return True if activation was successful. */
|
|
bool Printer::startProbing(bool runScript, bool enforceStartHeight) {
|
|
float cx, cy, cz;
|
|
realPosition(cx, cy, cz);
|
|
// Fix position to be inside print area when probe is enabled
|
|
#if EXTRUDER_IS_Z_PROBE == 0
|
|
float ZPOffsetX = EEPROM::zProbeXOffset();
|
|
float ZPOffsetY = EEPROM::zProbeYOffset();
|
|
#if DRIVE_SYSTEM == DELTA
|
|
float rad = EEPROM::deltaMaxRadius();
|
|
float dx = Printer::currentPosition[X_AXIS] - ZPOffsetX;
|
|
float dy = Printer::currentPosition[Y_AXIS] - ZPOffsetY;
|
|
if(sqrt(dx * dx + dy * dy) > rad)
|
|
#else
|
|
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))
|
|
#endif
|
|
{
|
|
Com::printErrorF(PSTR("Activating z-probe would lead to forbidden xy position: "));
|
|
Com::print(Printer::currentPosition[X_AXIS] - ZPOffsetX);
|
|
Com::printFLN(PSTR(", "), Printer::currentPosition[Y_AXIS] - ZPOffsetY);
|
|
GCode::fatalError(PSTR("Could not activate z-probe offset due to coordinate constraints - result is inaccurate!"));
|
|
return false;
|
|
} else {
|
|
if(runScript) {
|
|
GCode::executeFString(Com::tZProbeStartScript);
|
|
}
|
|
float maxStartHeight = EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0) + 0.1;
|
|
if(currentPosition[Z_AXIS] > maxStartHeight && enforceStartHeight) {
|
|
cz = maxStartHeight;
|
|
moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, maxStartHeight, IGNORE_COORDINATE, homingFeedrate[Z_AXIS]);
|
|
}
|
|
|
|
// Update position
|
|
Printer::offsetX = -ZPOffsetX;
|
|
Printer::offsetY = -ZPOffsetY;
|
|
Printer::offsetZ = 0;
|
|
#if FEATURE_AUTOLEVEL
|
|
// we must not change z for the probe offset even if we are rotated, so add a correction for z
|
|
float dx, dy;
|
|
transformToPrinter(EEPROM::zProbeXOffset(), EEPROM::zProbeYOffset(), 0, dx, dy, offsetZ2);
|
|
//Com::printFLN(PSTR("ZPOffset2:"),offsetZ2,3);
|
|
#endif
|
|
}
|
|
#else
|
|
if(runScript) {
|
|
GCode::executeFString(Com::tZProbeStartScript);
|
|
}
|
|
#endif
|
|
Printer::moveToReal(cx, cy, cz, IGNORE_COORDINATE, EXTRUDER_SWITCH_XY_SPEED);
|
|
updateCurrentPosition(false);
|
|
return true;
|
|
}
|
|
|
|
/** \brief Deactivate z-probe. */
|
|
void Printer::finishProbing() {
|
|
float cx, cy, cz;
|
|
realPosition(cx, cy, cz);
|
|
GCode::executeFString(Com::tZProbeEndScript);
|
|
if(Extruder::current) {
|
|
#if DUAL_X_AXIS
|
|
offsetX = 0; // offsets are parking positions for dual x axis!
|
|
#else
|
|
offsetX = -Extruder::current->xOffset * invAxisStepsPerMM[X_AXIS];
|
|
#endif
|
|
offsetY = -Extruder::current->yOffset * invAxisStepsPerMM[Y_AXIS];
|
|
offsetZ = -Extruder::current->zOffset * invAxisStepsPerMM[Z_AXIS];
|
|
} else {
|
|
offsetX = offsetY = offsetZ = 0;
|
|
}
|
|
offsetZ2 = 0;
|
|
Printer::moveToReal(cx, cy, cz, IGNORE_COORDINATE, EXTRUDER_SWITCH_XY_SPEED);
|
|
}
|
|
|
|
/** \brief Measure distance to bottom at current position.
|
|
|
|
This is the most important function for bed leveling. It does
|
|
1. Run probe start script if first = true and runStartScript = true
|
|
2. Position zProbe at current position if first = true. If we are more then maxStartHeight away from bed we also go down to that distance.
|
|
3. Measure the the steps until probe hits the bed.
|
|
4. Undo positioning to z probe and run finish script if last = true.
|
|
|
|
Now we compute the nozzle height as follows:
|
|
a) Compute average height from repeated measurements
|
|
b) Add zProbeHeight to correct difference between triggering point and nozzle height above bed
|
|
c) If Z_PROBE_Z_OFFSET_MODE == 1 we add zProbeZOffset() that is coating thickness if we measure below coating with indictive sensor.
|
|
d) Add distortion correction.
|
|
e) Add bending correction
|
|
|
|
Then we return the measured and corrected z distance.
|
|
|
|
\param first If true, Printer::startProbing is called.
|
|
\param last If true, Printer::finishProbing is called at the end.
|
|
\param repeat Number of repetitions to average measurement errors.
|
|
\param runStartScript If true tells startProbing to run start script.
|
|
\param enforceStartHeight Tells start script to enforce a maximum distance to bed.
|
|
\return ILLEGAL_Z_PROBE on errors or measured distance.
|
|
*/
|
|
float Printer::runZProbe(bool first, bool last, uint8_t repeat, bool runStartScript, bool enforceStartHeight) {
|
|
float oldOffX = Printer::offsetX;
|
|
float oldOffY = Printer::offsetY;
|
|
float oldOffZ = Printer::offsetZ;
|
|
if(first) {
|
|
if(!startProbing(runStartScript, enforceStartHeight))
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
Commands::waitUntilEndOfAllMoves();
|
|
#if defined(Z_PROBE_USE_MEDIAN) && Z_PROBE_USE_MEDIAN
|
|
int32_t measurements[Z_PROBE_REPETITIONS];
|
|
repeat = RMath::min(repeat, Z_PROBE_REPETITIONS);
|
|
#else
|
|
int32_t sum = 0;
|
|
#endif
|
|
int32_t probeDepth;
|
|
int32_t shortMove = static_cast<int32_t>((float)Z_PROBE_SWITCHING_DISTANCE * axisStepsPerMM[Z_AXIS]); // distance to go up for repeated moves
|
|
int32_t lastCorrection = currentPositionSteps[Z_AXIS]; // starting position
|
|
#if NONLINEAR_SYSTEM
|
|
realDeltaPositionSteps[Z_AXIS] = currentNonlinearPositionSteps[Z_AXIS]; // update real
|
|
#endif
|
|
//int32_t updateZ = 0;
|
|
waitForZProbeStart();
|
|
#if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
|
|
HAL::delayMilliseconds(Z_PROBE_DELAY);
|
|
#endif
|
|
Endstops::update();
|
|
Endstops::update(); // need to call twice for full update!
|
|
if(Endstops::zProbe()) {
|
|
Com::printErrorFLN(PSTR("z-probe triggered before starting probing."));
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
#if Z_PROBE_DISABLE_HEATERS
|
|
Extruder::pauseExtruders(true);
|
|
HAL::delayMilliseconds(70);
|
|
#endif
|
|
for(int8_t r = 0; r < repeat; r++) {
|
|
probeDepth = 2 * (Printer::zMaxSteps - Printer::zMinSteps); // probe should always hit within this distance
|
|
stepsRemainingAtZHit = -1; // Marker that we did not hit z probe
|
|
setZProbingActive(true);
|
|
#if defined(Z_PROBE_DELAY) && Z_PROBE_DELAY > 0
|
|
HAL::delayMilliseconds(Z_PROBE_DELAY);
|
|
#endif
|
|
PrintLine::moveRelativeDistanceInSteps(0, 0, -probeDepth, 0, EEPROM::zProbeSpeed(), true, true);
|
|
setZProbingActive(false);
|
|
if(stepsRemainingAtZHit < 0) {
|
|
Com::printErrorFLN(Com::tZProbeFailed);
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
#if NONLINEAR_SYSTEM
|
|
stepsRemainingAtZHit = realDeltaPositionSteps[C_TOWER] - currentNonlinearPositionSteps[C_TOWER]; // nonlinear moves may split z so stepsRemainingAtZHit is only what is left from last segment not total move. This corrects the problem.
|
|
#endif
|
|
#if DRIVE_SYSTEM == DELTA
|
|
currentNonlinearPositionSteps[A_TOWER] += stepsRemainingAtZHit; // Update difference
|
|
currentNonlinearPositionSteps[B_TOWER] += stepsRemainingAtZHit;
|
|
currentNonlinearPositionSteps[C_TOWER] += stepsRemainingAtZHit;
|
|
#elif NONLINEAR_SYSTEM
|
|
currentNonlinearPositionSteps[Z_AXIS] += stepsRemainingAtZHit;
|
|
#endif
|
|
currentPositionSteps[Z_AXIS] += stepsRemainingAtZHit; // now current position is correct
|
|
#if defined(Z_PROBE_USE_MEDIAN) && Z_PROBE_USE_MEDIAN
|
|
measurements[r] = lastCorrection - currentPositionSteps[Z_AXIS];
|
|
#else
|
|
sum += lastCorrection - currentPositionSteps[Z_AXIS];
|
|
#endif
|
|
//Com::printFLN(PSTR("ZHSteps:"),lastCorrection - currentPositionSteps[Z_AXIS]);
|
|
if(r + 1 < repeat) {
|
|
// go only shortest possible move up for repetitions
|
|
PrintLine::moveRelativeDistanceInSteps(0, 0, shortMove, 0, HOMING_FEEDRATE_Z, true, true);
|
|
if(Endstops::zProbe()) {
|
|
Com::printErrorFLN(PSTR("z-probe did not untrigger on repetitive measurement - maybe you need to increase distance!"));
|
|
UI_MESSAGE(1);
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
}
|
|
#ifdef Z_PROBE_RUN_AFTER_EVERY_PROBE
|
|
GCode::executeFString(PSTR(Z_PROBE_RUN_AFTER_EVERY_PROBE));
|
|
#endif
|
|
}
|
|
#if Z_PROBE_DISABLE_HEATERS
|
|
Extruder::unpauseExtruders(false);
|
|
#endif
|
|
|
|
// Go back to start position
|
|
PrintLine::moveRelativeDistanceInSteps(0, 0, lastCorrection - currentPositionSteps[Z_AXIS], 0, HOMING_FEEDRATE_Z, true, true);
|
|
if(Endstops::zProbe()) { // did we untrigger? If not don't trust result!
|
|
Com::printErrorFLN(PSTR("z-probe did not untrigger on repetitive measurement - maybe you need to increase distance!"));
|
|
UI_MESSAGE(1);
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
updateCurrentPosition(false);
|
|
//Com::printFLN(PSTR("after probe"));
|
|
//Commands::printCurrentPosition();
|
|
#if defined(Z_PROBE_USE_MEDIAN) && Z_PROBE_USE_MEDIAN
|
|
// bubble sort the measurements
|
|
int32_t tmp;
|
|
for(fast8_t i = 0 ; i < repeat - 1; i++) { // n numbers require at most n-1 rounds of swapping
|
|
for(fast8_t j = 0; j < repeat - i - 1; j++) { //
|
|
if( measurements[j] > measurements[j + 1] ) { // out of order?
|
|
tmp = measurements[j]; // swap them:
|
|
measurements[j] = measurements[j + 1];
|
|
measurements[j + 1] = tmp;
|
|
}
|
|
}
|
|
}
|
|
// process result
|
|
float distance = static_cast<float>(measurements[repeat >> 1]) * invAxisStepsPerMM[Z_AXIS] + EEPROM::zProbeHeight();
|
|
#else
|
|
float distance = static_cast<float>(sum) * invAxisStepsPerMM[Z_AXIS] / static_cast<float>(repeat) + EEPROM::zProbeHeight();
|
|
#endif
|
|
#if FEATURE_AUTOLEVEL
|
|
// we must change z for the z change from moving in rotated coordinates away from real position
|
|
float dx, dy, dz;
|
|
transformToPrinter(0, 0, currentPosition[Z_AXIS], dx, dy, dz); // what is our x,y offset from z position
|
|
dz -= currentPosition[Z_AXIS];
|
|
//Com::printF(PSTR("ZXO:"),dx,3);Com::printF(PSTR(" ZYO:"),dy,3);
|
|
//transformToPrinter(dx,dy,0,dx,dy,dz); // how much changes z from x,y offset?
|
|
//Com::printFLN(PSTR(" Z from xy off:"), dz,7);
|
|
distance += dz;
|
|
#endif
|
|
//Com::printFLN(PSTR("OrigDistance:"),distance);
|
|
#if Z_PROBE_Z_OFFSET_MODE == 1
|
|
distance += EEPROM::zProbeZOffset(); // We measured including coating, so we need to add coating thickness!
|
|
#endif
|
|
|
|
#if DISTORTION_CORRECTION
|
|
float zCorr = 0;
|
|
if(Printer::distortion.isEnabled()) {
|
|
zCorr = distortion.correct(currentPositionSteps[X_AXIS]/* + EEPROM::zProbeXOffset() * axisStepsPerMM[X_AXIS]*/, currentPositionSteps[Y_AXIS]
|
|
/* + EEPROM::zProbeYOffset() * axisStepsPerMM[Y_AXIS]*/, zMinSteps) * invAxisStepsPerMM[Z_AXIS];
|
|
distance += zCorr;
|
|
}
|
|
#endif
|
|
|
|
distance += bendingCorrectionAt(currentPosition[X_AXIS], currentPosition[Y_AXIS]);
|
|
|
|
Com::printF(Com::tZProbe, distance, 3);
|
|
Com::printF(Com::tSpaceXColon, realXPosition());
|
|
#if DISTORTION_CORRECTION
|
|
if(Printer::distortion.isEnabled()) {
|
|
Com::printF(Com::tSpaceYColon, realYPosition());
|
|
Com::printFLN(PSTR(" zCorr:"), zCorr, 3);
|
|
} else {
|
|
Com::printFLN(Com::tSpaceYColon, realYPosition());
|
|
}
|
|
#else
|
|
Com::printFLN(Com::tSpaceYColon, realYPosition());
|
|
#endif
|
|
if(Endstops::zProbe()) {
|
|
Com::printErrorFLN(PSTR("z-probe did not untrigger after going back to start position."));
|
|
UI_MESSAGE(1);
|
|
return ILLEGAL_Z_PROBE;
|
|
}
|
|
if(last)
|
|
finishProbing();
|
|
return distance;
|
|
}
|
|
|
|
/**
|
|
* Having printer's height set properly (i.e. after calibration of Z=0), one can use this procedure to measure Z-probe height.
|
|
* It deploys the sensor, takes several probes at center, then updates Z-probe height with average.
|
|
*/
|
|
void Printer::measureZProbeHeight(float curHeight) {
|
|
#if FEATURE_Z_PROBE
|
|
currentPositionSteps[Z_AXIS] = curHeight * axisStepsPerMM[Z_AXIS];
|
|
updateCurrentPosition(true);
|
|
#if NONLINEAR_SYSTEM
|
|
transformCartesianStepsToDeltaSteps(currentPositionSteps, currentNonlinearPositionSteps);
|
|
#endif
|
|
float startHeight = EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0);
|
|
moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, startHeight, IGNORE_COORDINATE, homingFeedrate[Z_AXIS]);
|
|
float zheight = Printer::runZProbe(true, true, Z_PROBE_REPETITIONS, true);
|
|
if(zheight == ILLEGAL_Z_PROBE) {
|
|
return;
|
|
}
|
|
float zProbeHeight = EEPROM::zProbeHeight() + startHeight -zheight;
|
|
|
|
#if EEPROM_MODE != 0 // Com::tZProbeHeight is not declared when EEPROM_MODE is 0
|
|
EEPROM::setZProbeHeight(zProbeHeight); // will also report on output
|
|
#else
|
|
Com::printFLN(PSTR("Z-probe height [mm]:"), zProbeHeight);
|
|
#endif
|
|
#endif
|
|
}
|
|
|
|
float Printer::bendingCorrectionAt(float x, float y) {
|
|
PlaneBuilder builder;
|
|
builder.addPoint(EEPROM::zProbeX1(), EEPROM::zProbeY1(), EEPROM::bendingCorrectionA());
|
|
builder.addPoint(EEPROM::zProbeX2(), EEPROM::zProbeY2(), EEPROM::bendingCorrectionB());
|
|
builder.addPoint(EEPROM::zProbeX3(), EEPROM::zProbeY3(), EEPROM::bendingCorrectionC());
|
|
Plane plane;
|
|
builder.createPlane(plane, true);
|
|
return plane.z(x, y);
|
|
}
|
|
|
|
void Printer::waitForZProbeStart() {
|
|
#if Z_PROBE_WAIT_BEFORE_TEST
|
|
Endstops::update();
|
|
Endstops::update(); // double test to get right signal. Needed for crosstalk protection.
|
|
if(Endstops::zProbe()) return;
|
|
#if UI_DISPLAY_TYPE != NO_DISPLAY
|
|
uid.setStatusP(Com::tHitZProbe);
|
|
uid.refreshPage();
|
|
#endif
|
|
#ifdef DEBUG_PRINT
|
|
debugWaitLoop = 3;
|
|
#endif
|
|
while(!Endstops::zProbe()) {
|
|
defaultLoopActions();
|
|
Endstops::update();
|
|
Endstops::update(); // double test to get right signal. Needed for crosstalk protection.
|
|
}
|
|
#ifdef DEBUG_PRINT
|
|
debugWaitLoop = 4;
|
|
#endif
|
|
HAL::delayMilliseconds(30);
|
|
while(Endstops::zProbe()) {
|
|
defaultLoopActions();
|
|
Endstops::update();
|
|
Endstops::update(); // double test to get right signal. Needed for crosstalk protection.
|
|
}
|
|
HAL::delayMilliseconds(30);
|
|
UI_CLEAR_STATUS;
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
/*
|
|
Transforms theoretical correct coordinates to corrected coordinates resulting from bed rotation
|
|
and shear transformations.
|
|
|
|
We have 2 coordinate systems. The printer step position where we want to be. These are the positions
|
|
we send to printers, the theoretical coordinates. In contrast we have the printer coordinates that
|
|
we need to be at to get the desired result, the real coordinates.
|
|
*/
|
|
void Printer::transformToPrinter(float x, float y, float z, float &transX, float &transY, float &transZ) {
|
|
#if FEATURE_AXISCOMP
|
|
// Axis compensation:
|
|
x = x + y * EEPROM::axisCompTanXY() + z * EEPROM::axisCompTanXZ();
|
|
y = y + z * EEPROM::axisCompTanYZ();
|
|
#endif
|
|
#if BED_CORRECTION_METHOD != 1 && FEATURE_AUTOLEVEL
|
|
if(isAutolevelActive()) {
|
|
transX = x * autolevelTransformation[0] + y * autolevelTransformation[3] + z * autolevelTransformation[6];
|
|
transY = x * autolevelTransformation[1] + y * autolevelTransformation[4] + z * autolevelTransformation[7];
|
|
transZ = x * autolevelTransformation[2] + y * autolevelTransformation[5] + z * autolevelTransformation[8];
|
|
} else {
|
|
transX = x;
|
|
transY = y;
|
|
transZ = z;
|
|
}
|
|
#else
|
|
transX = x;
|
|
transY = y;
|
|
transZ = z;
|
|
#endif
|
|
}
|
|
|
|
/* Transform back to real printer coordinates. */
|
|
void Printer::transformFromPrinter(float x, float y, float z, float &transX, float &transY, float &transZ) {
|
|
#if BED_CORRECTION_METHOD != 1 && FEATURE_AUTOLEVEL
|
|
if(isAutolevelActive()) {
|
|
transX = x * autolevelTransformation[0] + y * autolevelTransformation[1] + z * autolevelTransformation[2];
|
|
transY = x * autolevelTransformation[3] + y * autolevelTransformation[4] + z * autolevelTransformation[5];
|
|
transZ = x * autolevelTransformation[6] + y * autolevelTransformation[7] + z * autolevelTransformation[8];
|
|
} else {
|
|
transX = x;
|
|
transY = y;
|
|
transZ = z;
|
|
}
|
|
#else
|
|
transX = x;
|
|
transY = y;
|
|
transZ = z;
|
|
#endif
|
|
#if FEATURE_AXISCOMP
|
|
// Axis compensation:
|
|
transY = transY - transZ * EEPROM::axisCompTanYZ();
|
|
transX = transX - transY * EEPROM::axisCompTanXY() - transZ * EEPROM::axisCompTanXZ();
|
|
#endif
|
|
}
|
|
#if FEATURE_AUTOLEVEL
|
|
void Printer::resetTransformationMatrix(bool silent) {
|
|
autolevelTransformation[0] = autolevelTransformation[4] = autolevelTransformation[8] = 1;
|
|
autolevelTransformation[1] = autolevelTransformation[2] = autolevelTransformation[3] =
|
|
autolevelTransformation[5] = autolevelTransformation[6] = autolevelTransformation[7] = 0;
|
|
if(!silent)
|
|
Com::printInfoFLN(Com::tAutolevelReset);
|
|
}
|
|
|
|
void Printer::buildTransformationMatrix(Plane &plane) {
|
|
float z0 = plane.z(0, 0);
|
|
float az = z0 - plane.z(1, 0); // ax = 1, ay = 0
|
|
float bz = z0 - plane.z(0, 1); // bx = 0, by = 1
|
|
// First z direction
|
|
autolevelTransformation[6] = -az;
|
|
autolevelTransformation[7] = -bz;
|
|
autolevelTransformation[8] = 1;
|
|
float len = sqrt(az * az + bz * bz + 1);
|
|
autolevelTransformation[6] /= len;
|
|
autolevelTransformation[7] /= len;
|
|
autolevelTransformation[8] /= len;
|
|
autolevelTransformation[0] = 1;
|
|
autolevelTransformation[1] = 0;
|
|
autolevelTransformation[2] = -autolevelTransformation[6] / autolevelTransformation[8];
|
|
len = sqrt(autolevelTransformation[0] * autolevelTransformation[0] + autolevelTransformation[1] * autolevelTransformation[1] + autolevelTransformation[2] * autolevelTransformation[2]);
|
|
autolevelTransformation[0] /= len;
|
|
autolevelTransformation[1] /= len;
|
|
autolevelTransformation[2] /= len;
|
|
// cross(z,x) y,z)
|
|
autolevelTransformation[3] = autolevelTransformation[7] * autolevelTransformation[2] - autolevelTransformation[8] * autolevelTransformation[1];
|
|
autolevelTransformation[4] = autolevelTransformation[8] * autolevelTransformation[0] - autolevelTransformation[6] * autolevelTransformation[2];
|
|
autolevelTransformation[5] = autolevelTransformation[6] * autolevelTransformation[1] - autolevelTransformation[7] * autolevelTransformation[0];
|
|
len = sqrt(autolevelTransformation[3] * autolevelTransformation[3] + autolevelTransformation[4] * autolevelTransformation[4] + autolevelTransformation[5] * autolevelTransformation[5]);
|
|
autolevelTransformation[3] /= len;
|
|
autolevelTransformation[4] /= len;
|
|
autolevelTransformation[5] /= len;
|
|
|
|
Com::printArrayFLN(Com::tTransformationMatrix, autolevelTransformation, 9, 6);
|
|
}
|
|
/*
|
|
void Printer::buildTransformationMatrix(float h1,float h2,float h3) {
|
|
float ax = EEPROM::zProbeX2() - EEPROM::zProbeX1();
|
|
float ay = EEPROM::zProbeY2() - EEPROM::zProbeY1();
|
|
float az = h1 - h2;
|
|
float bx = EEPROM::zProbeX3() - EEPROM::zProbeX1();
|
|
float by = EEPROM::zProbeY3() - EEPROM::zProbeY1();
|
|
float bz = h1 - h3;
|
|
// First z direction
|
|
autolevelTransformation[6] = ay * bz - az * by;
|
|
autolevelTransformation[7] = az * bx - ax * bz;
|
|
autolevelTransformation[8] = ax * by - ay * bx;
|
|
float len = sqrt(autolevelTransformation[6] * autolevelTransformation[6] + autolevelTransformation[7] * autolevelTransformation[7] + autolevelTransformation[8] * autolevelTransformation[8]);
|
|
if(autolevelTransformation[8] < 0) len = -len;
|
|
autolevelTransformation[6] /= len;
|
|
autolevelTransformation[7] /= len;
|
|
autolevelTransformation[8] /= len;
|
|
autolevelTransformation[3] = 0;
|
|
autolevelTransformation[4] = autolevelTransformation[8];
|
|
autolevelTransformation[5] = -autolevelTransformation[7];
|
|
// cross(y,z)
|
|
autolevelTransformation[0] = autolevelTransformation[4] * autolevelTransformation[8] - autolevelTransformation[5] * autolevelTransformation[7];
|
|
autolevelTransformation[1] = autolevelTransformation[5] * autolevelTransformation[6];// - autolevelTransformation[3] * autolevelTransformation[8];
|
|
autolevelTransformation[2] = autolevelTransformation[3] * autolevelTransformation[7] - autolevelTransformation[4] * autolevelTransformation[6];
|
|
len = sqrt(autolevelTransformation[0] * autolevelTransformation[0] + autolevelTransformation[1] * autolevelTransformation[1] + autolevelTransformation[2] * autolevelTransformation[2]);
|
|
autolevelTransformation[0] /= len;
|
|
autolevelTransformation[1] /= len;
|
|
autolevelTransformation[2] /= len;
|
|
len = sqrt(autolevelTransformation[4] * autolevelTransformation[4] + autolevelTransformation[5] * autolevelTransformation[5]);
|
|
autolevelTransformation[4] /= len;
|
|
autolevelTransformation[5] /= len;
|
|
Com::printArrayFLN(Com::tTransformationMatrix,autolevelTransformation, 9, 6);
|
|
}
|
|
*/
|
|
#endif
|