diff --git a/Repetier/BedLeveling.cpp b/Repetier/BedLeveling.cpp index 5925f79..737e231 100644 --- a/Repetier/BedLeveling.cpp +++ b/Repetier/BedLeveling.cpp @@ -1,430 +1,442 @@ - -/* -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. - -*/ - -#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_AUTOLEVEL && FEATURE_Z_PROBE - -class Plane { - public: - // f(x, y) = ax + by + c - float a,b,c; - float z(float x,float y) { - return a * x + y * b + c; - } -}; -class PlaneBuilder { - float sum_xx,sum_xy,sum_yy,sum_x,sum_y,sum_xz,sum_yz,sum_z,n; - public: - void reset() { - sum_xx = sum_xy = sum_yy = sum_x = sum_y = sum_xz = sum_yz = sum_z = n = 0; - } - void addPoint(float x,float y,float z) { - n++; - sum_xx += x * x; - sum_xy += x * y; - sum_yy += y * y; - sum_x += x; - sum_y += y; - sum_xz += x * z; - sum_yz += y * z; - sum_z += z; - } - void createPlane(Plane &plane) { - float det = (sum_x*(sum_xy*sum_y-sum_x*sum_yy)+sum_xx*(n*sum_yy-sum_y*sum_y)+sum_xy*(sum_x*sum_y-n*sum_xy)); - plane.a = ((sum_xy*sum_y-sum_x*sum_yy)*sum_z+(sum_x*sum_y-n*sum_xy)*sum_yz+sum_xz*(n*sum_yy-sum_y*sum_y)) /det; - plane.b = ((sum_x*sum_xy-sum_xx*sum_y)*sum_z+(n*sum_xx-sum_x*sum_x)*sum_yz+sum_xz*(sum_x*sum_y-n*sum_xy)) /det; - plane.c = ((sum_xx*sum_yy-sum_xy*sum_xy)*sum_z+(sum_x*sum_xy-sum_xx*sum_y)*sum_yz+sum_xz*(sum_xy*sum_y-sum_x*sum_yy))/det; - Com::printF(PSTR("plane: a = "),plane.a,4); - Com::printF(PSTR(" b = "),plane.b,4); - Com::printFLN(PSTR(" c = "),plane.c,4); - } -}; - -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 < -1) - 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 < -1) - 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 < -1) - 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(ix) * ax + static_cast(iy) * bx; - float py = oy + static_cast(ix) * ay + static_cast(iy) * by; - Printer::moveTo(px,py,IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - float h = Printer::runZProbe(false,false); - if(h < -1) - 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 < -1) - return false; - Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h2 = Printer::runZProbe(false,false); - if(h2 < -1) - return false; - Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h3 = Printer::runZProbe(false,false); - if(h3 < -1) - return false; - Printer::moveTo(x1Mirror,y1Mirror,IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h4 = Printer::runZProbe(false,false); - if(h4 < -1) - 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); - return true; -} - -void correctAutolevel(GCode *code,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())); - #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; - 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]; - #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. -*/ -void runBedLeveling(GCode *com) { - float h1,h2,h3,hc,oldFeedrate = Printer::feedrate; - int s = com->hasS() ? com->S : -1; - #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(), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]); - #endif - Printer::startProbing(true); - //GCode::executeFString(Com::tZProbeStartScript); - Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0; - Plane plane; -#if BED_CORRECTION_METHOD == 1 - for(int r = 0; r < BED_LEVELING_REPETITIONS; r++) { + +/* +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 + + +class PlaneBuilder { + float sum_xx,sum_xy,sum_yy,sum_x,sum_y,sum_xz,sum_yz,sum_z,n; + public: + PlaneBuilder() { + reset(); + } + void reset() { + sum_xx = sum_xy = sum_yy = sum_x = sum_y = sum_xz = sum_yz = sum_z = n = 0; + } + void addPoint(float x,float y,float z) { + n++; + sum_xx += x * x; + sum_xy += x * y; + sum_yy += y * y; + sum_x += x; + sum_y += y; + sum_xz += x * z; + sum_yz += y * z; + sum_z += z; + } + void createPlane(Plane &plane,bool silent=false) { + float det = (sum_x * (sum_xy * sum_y - sum_x * sum_yy) + sum_xx * (n * sum_yy - sum_y * sum_y) + sum_xy * (sum_x * sum_y - n * sum_xy)); + plane.a = ((sum_xy * sum_y - sum_x * sum_yy) * sum_z + (sum_x * sum_y - n * sum_xy) * sum_yz + sum_xz * (n * sum_yy - sum_y * sum_y)) / det; + plane.b = ((sum_x * sum_xy - sum_xx * sum_y) * sum_z + (n * sum_xx - sum_x * sum_x) * sum_yz + sum_xz * (sum_x * sum_y - n * sum_xy)) / det; + plane.c = ((sum_xx * sum_yy - sum_xy * sum_xy) * sum_z + (sum_x * sum_xy - sum_xx * sum_y) * sum_yz + sum_xz * (sum_xy * sum_y - sum_x * sum_yy)) / det; + if(!silent) { + Com::printF(PSTR("plane: a = "),plane.a,4); + Com::printF(PSTR(" b = "),plane.b,4); + Com::printFLN(PSTR(" c = "),plane.c,4); + } + } +}; + +#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(ix) * ax + static_cast(iy) * bx; + float py = oy + static_cast(ix) * ay + static_cast(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(GCode *code,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]; +#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(GCode *com) { + float h1,h2,h3,hc,oldFeedrate = Printer::feedrate; + int s = com->hasS() ? com->S : -1; +#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 - if(r > 0) { - Printer::finishProbing(); - Printer::homeAxis(true, true, true); - Printer::moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeBedDistance() + EEPROM::zProbeHeight(), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]); - Printer::startProbing(true); - } -#endif -#endif - if(!measureAutolevelPlane(plane)) { - Com::printErrorFLN(PSTR("Probing had returned errors - autoleveling canceled.")); - return; - } - correctAutolevel(com,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]); - Com::printF(PSTR("CurrentZ:"),currentZ);Com::printFLN(PSTR(" atZ:"),Printer::currentPosition[Z_AXIS]); - // With max z endstop we adjust zlength so after next homing we have also a calibrated printer - Printer::zMin = 0; - #if MAX_HARDWARE_ENDSTOP_Z - float xRot,yRot,zRot; - #if BED_CORRECTION_METHOD != 1 - Printer::transformFromPrinter(Printer::currentPosition[X_AXIS],Printer::currentPosition[Y_AXIS],Printer::currentPosition[Z_AXIS],xRot,yRot,zRot); - Com::printFLN(PSTR("Z after rotation:"),zRot); - #else - zRot = Printer::currentPosition[Z_AXIS]; - #endif - // With max z endstop we adjust zlength so after next homing we have also a calibrated printer - if(s != 0) { - Printer::zLength += currentZ - zRot; - Com::printFLN(Com::tZProbePrinterHeight, Printer::zLength); - } - #endif - Printer::currentPositionSteps[Z_AXIS] = currentZ * Printer::axisStepsPerMM[Z_AXIS]; - Printer::updateCurrentPosition(true); + // 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(), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]); +#endif + Printer::startProbing(true); + //GCode::executeFString(Com::tZProbeStartScript); + Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0; + Plane plane; #if BED_CORRECTION_METHOD == 1 - if(fabs(plane.a) < 0.00025 && fabsf(plane.b) < 0.00025 ) - break; // we reached achievable precision so we can stop - } -#endif - 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(PSTR("G32 ")); - #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; -} - -#endif - -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 + 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(), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]); + Printer::startProbing(true); + } +#endif +#endif + if(!measureAutolevelPlane(plane)) { + Com::printErrorFLN(PSTR("Probing had returned errors - autoleveling canceled.")); + return false; + } + correctAutolevel(com,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]); + Com::printF(PSTR("CurrentZ:"),currentZ); + Com::printFLN(PSTR(" atZ:"),Printer::currentPosition[Z_AXIS]); + // With max z endstop we adjust zlength 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 endstop we adjust zlength so after next homing we have also a calibrated printer + if(s != 0) { + Printer::zLength += currentZ - zRot; + Com::printFLN(Com::tZProbePrinterHeight, Printer::zLength); + } +#endif + Printer::currentPositionSteps[Z_AXIS] = currentZ * Printer::axisStepsPerMM[Z_AXIS]; + Printer::updateCurrentPosition(true); +#if BED_CORRECTION_METHOD == 1 + if(fabs(plane.a) < 0.00025 && fabsf(plane.b) < 0.00025 ) + break; // we reached achievable precision so we can stop + } +#endif + 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(PSTR("G32 ")); +#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; + return true; +} + +#endif + +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 -float Printer::runZMaxProbe() -{ - #if NONLINEAR_SYSTEM - long startZ = realDeltaPositionSteps[Z_AXIS] = currentDeltaPositionSteps[Z_AXIS]; // update real - #endif - Commands::waitUntilEndOfAllMoves(); - long probeDepth = 2*(Printer::zMaxSteps-Printer::zMinSteps); - stepsRemainingAtZHit = -1; - setZProbingActive(true); - PrintLine::moveRelativeDistanceInSteps(0,0,probeDepth,0,EEPROM::zProbeSpeed(),true,true); - if(stepsRemainingAtZHit < 0) - { - Com::printErrorFLN(Com::tZProbeFailed); - return -1; - } - setZProbingActive(false); - currentPositionSteps[Z_AXIS] -= stepsRemainingAtZHit; - #if NONLINEAR_SYSTEM - 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,EEPROM::zProbeSpeed(),true,true); - return distance; +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,EEPROM::zProbeSpeed(),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 + 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,EEPROM::zProbeSpeed(),true,true); + return distance; } #endif #if FEATURE_Z_PROBE void Printer::startProbing(bool runScript) { - float oldOffX = Printer::offsetX; - float oldOffY = Printer::offsetY; - float oldOffZ = Printer::offsetZ; - if(runScript) - GCode::executeFString(Com::tZProbeStartScript); - float maxStartHeight = EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0) + 0.1; - if(currentPosition[Z_AXIS] > maxStartHeight) { - moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, maxStartHeight, IGNORE_COORDINATE, homingFeedrate[Z_AXIS]); - } - Printer::offsetX = -EEPROM::zProbeXOffset(); - Printer::offsetY = -EEPROM::zProbeYOffset(); - Printer::offsetZ = 0; // we correct this with probe height - PrintLine::moveRelativeDistanceInSteps((Printer::offsetX - oldOffX) * Printer::axisStepsPerMM[X_AXIS], - (Printer::offsetY - oldOffY) * Printer::axisStepsPerMM[Y_AXIS], - 0, 0, EEPROM::zProbeXYSpeed(), true, ALWAYS_CHECK_ENDSTOPS); + float oldOffX = Printer::offsetX; + float oldOffY = Printer::offsetY; + float oldOffZ = Printer::offsetZ; + if(runScript) + GCode::executeFString(Com::tZProbeStartScript); + float maxStartHeight = EEPROM::zProbeBedDistance() + (EEPROM::zProbeHeight() > 0 ? EEPROM::zProbeHeight() : 0) + 0.1; + if(currentPosition[Z_AXIS] > maxStartHeight) { + moveTo(IGNORE_COORDINATE, IGNORE_COORDINATE, maxStartHeight, IGNORE_COORDINATE, homingFeedrate[Z_AXIS]); + } + Printer::offsetX = -EEPROM::zProbeXOffset(); + Printer::offsetY = -EEPROM::zProbeYOffset(); + Printer::offsetZ = 0; // we correct this with probe height + PrintLine::moveRelativeDistanceInSteps((Printer::offsetX - oldOffX) * Printer::axisStepsPerMM[X_AXIS], + (Printer::offsetY - oldOffY) * Printer::axisStepsPerMM[Y_AXIS], + 0, 0, EEPROM::zProbeXYSpeed(), true, ALWAYS_CHECK_ENDSTOPS); } void Printer::finishProbing() { - float oldOffX = Printer::offsetX; - float oldOffY = Printer::offsetY; - float oldOffZ = Printer::offsetZ; - GCode::executeFString(Com::tZProbeEndScript); - if(Extruder::current) - { - Printer::offsetX = -Extruder::current->xOffset * Printer::invAxisStepsPerMM[X_AXIS]; - Printer::offsetY = -Extruder::current->yOffset * Printer::invAxisStepsPerMM[Y_AXIS]; - Printer::offsetZ = -Extruder::current->zOffset * Printer::invAxisStepsPerMM[Z_AXIS]; - } - PrintLine::moveRelativeDistanceInSteps((Printer::offsetX - oldOffX) * Printer::axisStepsPerMM[X_AXIS], - (Printer::offsetY - oldOffY) * Printer::axisStepsPerMM[Y_AXIS], - (Printer::offsetZ - oldOffZ) * Printer::axisStepsPerMM[Z_AXIS], 0, EEPROM::zProbeXYSpeed(), true, ALWAYS_CHECK_ENDSTOPS); + float oldOffX = Printer::offsetX; + float oldOffY = Printer::offsetY; + float oldOffZ = Printer::offsetZ; + GCode::executeFString(Com::tZProbeEndScript); + if(Extruder::current) { + Printer::offsetX = -Extruder::current->xOffset * Printer::invAxisStepsPerMM[X_AXIS]; + Printer::offsetY = -Extruder::current->yOffset * Printer::invAxisStepsPerMM[Y_AXIS]; + Printer::offsetZ = -Extruder::current->zOffset * Printer::invAxisStepsPerMM[Z_AXIS]; + } + PrintLine::moveRelativeDistanceInSteps((Printer::offsetX - oldOffX) * Printer::axisStepsPerMM[X_AXIS], + (Printer::offsetY - oldOffY) * Printer::axisStepsPerMM[Y_AXIS], + (Printer::offsetZ - oldOffZ) * Printer::axisStepsPerMM[Z_AXIS], 0, EEPROM::zProbeXYSpeed(), true, ALWAYS_CHECK_ENDSTOPS); } /* @@ -443,193 +455,254 @@ e) Add bending correction Then we return the measured and corrected z distance. */ -float Printer::runZProbe(bool first,bool last,uint8_t repeat,bool runStartScript) -{ - float oldOffX = Printer::offsetX; - float oldOffY = Printer::offsetY; - float oldOffZ = Printer::offsetZ; - if(first) - startProbing(runStartScript); - Commands::waitUntilEndOfAllMoves(); - int32_t sum = 0, probeDepth; - int32_t shortMove = static_cast((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] = currentDeltaPositionSteps[Z_AXIS]; // update real - #endif - //int32_t updateZ = 0; - waitForZProbeStart(); - 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 - //int32_t offx = axisStepsPerMM[X_AXIS] * EEPROM::zProbeXOffset(); - //int32_t offy = axisStepsPerMM[Y_AXIS] * EEPROM::zProbeYOffset(); - //PrintLine::moveRelativeDistanceInSteps(-offx,-offy,0,0,EEPROM::zProbeXYSpeed(),true,true); - setZProbingActive(true); - PrintLine::moveRelativeDistanceInSteps(0, 0, -probeDepth, 0, EEPROM::zProbeSpeed(), true, true); - if(stepsRemainingAtZHit < 0) - { - Com::printErrorFLN(Com::tZProbeFailed); - return -1; - } - setZProbingActive(false); - #if NONLINEAR_SYSTEM - stepsRemainingAtZHit = realDeltaPositionSteps[C_TOWER] - currentDeltaPositionSteps[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 - currentDeltaPositionSteps[A_TOWER] += stepsRemainingAtZHit; // Update difference - currentDeltaPositionSteps[B_TOWER] += stepsRemainingAtZHit; - currentDeltaPositionSteps[C_TOWER] += stepsRemainingAtZHit; - #endif - currentPositionSteps[Z_AXIS] += stepsRemainingAtZHit; // now current position is correct - sum += lastCorrection - currentPositionSteps[Z_AXIS]; - if(r + 1 < repeat) // go only shortest possible move up for repetitions - PrintLine::moveRelativeDistanceInSteps(0, 0, shortMove, 0, EEPROM::zProbeSpeed(), true, false); - } - float distance = static_cast(sum) * invAxisStepsPerMM[Z_AXIS] / static_cast(repeat) + EEPROM::zProbeHeight(); - #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],0) * invAxisStepsPerMM[Z_AXIS]; - distance += zCorr; - } - #endif - distance += bendingCorrectionAt(currentPosition[X_AXIS], currentPosition[Y_AXIS]); - Com::printF(Com::tZProbe, distance); - Com::printF(Com::tSpaceXColon, realXPosition()); - #if DISTORTION_CORRECTION - if(Printer::distortion.isEnabled()) { - Com::printF(Com::tSpaceYColon, realYPosition()); - Com::printFLN(PSTR(" zCorr:"), zCorr); - } else { - Com::printFLN(Com::tSpaceYColon, realYPosition()); - } - #else - Com::printFLN(Com::tSpaceYColon, realYPosition()); - #endif - // Go back to start position - PrintLine::moveRelativeDistanceInSteps(0, 0, lastCorrection - currentPositionSteps[Z_AXIS], 0, EEPROM::zProbeSpeed(), true, false); - //PrintLine::moveRelativeDistanceInSteps(offx,offy,0,0,EEPROM::zProbeXYSpeed(),true,true); - if(last) - finishProbing(); - return distance; +float Printer::runZProbe(bool first,bool last,uint8_t repeat,bool runStartScript) { + float oldOffX = Printer::offsetX; + float oldOffY = Printer::offsetY; + float oldOffZ = Printer::offsetZ; + if(first) + startProbing(runStartScript); + Commands::waitUntilEndOfAllMoves(); + int32_t sum = 0, probeDepth; + int32_t shortMove = static_cast((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(); + Endstops::update(); + Endstops::update(); + if(Endstops::zProbe()) { + Com::printErrorFLN(PSTR("z-probe triggered before starting probing.")); + return ILLEGAL_Z_PROBE; + } + 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 + //int32_t offx = axisStepsPerMM[X_AXIS] * EEPROM::zProbeXOffset(); + //int32_t offy = axisStepsPerMM[Y_AXIS] * EEPROM::zProbeYOffset(); + //PrintLine::moveRelativeDistanceInSteps(-offx,-offy,0,0,EEPROM::zProbeXYSpeed(),true,true); + setZProbingActive(true); + PrintLine::moveRelativeDistanceInSteps(0, 0, -probeDepth, 0, EEPROM::zProbeSpeed(), true, true); + if(stepsRemainingAtZHit < 0) { + Com::printErrorFLN(Com::tZProbeFailed); + return ILLEGAL_Z_PROBE; + } + setZProbingActive(false); +#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; +#endif + currentPositionSteps[Z_AXIS] += stepsRemainingAtZHit; // now current position is correct + sum += lastCorrection - currentPositionSteps[Z_AXIS]; + if(r + 1 < repeat) { + // go only shortest possible move up for repetitions + PrintLine::moveRelativeDistanceInSteps(0, 0, shortMove, 0, EEPROM::zProbeSpeed(), true, true); + if(Endstops::zProbe()) { + Com::printErrorFLN(PSTR("z-probe did not untrigger on repetitive measurement - maybe you need to increase distance!")); + return ILLEGAL_Z_PROBE; + } + } + } + float distance = static_cast(sum) * invAxisStepsPerMM[Z_AXIS] / static_cast(repeat) + EEPROM::zProbeHeight(); +#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],0) * invAxisStepsPerMM[Z_AXIS]; + distance += zCorr; + } +#endif + distance += bendingCorrectionAt(currentPosition[X_AXIS], currentPosition[Y_AXIS]); + Com::printF(Com::tZProbe, distance); + Com::printF(Com::tSpaceXColon, realXPosition()); +#if DISTORTION_CORRECTION + if(Printer::distortion.isEnabled()) { + Com::printF(Com::tSpaceYColon, realYPosition()); + Com::printFLN(PSTR(" zCorr:"), zCorr); + } else { + Com::printFLN(Com::tSpaceYColon, realYPosition()); + } +#else + Com::printFLN(Com::tSpaceYColon, realYPosition()); +#endif + // Go back to start position + PrintLine::moveRelativeDistanceInSteps(0, 0, lastCorrection - currentPositionSteps[Z_AXIS], 0, EEPROM::zProbeSpeed(), true, true); + if(Endstops::zProbe()) { + Com::printErrorFLN(PSTR("z-probe did not untrigger after going back to start position.")); + return ILLEGAL_Z_PROBE; + } + //PrintLine::moveRelativeDistanceInSteps(offx,offy,0,0,EEPROM::zProbeXYSpeed(),true,true); + if(last) + finishProbing(); + return distance; } float Printer::bendingCorrectionAt(float x, float y) { - RVector3 p0(EEPROM::zProbeX1(),EEPROM::zProbeY1(),EEPROM::bendingCorrectionA()); - RVector3 p1(EEPROM::zProbeX2(),EEPROM::zProbeY2(),EEPROM::bendingCorrectionB()); - RVector3 p2(EEPROM::zProbeX3(),EEPROM::zProbeY3(),EEPROM::bendingCorrectionC()); - RVector3 a = p1-p0,b = p2 - p0; - RVector3 n = a.cross(b); - RVector3 l0(x,y,0); - return ((p0 - l0).scalar(n)) / n.z; + 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 +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 +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 +} + +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::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 - 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]; +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::transformFromPrinter(float x,float y,float z,float &transX,float &transY,float &transZ) -{ - 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]; - #if FEATURE_AXISCOMP - // Axis compensation: - transY = transY - transZ * EEPROM::axisCompTanYZ(); - transX = transX - transY * EEPROM::axisCompTanXY() - transZ * EEPROM::axisCompTanXZ(); - #endif +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::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(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); +/* +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 diff --git a/Repetier/Commands.cpp b/Repetier/Commands.cpp index 59a6816..7f70262 100644 --- a/Repetier/Commands.cpp +++ b/Repetier/Commands.cpp @@ -1,22 +1,22 @@ /* - This file is part of Repetier-Firmware. +This file is part of Repetier-Firmware. - Repetier-Firmware is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. +Repetier-Firmware is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. - Repetier-Firmware is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. +Repetier-Firmware is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with Repetier-Firmware. If not, see . +You should have received a copy of the GNU General Public License +along with Repetier-Firmware. If not, see . - This firmware is a nearly complete rewrite of the sprinter firmware - by kliment (https://github.com/kliment/Sprinter) - which based on Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. +This firmware is a nearly complete rewrite of the sprinter firmware +by kliment (https://github.com/kliment/Sprinter) +which based on Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. */ #include "Repetier.h" @@ -25,24 +25,19 @@ const int8_t sensitive_pins[] PROGMEM = SENSITIVE_PINS; // Sensitive pin list fo int Commands::lowestRAMValue = MAX_RAM; int Commands::lowestRAMValueSend = MAX_RAM; -void Commands::commandLoop() -{ - while(true) - { +void Commands::commandLoop() { + while(true) { #ifdef DEBUG_PRINT debugWaitLoop = 1; #endif - if(!Printer::isBlockingReceive()) - { + if(!Printer::isBlockingReceive()) { GCode::readFromSerial(); GCode *code = GCode::peekCurrentCommand(); //UI_SLOW; // do longer timed user interface action UI_MEDIUM; // do check encoder - if(code) - { + if(code) { #if SDSUPPORT - if(sd.savetosd) - { + if(sd.savetosd) { if(!(code->hasM() && code->M == 29)) // still writing to file sd.writeCommand(code); else @@ -50,38 +45,33 @@ void Commands::commandLoop() #if ECHO_ON_EXECUTE code->echoCommand(); #endif - } - else + } else #endif Commands::executeGCode(code); code->popCurrentCommand(); } - } - else - { + } else { UI_MEDIUM; } Printer::defaultLoopActions(); } } -void Commands::checkForPeriodicalActions(bool allowNewMoves) -{ +void Commands::checkForPeriodicalActions(bool allowNewMoves) { Printer::handleInterruptEvent(); EVENT_PERIODICAL; if(!executePeriodical) return; executePeriodical = 0; EVENT_TIMER_100MS; Extruder::manageTemperatures(); - if(--counter250ms == 0) - { + if(--counter250ms == 0) { if(manageMonitor) writeMonitor(); counter250ms = 5; EVENT_TIMER_500MS; } // If called from queueDelta etc. it is an error to start a new move since it - // would invalidate old computation resulting in unpredicted behaviour. + // would invalidate old computation resulting in unpredicted behavior. // lcd controller can start new moves, so we disallow it if called from within // a move command. UI_SLOW(allowNewMoves); @@ -89,39 +79,33 @@ void Commands::checkForPeriodicalActions(bool allowNewMoves) /** \brief Waits until movement cache is empty. - Some commands expect no movement, before they can execute. This function - waits, until the steppers are stopped. In the meanwhile it buffers incoming - commands and manages temperatures. +Some commands expect no movement, before they can execute. This function +waits, until the steppers are stopped. In the meanwhile it buffers incoming +commands and manages temperatures. */ -void Commands::waitUntilEndOfAllMoves() -{ +void Commands::waitUntilEndOfAllMoves() { #ifdef DEBUG_PRINT debugWaitLoop = 8; #endif - while(PrintLine::hasLines()) - { + while(PrintLine::hasLines()) { GCode::readFromSerial(); checkForPeriodicalActions(false); UI_MEDIUM; } } -void Commands::waitUntilEndOfAllBuffers() -{ +void Commands::waitUntilEndOfAllBuffers() { GCode *code = NULL; #ifdef DEBUG_PRINT debugWaitLoop = 9; #endif - while(PrintLine::hasLines() || (code != NULL)) - { + while(PrintLine::hasLines() || (code != NULL)) { GCode::readFromSerial(); code = GCode::peekCurrentCommand(); UI_MEDIUM; // do check encoder - if(code) - { + if(code) { #if SDSUPPORT - if(sd.savetosd) - { + if(sd.savetosd) { if(!(code->hasM() && code->M == 29)) // still writing to file sd.writeCommand(code); else @@ -129,8 +113,7 @@ void Commands::waitUntilEndOfAllBuffers() #if ECHO_ON_EXECUTE code->echoCommand(); #endif - } - else + } else #endif Commands::executeGCode(code); code->popCurrentCommand(); @@ -140,12 +123,10 @@ void Commands::waitUntilEndOfAllBuffers() } } -void Commands::printCurrentPosition(FSTRINGPARAM(s)) -{ +void Commands::printCurrentPosition(FSTRINGPARAM(s)) { float x, y, z; Printer::realPosition(x, y, z); - if (isnan(x) || isinf(x) || isnan(y) || isinf(y) || isnan(z) || isinf(z)) - { + if (isnan(x) || isinf(x) || isnan(y) || isinf(y) || isnan(z) || isinf(z)) { Com::printErrorFLN(s); // flag where the error condition came from } x += Printer::coordinateOffset[X_AXIS]; @@ -159,8 +140,7 @@ void Commands::printCurrentPosition(FSTRINGPARAM(s)) //Com::printFLN(PSTR(" OffY:"),Printer::offsetY); } -void Commands::printTemperatures(bool showRaw) -{ +void Commands::printTemperatures(bool showRaw) { #if NUM_EXTRUDER > 0 float temp = Extruder::current->tempControl.currentTemperatureC; #if HEATED_BED_SENSOR_TYPE == 0 @@ -172,20 +152,18 @@ void Commands::printTemperatures(bool showRaw) #if HAVE_HEATED_BED Com::printF(Com::tSpaceBColon,Extruder::getHeatedBedTemperature()); Com::printF(Com::tSpaceSlash,heatedBedController.targetTemperatureC,0); - if(showRaw) - { + if(showRaw) { Com::printF(Com::tSpaceRaw,(int)NUM_EXTRUDER); Com::printF(Com::tColon,(1023 << (2 - ANALOG_REDUCE_BITS)) - heatedBedController.currentTemperature); } - Com::printF(Com::tSpaceBAtColon,(pwm_pos[heatedBedController.pwmIndex])); // Show output of autotune when tuning! + Com::printF(Com::tSpaceBAtColon,(pwm_pos[heatedBedController.pwmIndex])); // Show output of auto tune when tuning! #endif #endif #if TEMP_PID - Com::printF(Com::tSpaceAtColon,(autotuneIndex == 255 ? pwm_pos[Extruder::current->id] : pwm_pos[autotuneIndex])); // Show output of autotune when tuning! + Com::printF(Com::tSpaceAtColon,(autotuneIndex == 255 ? pwm_pos[Extruder::current->id] : pwm_pos[autotuneIndex])); // Show output of auto tune when tuning! #endif #if NUM_EXTRUDER > 1 && MIXING_EXTRUDER == 0 - for(uint8_t i = 0; i < NUM_EXTRUDER; i++) - { + for(uint8_t i = 0; i < NUM_EXTRUDER; i++) { Com::printF(Com::tSpaceT,(int)i); Com::printF(Com::tColon,extruder[i].tempControl.currentTemperatureC); Com::printF(Com::tSpaceSlash,extruder[i].tempControl.targetTemperatureC,0); @@ -193,24 +171,21 @@ void Commands::printTemperatures(bool showRaw) Com::printF(Com::tSpaceAt,(int)i); Com::printF(Com::tColon,(pwm_pos[extruder[i].tempControl.pwmIndex])); // Show output of autotune when tuning! #endif - if(showRaw) - { + if(showRaw) { Com::printF(Com::tSpaceRaw,(int)i); Com::printF(Com::tColon,(1023 << (2 - ANALOG_REDUCE_BITS)) - extruder[i].tempControl.currentTemperature); } } #elif NUM_EXTRUDER == 1 - if(showRaw) - { - Com::printF(Com::tSpaceRaw,(int)0); - Com::printF(Com::tColon,(1023 << (2 - ANALOG_REDUCE_BITS)) - extruder[0].tempControl.currentTemperature); + if(showRaw) { + Com::printF(Com::tSpaceRaw,(int)0); + Com::printF(Com::tColon,(1023 << (2 - ANALOG_REDUCE_BITS)) - extruder[0].tempControl.currentTemperature); } #endif Com::println(); #endif } -void Commands::changeFeedrateMultiply(int factor) -{ +void Commands::changeFeedrateMultiply(int factor) { if(factor < 25) factor = 25; if(factor > 500) factor = 500; Printer::feedrate *= (float)factor / (float)Printer::feedrateMultiply; @@ -218,8 +193,7 @@ void Commands::changeFeedrateMultiply(int factor) Com::printFLN(Com::tSpeedMultiply, factor); } -void Commands::changeFlowrateMultiply(int factor) -{ +void Commands::changeFlowrateMultiply(int factor) { if(factor < 25) factor = 25; if(factor > 200) factor = 200; Printer::extrudeMultiply = factor; @@ -237,8 +211,7 @@ uint8_t fanKickstart; uint8_t fan2Kickstart; #endif -void Commands::setFanSpeed(int speed, bool immediately) -{ +void Commands::setFanSpeed(int speed, bool immediately) { #if FAN_PIN >- 1 && FEATURE_FAN_CONTROL if(Printer::fanSpeed == speed) return; @@ -246,27 +219,24 @@ void Commands::setFanSpeed(int speed, bool immediately) Printer::setMenuMode(MENU_MODE_FAN_RUNNING,speed != 0); Printer::fanSpeed = speed; if(PrintLine::linesCount == 0 || immediately) { - if(Printer::mode == PRINTER_MODE_FFF) - { - for(fast8_t i = 0; i < PRINTLINE_CACHE_SIZE; i++) - PrintLine::lines[i].secondSpeed = speed; // fill all printline buffers with new fan speed value + if(Printer::mode == PRINTER_MODE_FFF) { + for(fast8_t i = 0; i < PRINTLINE_CACHE_SIZE; i++) + PrintLine::lines[i].secondSpeed = speed; // fill all printline buffers with new fan speed value } - Printer::setFanSpeedDirectly(speed); - } + Printer::setFanSpeedDirectly(speed); + } Com::printFLN(Com::tFanspeed,speed); // send only new values to break update loops! #endif } -void Commands::setFan2Speed(int speed) -{ - #if FAN2_PIN >- 1 && FEATURE_FAN2_CONTROL - speed = constrain(speed,0,255); - Printer::setFan2SpeedDirectly(speed); - Com::printFLN(Com::tFan2speed,speed); // send only new values to break update loops! - #endif +void Commands::setFan2Speed(int speed) { +#if FAN2_PIN >- 1 && FEATURE_FAN2_CONTROL + speed = constrain(speed,0,255); + Printer::setFan2SpeedDirectly(speed); + Com::printFLN(Com::tFan2speed,speed); // send only new values to break update loops! +#endif } -void Commands::reportPrinterUsage() -{ +void Commands::reportPrinterUsage() { #if EEPROM_MODE != 0 float dist = Printer::filamentPrinted * 0.001 + HAL::eprGetFloat(EPR_PRINTING_DISTANCE); Com::printF(Com::tPrintedFilament, dist, 2); @@ -292,8 +262,7 @@ void Commands::reportPrinterUsage() // Digipot methods for controling current and microstepping #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1 -int digitalPotWrite(int address, uint16_t value) // From Arduino DigitalPotControl example -{ +int digitalPotWrite(int address, uint16_t value) { // From Arduino DigitalPotControl example if(value > 255) value = 255; WRITE(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip @@ -303,22 +272,19 @@ int digitalPotWrite(int address, uint16_t value) // From Arduino DigitalPotContr //delay(10); } -void setMotorCurrent(uint8_t driver, uint16_t current) -{ +void setMotorCurrent(uint8_t driver, uint16_t current) { if(driver > 4) return; const uint8_t digipot_ch[] = DIGIPOT_CHANNELS; digitalPotWrite(digipot_ch[driver], current); } -void setMotorCurrentPercent( uint8_t channel, float level) -{ +void setMotorCurrentPercent( uint8_t channel, float level) { uint16_t raw_level = ( level * 255 / 100 ); setMotorCurrent(channel,raw_level); } #endif -void motorCurrentControlInit() //Initialize Digipot Motor Current -{ +void motorCurrentControlInit() { //Initialize Digipot Motor Current #if DIGIPOTSS_PIN && DIGIPOTSS_PIN > -1 HAL::spiInit(0); //SPI.begin(); SET_OUTPUT(DIGIPOTSS_PIN); @@ -339,8 +305,7 @@ void motorCurrentControlInit() //Initialize Digipot Motor Current #if STEPPER_CURRENT_CONTROL == CURRENT_CONTROL_LTC2600 -void setMotorCurrent( uint8_t channel, unsigned short level ) -{ +void setMotorCurrent( uint8_t channel, unsigned short level ) { if(channel >= LTC2600_NUM_CHANNELS) return; const uint8_t ltc_channels[] = LTC2600_CHANNELS; if(channel > LTC2600_NUM_CHANNELS) return; @@ -364,16 +329,14 @@ void setMotorCurrent( uint8_t channel, unsigned short level ) WRITE( LTC2600_CS_PIN, LOW ); // transfer command and address - for( i = 7; i >= 0; i-- ) - { + for( i = 7; i >= 0; i-- ) { WRITE( LTC2600_SDI_PIN, address & (0x01 << i)); WRITE( LTC2600_SCK_PIN, 1 ); WRITE( LTC2600_SCK_PIN, 0 ); } // transfer the data word - for( i = 15; i >= 0; i-- ) - { + for( i = 15; i >= 0; i-- ) { WRITE( LTC2600_SDI_PIN, level & (0x01 << i)); WRITE( LTC2600_SCK_PIN, 1 ); WRITE( LTC2600_SCK_PIN, 0 ); @@ -384,15 +347,13 @@ void setMotorCurrent( uint8_t channel, unsigned short level ) WRITE( LTC2600_CS_PIN, HIGH ); } // setLTC2600 -void setMotorCurrentPercent( uint8_t channel, float level) -{ +void setMotorCurrentPercent( uint8_t channel, float level) { if(level > 100.0f) level = 100.0f; uint16_t raw_level = static_cast( (long)level * 65535L / 100L ); setMotorCurrent(channel,raw_level); } -void motorCurrentControlInit() //Initialize LTC2600 Motor Current -{ +void motorCurrentControlInit() { //Initialize LTC2600 Motor Current uint8_t i; #ifdef MOTOR_CURRENT_PERCENT const float digipot_motor_current[] = MOTOR_CURRENT_PERCENT; @@ -401,8 +362,7 @@ void motorCurrentControlInit() //Initialize LTC2600 Motor Current setMotorCurrentPercent(i,digipot_motor_current[i]); #else const unsigned int ltc_current[] = MOTOR_CURRENT; - for(i = 0; i < LTC2600_NUM_CHANNELS; i++) - { + for(i = 0; i < LTC2600_NUM_CHANNELS; i++) { setMotorCurrent(i, ltc_current[i] ); } #endif @@ -410,8 +370,7 @@ void motorCurrentControlInit() //Initialize LTC2600 Motor Current #endif #if STEPPER_CURRENT_CONTROL == CURRENT_CONTROL_ALLIGATOR -void setMotorCurrent(uint8_t channel, unsigned short value) -{ +void setMotorCurrent(uint8_t channel, unsigned short value) { if(channel >= 7) // max channel (X,Y,Z,E0,E1,E2,E3) return; if(value > 255) @@ -435,16 +394,13 @@ void setMotorCurrent(uint8_t channel, unsigned short value) WRITE(SPI_FLASH_CS, HIGH); WRITE(SDSS, HIGH); - if(channel > 3) // DAC Piggy E1,E2,E3 - { + if(channel > 3) { // DAC Piggy E1,E2,E3 WRITE(DAC1_SYNC,LOW); HAL::delayMicroseconds(2); WRITE(DAC1_SYNC,HIGH); HAL::delayMicroseconds(2); WRITE(DAC1_SYNC,LOW); - } - else // DAC onboard X,Y,Z,E0 - { + } else { // DAC onboard X,Y,Z,E0 WRITE(DAC0_SYNC,LOW); HAL::delayMicroseconds(2); WRITE(DAC0_SYNC,HIGH); @@ -456,14 +412,12 @@ void setMotorCurrent(uint8_t channel, unsigned short value) HAL::spiSend(SPI_CHAN_DAC, externalDac_buf, 2); } -void setMotorCurrentPercent( uint8_t channel, float level) -{ +void setMotorCurrentPercent( uint8_t channel, float level) { uint16_t raw_level = ( level * 255 / 100 ); setMotorCurrent(channel,raw_level); } -void motorCurrentControlInit() //Initialize Motor Current -{ +void motorCurrentControlInit() { //Initialize Motor Current uint8_t externalDac_buf[2] = {0x20, 0x00};//all off // All SPI chip-select HIGH @@ -522,35 +476,29 @@ int16_t _valuesEp[] = {0,0,0,0}; uint8_t dac_stepper_channel[] = MCP4728_STEPPER_ORDER; -int dacSimpleCommand(uint8_t simple_command) -{ +int dacSimpleCommand(uint8_t simple_command) { HAL::i2cStartWait(MCP4728_GENERALCALL_ADDRESS + I2C_WRITE); HAL::i2cWrite(simple_command); HAL::i2cStop(); } -void dacReadStatus() -{ +void dacReadStatus() { HAL::delayMilliseconds(500); HAL::i2cStartWait(MCP4728_I2C_ADDRESS | I2C_READ); - for (int i = 0; i < 8; i++) // 2 sets of 4 Channels (1 EEPROM, 1 Runtime) - { + for (int i = 0; i < 8; i++) { // 2 sets of 4 Channels (1 EEPROM, 1 Runtime) uint8_t deviceID = HAL::i2cReadAck(); uint8_t hiByte = HAL::i2cReadAck(); uint8_t loByte = ((i < 7) ? HAL::i2cReadAck() : HAL::i2cReadNak()); uint8_t isEEPROM = (deviceID & 0B00001000) >> 3; uint8_t channel = (deviceID & 0B00110000) >> 4; - if (isEEPROM == 1) - { + if (isEEPROM == 1) { _intVrefEp[channel] = (hiByte & 0B10000000) >> 7; _gainEp[channel] = (hiByte & 0B00010000) >> 4; _powerDownEp[channel] = (hiByte & 0B01100000) >> 5; _valuesEp[channel] = word((hiByte & 0B00001111), loByte); - } - else - { + } else { _intVref[channel] = (hiByte & 0B10000000) >> 7; _gain[channel] = (hiByte & 0B00010000) >> 4; _powerDown[channel] = (hiByte & 0B01100000) >> 5; @@ -561,15 +509,13 @@ void dacReadStatus() HAL::i2cStop(); } -void dacAnalogUpdate(bool saveEEPROM = false) -{ +void dacAnalogUpdate(bool saveEEPROM = false) { uint8_t dac_write_cmd = MCP4728_CMD_SEQ_WRITE; HAL::i2cStartWait(MCP4728_I2C_ADDRESS + I2C_WRITE); if (saveEEPROM) HAL::i2cWrite(dac_write_cmd); - for (int i = 0; i < MCP4728_NUM_CHANNELS; i++) - { + for (int i = 0; i < MCP4728_NUM_CHANNELS; i++) { uint16_t level = dac_motor_current[i]; uint8_t highbyte = ( _intVref[i] << 7 | _gain[i] << 4 | (uint8_t)((level) >> 8) ); @@ -589,16 +535,13 @@ void dacAnalogUpdate(bool saveEEPROM = false) // if (saveEEPROM) dacReadStatus(); // Not necessary, just a read-back sanity check. } -void dacCommitEeprom() -{ +void dacCommitEeprom() { dacAnalogUpdate(true); dacReadStatus(); // Refresh EEPROM Values with values actually stored in EEPROM. . } -void dacPrintSet(int dacChannelSettings[], const char* dacChannelPrefixes[]) -{ - for (int i = 0; i < MCP4728_NUM_CHANNELS; i++) - { +void dacPrintSet(int dacChannelSettings[], const char* dacChannelPrefixes[]) { + for (int i = 0; i < MCP4728_NUM_CHANNELS; i++) { uint8_t dac_channel = dac_stepper_channel[i]; // DAC Channel is a mapped lookup. Com::printF(dacChannelPrefixes[i], ((float)dacChannelSettings[dac_channel] * 100 / MCP4728_VOUT_MAX)); Com::printF(Com::tSpaceRaw); @@ -606,8 +549,7 @@ void dacPrintSet(int dacChannelSettings[], const char* dacChannelPrefixes[]) } } -void dacPrintValues() -{ +void dacPrintValues() { const char* dacChannelPrefixes[] = {Com::tSpaceXColon, Com::tSpaceYColon, Com::tSpaceZColon, Com::tSpaceEColon}; Com::printFLN(Com::tMCPEpromSettings); @@ -617,121 +559,111 @@ void dacPrintValues() dacPrintSet(dac_motor_current, dacChannelPrefixes); // And another for the RUNTIME set } -void setMotorCurrent( uint8_t xyz_channel, uint16_t level ) -{ +void setMotorCurrent( uint8_t xyz_channel, uint16_t level ) { if (xyz_channel >= MCP4728_NUM_CHANNELS) return; uint8_t stepper_channel = dac_stepper_channel[xyz_channel]; dac_motor_current[stepper_channel] = level < MCP4728_VOUT_MAX ? level : MCP4728_VOUT_MAX; dacAnalogUpdate(); } -void setMotorCurrentPercent( uint8_t channel, float level) -{ +void setMotorCurrentPercent( uint8_t channel, float level) { uint16_t raw_level = ( level * MCP4728_VOUT_MAX / 100 ); setMotorCurrent(channel,raw_level); } -void motorCurrentControlInit() //Initialize MCP4728 Motor Current -{ +void motorCurrentControlInit() { //Initialize MCP4728 Motor Current HAL::i2cInit(400000); // Initialize the i2c bus. dacSimpleCommand((uint8_t)MCP4728_CMD_GC_RESET); // MCP4728 General Command Reset dacReadStatus(); // Load Values from EEPROM. - for(int i = 0; i < MCP4728_NUM_CHANNELS; i++) - { + for(int i = 0; i < MCP4728_NUM_CHANNELS; i++) { setMotorCurrent(dac_stepper_channel[i], _valuesEp[i] ); // This is not strictly necessary, but serves as a good sanity check to ensure we're all on the same page. } } #endif #if defined(X_MS1_PIN) && X_MS1_PIN > -1 -void microstepMS(uint8_t driver, int8_t ms1, int8_t ms2) -{ - if(ms1 > -1) switch(driver) - { - case 0: +void microstepMS(uint8_t driver, int8_t ms1, int8_t ms2) { + if(ms1 > -1) switch(driver) { + case 0: #if X_MS1_PIN > -1 - WRITE( X_MS1_PIN,ms1); + WRITE( X_MS1_PIN,ms1); #endif - break; - case 1: + break; + case 1: #if Y_MS1_PIN > -1 - WRITE( Y_MS1_PIN,ms1); + WRITE( Y_MS1_PIN,ms1); #endif - break; - case 2: + break; + case 2: #if Z_MS1_PIN > -1 - WRITE( Z_MS1_PIN,ms1); + WRITE( Z_MS1_PIN,ms1); #endif - break; - case 3: + break; + case 3: #if E0_MS1_PIN > -1 - WRITE(E0_MS1_PIN,ms1); + WRITE(E0_MS1_PIN,ms1); #endif - break; - case 4: + break; + case 4: #if E1_MS1_PIN > -1 - WRITE(E1_MS1_PIN,ms1); + WRITE(E1_MS1_PIN,ms1); #endif - break; + break; } - if(ms2 > -1) switch(driver) - { - case 0: + if(ms2 > -1) switch(driver) { + case 0: #if X_MS2_PIN > -1 - WRITE( X_MS2_PIN,ms2); + WRITE( X_MS2_PIN,ms2); #endif - break; - case 1: + break; + case 1: #if Y_MS2_PIN > -1 - WRITE( Y_MS2_PIN,ms2); + WRITE( Y_MS2_PIN,ms2); #endif - break; - case 2: + break; + case 2: #if Z_MS2_PIN > -1 - WRITE( Z_MS2_PIN,ms2); + WRITE( Z_MS2_PIN,ms2); #endif - break; - case 3: + break; + case 3: #if E0_MS2_PIN > -1 - WRITE(E0_MS2_PIN,ms2); + WRITE(E0_MS2_PIN,ms2); #endif - break; - case 4: + break; + case 4: #if E1_MS2_PIN > -1 - WRITE(E1_MS2_PIN,ms2); + WRITE(E1_MS2_PIN,ms2); #endif - break; + break; } } -void microstepMode(uint8_t driver, uint8_t stepping_mode) -{ - switch(stepping_mode) - { - case 1: - microstepMS(driver,MICROSTEP1); - break; - case 2: - microstepMS(driver,MICROSTEP2); - break; - case 4: - microstepMS(driver,MICROSTEP4); - break; - case 8: - microstepMS(driver,MICROSTEP8); - break; - case 16: - microstepMS(driver,MICROSTEP16); - break; - case 32: - microstepMS(driver,MICROSTEP32); - break; +void microstepMode(uint8_t driver, uint8_t stepping_mode) { + switch(stepping_mode) { + case 1: + microstepMS(driver,MICROSTEP1); + break; + case 2: + microstepMS(driver,MICROSTEP2); + break; + case 4: + microstepMS(driver,MICROSTEP4); + break; + case 8: + microstepMS(driver,MICROSTEP8); + break; + case 16: + microstepMS(driver,MICROSTEP16); + break; + case 32: + microstepMS(driver,MICROSTEP32); + break; } } -void microstepReadings() -{ +void microstepReadings() { Com::printFLN(Com::tMS1MS2Pins); #if X_MS1_PIN > -1 && X_MS2_PIN > -1 Com::printF(Com::tXColon,READ(X_MS1_PIN)); @@ -766,8 +698,7 @@ void microstepReadings() } #endif -void microstepInit() -{ +void microstepInit() { #if defined(X_MS1_PIN) && X_MS1_PIN > -1 const uint8_t microstep_modes[] = MICROSTEP_MODES; #if X_MS1_PIN > -1 @@ -805,66 +736,64 @@ void microstepInit() } /** - \brief Execute the Arc command stored in com. +\brief Execute the Arc command stored in com. */ #if ARC_SUPPORT -void Commands::processArc(GCode *com) -{ +void Commands::processArc(GCode *com) { float position[Z_AXIS_ARRAY]; Printer::realPosition(position[X_AXIS],position[Y_AXIS],position[Z_AXIS]); if(!Printer::setDestinationStepsFromGCode(com)) return; // For X Y Z E F float offset[2] = {Printer::convertToMM(com->hasI() ? com->I : 0),Printer::convertToMM(com->hasJ() ? com->J : 0)}; float target[E_AXIS_ARRAY] = {Printer::realXPosition(),Printer::realYPosition(),Printer::realZPosition(),Printer::destinationSteps[E_AXIS]*Printer::invAxisStepsPerMM[E_AXIS]}; float r; - if (com->hasR()) - { + if (com->hasR()) { /* - We need to calculate the center of the circle that has the designated radius and passes - through both the current position and the target position. This method calculates the following - set of equations where [x,y] is the vector from current to target position, d == magnitude of - that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to - the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the - length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point - [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the + length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point + [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. - d^2 == x^2 + y^2 - h^2 == r^2 - (d/2)^2 - i == x/2 - y/d*h - j == y/2 + x/d*h + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h - O <- [i,j] - - | - r - | - - | - - | h - - | - [0,0] -> C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O - Expanding the equations: + Expanding the equations: - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - Which can be written: + Which can be written: - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - Which we for size and speed reasons optimize to: + Which we for size and speed reasons optimize to: - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 */ r = Printer::convertToMM(com->R); @@ -875,40 +804,37 @@ void Commands::processArc(GCode *com) double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d) // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any // real CNC, and thus - for practical reasons - we will terminate promptly: - if(isnan(h_x2_div_d)) - { + if(isnan(h_x2_div_d)) { Com::printErrorFLN(Com::tInvalidArc); return; } // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (com->G == 3) - { + if (com->G == 3) { h_x2_div_d = -h_x2_div_d; } /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. + the left hand circle will be generated - when it is negative the right hand circle is generated. - T <-- Target position + T <-- Target position - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | + | + | - C <-- Current position */ + C <-- Current position */ // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), // even though it is advised against ever generating such circles in a single line of g-code. By // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of // travel and thus we get the unadvisably long arcs as prescribed. - if (r < 0) - { + if (r < 0) { h_x2_div_d = -h_x2_div_d; r = -r; // Finished with r. Set to positive for mc_arc } @@ -916,9 +842,7 @@ void Commands::processArc(GCode *com) offset[0] = 0.5 * (x - (y * h_x2_div_d)); offset[1] = 0.5 * (y + (x * h_x2_div_d)); - } - else // Offset mode specific computations - { + } else { // Offset mode specific computations r = hypot(offset[0], offset[1]); // Compute arc radius for arc } // Set clockwise/counter-clockwise sign for arc computations @@ -927,1566 +851,1537 @@ void Commands::processArc(GCode *com) PrintLine::arc(position, target, offset, r, isclockwise); } #endif -extern void runBedLeveling(GCode *com); +extern bool runBedLeveling(GCode *com); /** - \brief Execute the G command stored in com. +\brief Execute the G command stored in com. */ -void Commands::processGCode(GCode *com) -{ +void Commands::processGCode(GCode *com) { uint32_t codenum; //throw away variable - switch(com->G) - { - case 0: // G0 -> G1 - case 1: // G1 + switch(com->G) { + case 0: // G0 -> G1 + case 1: // G1 #if defined(SUPPORT_LASER) && SUPPORT_LASER - { // disable laser for G0 moves - bool laserOn = LaserDriver::laserOn; - if(com->G == 0 && Printer::mode == PRINTER_MODE_LASER) { - LaserDriver::laserOn = false; - } -#endif // defined - if(com->hasS()) Printer::setNoDestinationCheck(com->S != 0); - if(Printer::setDestinationStepsFromGCode(com)) // For X Y Z E F -#if NONLINEAR_SYSTEM - if (!PrintLine::queueDeltaMove(ALWAYS_CHECK_ENDSTOPS, true, true)) { - Com::printWarningFLN(PSTR("executeGCode / queueDeltaMove returns error")); - } + // disable laser for G0 moves + bool laserOn = LaserDriver::laserOn; + if(com->G == 0 && Printer::mode == PRINTER_MODE_LASER) { + LaserDriver::laserOn = false; + } +#endif // defined + if(com->hasS()) Printer::setNoDestinationCheck(com->S != 0); + if(Printer::setDestinationStepsFromGCode(com)) // For X Y Z E F +#if NONLINEAR_SYSTEM + if (!PrintLine::queueNonlinearMove(ALWAYS_CHECK_ENDSTOPS, true, true)) { + Com::printWarningFLN(PSTR("executeGCode / queueDeltaMove returns error")); + } #else - PrintLine::queueCartesianMove(ALWAYS_CHECK_ENDSTOPS, true); + PrintLine::queueCartesianMove(ALWAYS_CHECK_ENDSTOPS, true); #endif #if UI_HAS_KEYS - // ui can only execute motion commands if we are not waiting inside a move for an - // old move to finish. For normal response times, we always leave one free after - // sending a line. Drawback: 1 buffer line less for limited time. Since input cache - // gets filled while waiting, the lost is neglectible. - PrintLine::waitForXFreeLines(1, true); + // ui can only execute motion commands if we are not waiting inside a move for an + // old move to finish. For normal response times, we always leave one free after + // sending a line. Drawback: 1 buffer line less for limited time. Since input cache + // gets filled while waiting, the lost is neglectible. + PrintLine::waitForXFreeLines(1, true); #endif // UI_HAS_KEYS #ifdef DEBUG_QUEUE_MOVE - { - - InterruptProtectedBlock noInts; - int lc = (int)PrintLine::linesCount; - int lp = (int)PrintLine::linesPos; - int wp = (int)PrintLine::linesWritePos; - int n = (wp - lp); - if(n < 0) n += PRINTLINE_CACHE_SIZE; - noInts.unprotect(); - if(n != lc) - Com::printFLN(PSTR("Buffer corrupted")); - } -#endif -#if defined(SUPPORT_LASER) && SUPPORT_LASER - LaserDriver::laserOn = laserOn; - } -#endif // defined - break; -#if ARC_SUPPORT - case 2: // CW Arc - case 3: // CCW Arc MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: -#if defined(SUPPORT_LASER) && SUPPORT_LASER - { // disable laser for G0 moves - bool laserOn = LaserDriver::laserOn; - if(com->G == 0 && Printer::mode == PRINTER_MODE_LASER) { - LaserDriver::laserOn = false; - } -#endif // defined - processArc(com); -#if defined(SUPPORT_LASER) && SUPPORT_LASER - LaserDriver::laserOn = laserOn; - } -#endif // defined - break; -#endif - case 4: // G4 dwell - Commands::waitUntilEndOfAllMoves(); - codenum = 0; - if(com->hasP()) codenum = com->P; // milliseconds to wait - if(com->hasS()) codenum = com->S * 1000; // seconds to wait - codenum += HAL::timeInMilliseconds(); // keep track of when we started waiting - while((uint32_t)(codenum-HAL::timeInMilliseconds()) < 2000000000 ) - { - GCode::readFromSerial(); - Commands::checkForPeriodicalActions(true); - } - break; -#if FEATURE_RETRACTION && NUM_EXTRUDER > 0 - case 10: // G10 S<1 = long retract, 0 = short retract = default> retracts filament accoridng to stored setting -#if NUM_EXTRUDER > 1 - Extruder::current->retract(true, com->hasS() && com->S > 0); -#else - Extruder::current->retract(true, false); -#endif - break; - case 11: // G11 S<1 = long retract, 0 = short retract = default> = Undo retraction according to stored setting -#if NUM_EXTRUDER > 1 - Extruder::current->retract(false, com->hasS() && com->S > 0); -#else - Extruder::current->retract(false, false); -#endif - break; -#endif // FEATURE_RETRACTION - case 20: // G20 Units to inches - Printer::unitIsInches = 1; - break; - case 21: // G21 Units to mm - Printer::unitIsInches = 0; - break; - case 28: //G28 Home all Axis one at a time - { - uint8_t homeAllAxis = (com->hasNoXYZ() && !com->hasE()); - if(com->hasE()) - Printer::currentPositionSteps[E_AXIS] = 0; - if(homeAllAxis || !com->hasNoXYZ()) - Printer::homeAxis(homeAllAxis || com->hasX(),homeAllAxis || com->hasY(),homeAllAxis || com->hasZ()); - Printer::updateCurrentPosition(); - } - break; -#if FEATURE_Z_PROBE - case 29: // G29 3 points, build average or distortion compensation - { -#if DISTORTION_CORRECTION - float oldFeedrate = Printer::feedrate; - Printer::measureDistortion(); - Printer::feedrate = oldFeedrate; -#else - GCode::executeFString(Com::tZProbeStartScript); - bool oldAutolevel = Printer::isAutolevelActive(); - Printer::setAutolevelActive(false); - float sum = 0, last,oldFeedrate = Printer::feedrate; - Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed()); - sum = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); - if(sum < -1) break; - Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed()); - last = Printer::runZProbe(false,false); - if(last < -2) break; - sum+= last; - Printer::moveTo(EEPROM::zProbeX3(), EEPROM::zProbeY3(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed()); - last = Printer::runZProbe(false,true); - if(last < -3) break; - sum += last; - sum *= 0.33333333333333; - Com::printFLN(Com::tZProbeAverage, sum); - if(com->hasS() && com->S) - { -#if MAX_HARDWARE_ENDSTOP_Z -#if DRIVE_SYSTEM == DELTA - Printer::updateCurrentPosition(); - Printer::zLength += sum - Printer::currentPosition[Z_AXIS]; - Printer::updateDerivedParameter(); - Printer::homeAxis(true,true,true); -#else - Printer::currentPositionSteps[Z_AXIS] = sum * Printer::axisStepsPerMM[Z_AXIS]; - Printer::zLength = Printer::runZMaxProbe() + sum - ENDSTOP_Z_BACK_ON_HOME; -#endif - Com::printInfoFLN(Com::tZProbeZReset); - Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength); -#else - Printer::currentPositionSteps[Z_AXIS] = sum * Printer::axisStepsPerMM[Z_AXIS]; - Com::printFLN(PSTR("Adjusted z origin")); -#endif - } - Printer::feedrate = oldFeedrate; - Printer::setAutolevelActive(oldAutolevel); - if(com->hasS() && com->S == 2) - EEPROM::storeDataIntoEEPROM(); - Printer::updateCurrentPosition(true); - printCurrentPosition(PSTR("G29 ")); - GCode::executeFString(Com::tZProbeEndScript); - Printer::feedrate = oldFeedrate; -#endif // DISTORTION_CORRECTION - } - break; - case 30: // G30 single probe set Z0 - { - uint8_t p = (com->hasP() ? (uint8_t)com->P : 3); - //bool oldAutolevel = Printer::isAutolevelActive(); - //Printer::setAutolevelActive(false); - Printer::runZProbe(p & 1,p & 2); - //Printer::setAutolevelActive(oldAutolevel); - Printer::updateCurrentPosition(p & 1); - //printCurrentPosition(PSTR("G30 ")); - } - break; - case 31: // G31 display hall sensor output - Endstops::update(); - Endstops::update(); - Com::printF(Com::tZProbeState); - Com::printF(Endstops::zProbe() ? Com::tHSpace : Com::tLSpace); - Com::println(); - break; -#if FEATURE_AUTOLEVEL - case 32: // G32 Auto-Bed leveling - runBedLeveling(com); -#if 0 - { - -#if DISTORTION_CORRECTION - Printer::distortion.disable(true); // if level has changed, distortion is also invalid -#endif - Printer::setAutolevelActive(false); // iterate -#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(), IGNORE_COORDINATE, Printer::homingFeedrate[Z_AXIS]); -#endif - GCode::executeFString(Com::tZProbeStartScript); - //bool iterate = com->hasP() && com->P>0; - Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0; - float h1,h2,h3,hc,oldFeedrate = Printer::feedrate; - Printer::moveTo(EEPROM::zProbeX1(),EEPROM::zProbeY1(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h1 = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); - if(h1 < -1) break; - Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h2 = Printer::runZProbe(false,false); - if(h2 < -1) break; - Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h3 = Printer::runZProbe(false,true); - if(h3 < -1) break; -#if defined(MOTORIZED_BED_LEVELING) && defined(NUM_MOTOR_DRIVERS) && NUM_MOTOR_DRIVERS >= 2 - // h1 is reference heights, h2 => motor 0, h3 => motor 1 - h2 -= h1; - h3 -= h1; - 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]; -#else // defined(MOTORIZED_BED_LEVELING) - Printer::buildTransformationMatrix(h1,h2,h3); - //-(Rxx*Ryz*y-Rxz*Ryx*y+(Rxz*Ryy-Rxy*Ryz)*x)/(Rxy*Ryx-Rxx*Ryy) - // z = z-deviation from origin due to bed transformation - float z = -((Printer::autolevelTransformation[0] * Printer::autolevelTransformation[5] - - Printer::autolevelTransformation[2] * Printer::autolevelTransformation[3]) * - (float)Printer::currentPositionSteps[Y_AXIS] * Printer::invAxisStepsPerMM[Y_AXIS] + - (Printer::autolevelTransformation[2] * Printer::autolevelTransformation[4] - - Printer::autolevelTransformation[1] * Printer::autolevelTransformation[5]) * - (float)Printer::currentPositionSteps[X_AXIS] * Printer::invAxisStepsPerMM[X_AXIS]) / - (Printer::autolevelTransformation[1] * Printer::autolevelTransformation[3] - Printer::autolevelTransformation[0] * Printer::autolevelTransformation[4]); - Printer::zMin = 0; - if(com->hasS() && com->S < 3 && com->S > 0) - { -#if MAX_HARDWARE_ENDSTOP_Z -#if DRIVE_SYSTEM == DELTA - /* Printer::offsetX = 0; - Printer::offsetY = 0; - Printer::moveToReal(0,0,cz,IGNORE_COORDINATE,Printer::homingFeedrate[X_AXIS]); - PrintLine::moveRelativeDistanceInSteps(Printer::offsetX-Printer::currentPositionSteps[X_AXIS],Printer::offsetY-Printer::currentPositionSteps[Y_AXIS],0,0,Printer::homingFeedrate[X_AXIS],true,ALWAYS_CHECK_ENDSTOPS); - Printer::offsetX = 0; - Printer::offsetY = 0;*/ - Printer::zLength += (h3 + z) - Printer::currentPosition[Z_AXIS]; -#else - int32_t zBottom = Printer::currentPositionSteps[Z_AXIS] = (h3 + z) * Printer::axisStepsPerMM[Z_AXIS]; - Printer::zLength = Printer::runZMaxProbe() + zBottom * Printer::invAxisStepsPerMM[Z_AXIS] - ENDSTOP_Z_BACK_ON_HOME; -#endif - Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength); -#else // max hardware endstop -#if DRIVE_SYSTEM != DELTA - Printer::currentPositionSteps[Z_AXIS] = (h3 + z) * Printer::axisStepsPerMM[Z_AXIS]; -#endif -#endif - Printer::setAutolevelActive(true); - if(com->S == 2) - EEPROM::storeDataIntoEEPROM(); - } - else - { -#if DRIVE_SYSTEM != DELTA - Printer::currentPositionSteps[Z_AXIS] = (h3 + z) * Printer::axisStepsPerMM[Z_AXIS]; -#endif - if(com->hasS() && com->S == 3) - EEPROM::storeDataIntoEEPROM(); - } - Printer::setAutolevelActive(true); -#endif // defined(MOTORIZED_BED_LEVELING) - Printer::updateDerivedParameter(); - Printer::updateCurrentPosition(true); - printCurrentPosition(PSTR("G32 ")); -#if DRIVE_SYSTEM == DELTA - Printer::homeAxis(true, true, true); -#endif - Printer::feedrate = oldFeedrate; - } -#endif - break; -#endif -#endif -#if DISTORTION_CORRECTION - case 33: { - if(com->hasL()) { // G33 L0 - List distortion matrix - Printer::distortion.showMatrix(); - } else if(com->hasR()) { // G33 R0 - Reset distortion matrix - Printer::distortion.resetCorrection(); - } else if(com->hasX() || com->hasY() || com->hasZ()) { // G33 X Y Z - Set correction for nearest point - if(com->hasX() && com->hasY() && com->hasZ()) { - Printer::distortion.set(com->X, com->Y, com->Z); - } else { - Com::printErrorFLN(PSTR("You need to define X, Y and Z to set a point!")); - } - } else { // G33 - float oldFeedrate = Printer::feedrate; - Printer::measureDistortion(); - Printer::feedrate = oldFeedrate; - } - } - break; -#endif - case 90: // G90 - Printer::relativeCoordinateMode = false; - if(com->internalCommand) - Com::printInfoFLN(PSTR("Absolute positioning")); - break; - case 91: // G91 - Printer::relativeCoordinateMode = true; - if(com->internalCommand) - Com::printInfoFLN(PSTR("Relative positioning")); - break; - case 92: // G92 - { - float xOff = Printer::coordinateOffset[X_AXIS]; - float yOff = Printer::coordinateOffset[Y_AXIS]; - float zOff = Printer::coordinateOffset[Z_AXIS]; - if(com->hasX()) xOff = Printer::convertToMM(com->X) - Printer::currentPosition[X_AXIS]; - if(com->hasY()) yOff = Printer::convertToMM(com->Y) - Printer::currentPosition[Y_AXIS]; - if(com->hasZ()) zOff = Printer::convertToMM(com->Z) - Printer::currentPosition[Z_AXIS]; - Printer::setOrigin(xOff, yOff, zOff); - if(com->hasE()) - { - Printer::currentPositionSteps[E_AXIS] = Printer::convertToMM(com->E) * Printer::axisStepsPerMM[E_AXIS]; - } - } - break; -#if DRIVE_SYSTEM == DELTA - case 100: // G100 Calibrate floor or rod radius - { - // Using manual control, adjust hot end to contact floor. - // G100 No action. Avoid accidental floor reset. - // G100 [X] [Y] [Z] set floor for argument passed in. Number ignored and may be absent. - // G100 R with X Y or Z flag error, sets only floor or radius, not both. - // G100 R[n] Add n to radius. Adjust to be above floor if necessary - // G100 R[0] set radius based on current z measurement. Moves to (0,0,0) - float currentZmm = Printer::currentPosition[Z_AXIS]; - if (currentZmm/Printer::zLength > 0.1) - { - Com::printErrorFLN(PSTR("Calibration code is limited to bottom 10% of Z height")); - break; - } - if (com->hasR()) - { - if (com->hasX() || com->hasY() || com->hasZ()) - Com::printErrorFLN(PSTR("Cannot set radius and floor at same time.")); - else if (com->R != 0) - { - //add r to radius - if (abs(com->R) <= 10) EEPROM::incrementRodRadius(com->R); - else Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm.")); - } - else - { - // auto set radius. Head must be at 0,0 and touching - // Z offset will be corrected for. - if (Printer::currentPosition[X_AXIS] == 0 - && Printer::currentPosition[Y_AXIS] == 0) { - if(Printer::isLargeMachine()) - { - // calculate radius assuming we are at surface - // If Z is greater than 0 it will get calculated out for correct radius - // Use either A or B tower as they acnhor x cartesian axis and always have - // Radius distance to center in simplest set up. - float h = Printer::deltaDiagonalStepsSquaredB.f; - unsigned long bSteps = Printer::currentDeltaPositionSteps[B_TOWER]; - // The correct Rod Radius would put us here at z==0 and B height is - // square root (rod length squared minus rod radius squared) - // Reverse that to get calculated Rod Radius given B height - h -= RMath::sqr((float)bSteps); - h = sqrt(h); - EEPROM::setRodRadius(h*Printer::invAxisStepsPerMM[Z_AXIS]); - } - else - { - // calculate radius assuming we are at surface - // If Z is greater than 0 it will get calculated out for correct radius - // Use either A or B tower as they acnhor x cartesian axis and always have - // Radius distance to center in simplest set up. - unsigned long h = Printer::deltaDiagonalStepsSquaredB.l; - unsigned long bSteps = Printer::currentDeltaPositionSteps[B_TOWER]; - // The correct Rod Radius would put us here at z==0 and B height is - // square root (rod length squared minus rod radius squared) - // Reverse that to get calculated Rod Radius given B height - h -= RMath::sqr(bSteps); - h = SQRT(h); - EEPROM::setRodRadius(h*Printer::invAxisStepsPerMM[Z_AXIS]); - } + + InterruptProtectedBlock noInts; + int lc = (int)PrintLine::linesCount; + int lp = (int)PrintLine::linesPos; + int wp = (int)PrintLine::linesWritePos; + int n = (wp - lp); + if(n < 0) n += PRINTLINE_CACHE_SIZE; + noInts.unprotect(); + if(n != lc) + Com::printFLN(PSTR("Buffer corrupted")); } - else - Com::printErrorFLN(PSTR("First move to touch at x,y=0,0 to auto-set radius.")); - } - } - else - { - bool tooBig = false; - if (com->hasX()) - { - if (abs(com->X) <= 10) - EEPROM::setTowerXFloor(com->X + currentZmm + Printer::xMin); - else tooBig = true; - } - if (com->hasY()) - { - if (abs(com->Y) <= 10) - EEPROM::setTowerYFloor(com->Y + currentZmm + Printer::yMin); - else tooBig = true; - } - if (com->hasZ()) - { - if (abs(com->Z) <= 10) - EEPROM::setTowerZFloor(com->Z + currentZmm + Printer::zMin); - else tooBig = true; - } - if (tooBig) - Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm.")); - } - // after adjusting zero, physical position is out of sync with memory position - // this could cause jerky movement or push head into print surface. - // moving gets back into safe zero'ed position with respect to newle set floor or Radius. - Printer::moveTo(IGNORE_COORDINATE,IGNORE_COORDINATE,12.0,IGNORE_COORDINATE,IGNORE_COORDINATE); - break; - } - case 131: // G131 Remove offset - { - float cx,cy,cz; - Printer::realPosition(cx,cy,cz); - float oldfeedrate = Printer::feedrate; - Printer::offsetX = 0; - Printer::offsetY = 0; - Printer::moveToReal(cx,cy,cz,IGNORE_COORDINATE,Printer::homingFeedrate[X_AXIS]); - Printer::feedrate = oldfeedrate; - Printer::updateCurrentPosition(); - } - break; - case 132: // G132 Calibrate endstop offsets - { -// This has the probably unintended side effect of turning off leveling. - Printer::setAutolevelActive(false); // don't let transformations change result! - Printer::coordinateOffset[X_AXIS] = 0; - Printer::coordinateOffset[Y_AXIS] = 0; - Printer::coordinateOffset[Z_AXIS] = 0; -// I think this is coded incorrectly, as it depends on the biginning position of the -// of the hot end, and so should first move to x,y,z= 0,0,0, but as that may not -// be possible if the printer is not in the homes/zeroed state, the printer -// cannot safely move to 0 z coordinate without crashong into the print surface. -// so other than commenting, I'm not meddling. -// but you will always get different counts from different positions. - Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]); - int32_t m = RMath::max(Printer::stepsRemainingAtXHit,RMath::max(Printer::stepsRemainingAtYHit,Printer::stepsRemainingAtZHit)); - int32_t offx = m - Printer::stepsRemainingAtXHit; - int32_t offy = m - Printer::stepsRemainingAtYHit; - int32_t offz = m - Printer::stepsRemainingAtZHit; - Com::printFLN(Com::tTower1, offx); - Com::printFLN(Com::tTower2, offy); - Com::printFLN(Com::tTower3, offz); -#if EEPROM_MODE != 0 - if(com->hasS() && com->S > 0) - { - EEPROM::setDeltaTowerXOffsetSteps(offx); - EEPROM::setDeltaTowerYOffsetSteps(offy); - EEPROM::setDeltaTowerZOffsetSteps(offz); - } #endif - PrintLine::moveRelativeDistanceInSteps(0, 0, -5*Printer::axisStepsPerMM[Z_AXIS], 0, Printer::homingFeedrate[Z_AXIS], true, true); - Printer::homeAxis(true,true,true); - } - break; - case 133: // G133 Measure steps to top - { - bool oldAuto = Printer::isAutolevelActive(); - Printer::setAutolevelActive(false); // don't let transformations change result! - Printer::currentPositionSteps[X_AXIS] = 0; - Printer::currentPositionSteps[Y_AXIS] = 0; - Printer::currentPositionSteps[Z_AXIS] = 0; - Printer::coordinateOffset[X_AXIS] = 0; - Printer::coordinateOffset[Y_AXIS] = 0; - Printer::coordinateOffset[Z_AXIS] = 0; - Printer::currentDeltaPositionSteps[A_TOWER] = 0; - Printer::currentDeltaPositionSteps[B_TOWER] = 0; - Printer::currentDeltaPositionSteps[C_TOWER] = 0; -// similar to comment above, this will get a different answer from any different starting point -// so it is unclear how this is helpful. It must start at a well defined point. - Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]); - int32_t offx = HOME_DISTANCE_STEPS - Printer::stepsRemainingAtXHit; - int32_t offy = HOME_DISTANCE_STEPS - Printer::stepsRemainingAtYHit; - int32_t offz = HOME_DISTANCE_STEPS - Printer::stepsRemainingAtZHit; - Com::printFLN(Com::tTower1,offx); - Com::printFLN(Com::tTower2,offy); - Com::printFLN(Com::tTower3,offz); - Printer::setAutolevelActive(oldAuto); - PrintLine::moveRelativeDistanceInSteps(0, 0, Printer::axisStepsPerMM[Z_AXIS] * -ENDSTOP_Z_BACK_MOVE, 0, Printer::homingFeedrate[Z_AXIS] / ENDSTOP_X_RETEST_REDUCTION_FACTOR, true, false); - Printer::homeAxis(true,true,true); - } - break; - case 135: // G135 - Com::printF(PSTR("CompDelta:"),Printer::currentDeltaPositionSteps[A_TOWER]); - Com::printF(Com::tComma,Printer::currentDeltaPositionSteps[B_TOWER]); - Com::printFLN(Com::tComma,Printer::currentDeltaPositionSteps[C_TOWER]); -#ifdef DEBUG_REAL_POSITION - Com::printF(PSTR("RealDelta:"),Printer::realDeltaPositionSteps[A_TOWER]); - Com::printF(Com::tComma,Printer::realDeltaPositionSteps[B_TOWER]); - Com::printFLN(Com::tComma,Printer::realDeltaPositionSteps[C_TOWER]); -#endif - Printer::updateCurrentPosition(); - Com::printF(PSTR("PosFromSteps:")); - printCurrentPosition(PSTR("G134 ")); - break; - -#endif // DRIVE_SYSTEM -#if FEATURE_Z_PROBE - case 134: // - G134 Px Sx Zx - Calibrate nozzle height difference (need z probe in nozzle!) Px = reference extruder, Sx = only measure extrude x against reference, Zx = add to measured z distance for Sx for correction. - { - float z = com->hasZ() ? com->Z : 0; - int p = com->hasP() ? com->P : 0; - int s = com->hasS() ? com->S : -1; - extruder[p].zOffset = 0; - Extruder::selectExtruderById(p); - float refHeight = Printer::runZProbe(true,false,true); - for(int i = 0;i < NUM_EXTRUDER;i++) { - if(i == p) continue; - if(s >= 0 && i != s) continue; - extruder[i].zOffset = 0; - Extruder::selectExtruderById(i); - float height = Printer::runZProbe(false,false); - extruder[i].zOffset = (height - refHeight + z)*Printer::axisStepsPerMM[Z_AXIS]; - } - Extruder::selectExtruderById(p); - Printer::runZProbe(false,true); -#if EEPROM_MODE != 0 - EEPROM::storeDataIntoEEPROM(0); -#endif - } - break; -#endif -#if defined(NUM_MOTOR_DRIVERS) && NUM_MOTOR_DRIVERS > 0 - case 201: - commandG201(*com); - break; - case 202: - commandG202(*com); - break; - case 203: - commandG203(*com); - break; - case 204: - commandG204(*com); - break; -#endif // defined - default: - if(!EVENT_UNHANDLED_G_CODE(com) && Printer::debugErrors()) - { - Com::printF(Com::tUnknownCommand); - com->printCommand(); - } - } - previousMillisCmd = HAL::timeInMilliseconds(); -} -/** - \brief Execute the G command stored in com. -*/ -void Commands::processMCode(GCode *com) -{ - switch( com->M ) - { - case 3: // Spindle/laser on #if defined(SUPPORT_LASER) && SUPPORT_LASER - if(Printer::mode == PRINTER_MODE_LASER) { - if(com->hasS()) - LaserDriver::intensity = constrain(com->S,0,255); - LaserDriver::laserOn = true; - Com::printFLN(PSTR("LaserOn:"),(int)LaserDriver::intensity); - } -#endif // defined -#if defined(SUPPORT_CNC) && SUPPORT_CNC - if(Printer::mode == PRINTER_MODE_CNC) { - waitUntilEndOfAllMoves(); - CNCDriver::spindleOnCW(com->hasS() ? com->S : 0); - } -#endif // defined - break; - case 4: // Spindle CCW -#if defined(SUPPORT_CNC) && SUPPORT_CNC - if(Printer::mode == PRINTER_MODE_CNC) { - waitUntilEndOfAllMoves(); - CNCDriver::spindleOnCCW(com->hasS() ? com->S : 0); - } -#endif // defined - break; - case 5: // Spindle/laser off -#if defined(SUPPORT_LASER) && SUPPORT_LASER - if(Printer::mode == PRINTER_MODE_LASER) { - LaserDriver::laserOn = false; - } -#endif // defined -#if defined(SUPPORT_CNC) && SUPPORT_CNC - if(Printer::mode == PRINTER_MODE_CNC) { - waitUntilEndOfAllMoves(); - CNCDriver::spindleOff(); - } -#endif // defined - break; -#if SDSUPPORT - case 20: // M20 - list SD card -#if JSON_OUTPUT - if (com->hasString() && com->text[1] == '2') { // " S2 P/folder" - if (com->text[3] == 'P') { - sd.lsJSON(com->text + 4); + LaserDriver::laserOn = laserOn; } - } else sd.ls(); +#endif // defined + break; +#if ARC_SUPPORT + case 2: // CW Arc + case 3: // CCW Arc MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: +#if defined(SUPPORT_LASER) && SUPPORT_LASER + { + // disable laser for G0 moves + bool laserOn = LaserDriver::laserOn; + if(com->G == 0 && Printer::mode == PRINTER_MODE_LASER) { + LaserDriver::laserOn = false; + } +#endif // defined + processArc(com); +#if defined(SUPPORT_LASER) && SUPPORT_LASER + LaserDriver::laserOn = laserOn; + } +#endif // defined + break; +#endif + case 4: // G4 dwell + Commands::waitUntilEndOfAllMoves(); + codenum = 0; + if(com->hasP()) codenum = com->P; // milliseconds to wait + if(com->hasS()) codenum = com->S * 1000; // seconds to wait + codenum += HAL::timeInMilliseconds(); // keep track of when we started waiting + while((uint32_t)(codenum-HAL::timeInMilliseconds()) < 2000000000 ) { + GCode::readFromSerial(); + Commands::checkForPeriodicalActions(true); + } + break; +#if FEATURE_RETRACTION && NUM_EXTRUDER > 0 + case 10: // G10 S<1 = long retract, 0 = short retract = default> retracts filament accoridng to stored setting +#if NUM_EXTRUDER > 1 + Extruder::current->retract(true, com->hasS() && com->S > 0); #else - sd.ls(); + Extruder::current->retract(true, false); #endif - break; - case 21: // M21 - init SD card - sd.mount(); - break; - case 22: //M22 - release SD card - sd.unmount(); - break; - case 23: //M23 - Select file - if(com->hasString()) - { - sd.fat.chdir(); - sd.selectFile(com->text); - } - break; - case 24: //M24 - Start SD print - sd.startPrint(); - break; - case 25: //M25 - Pause SD print - sd.pausePrint(); - break; - case 26: //M26 - Set SD index - if(com->hasS()) - sd.setIndex(com->S); - break; - case 27: //M27 - Get SD status - sd.printStatus(); - break; - case 28: //M28 - Start SD write - if(com->hasString()) - sd.startWrite(com->text); - break; - case 29: //M29 - Stop SD write - //processed in write to file routine above - //savetosd = false; - break; - case 30: // M30 filename - Delete file - if(com->hasString()) - { - sd.fat.chdir(); - sd.deleteFile(com->text); - } - break; - case 32: // M32 directoryname - if(com->hasString()) - { - sd.fat.chdir(); - sd.makeDirectory(com->text); - } - break; + break; + case 11: // G11 S<1 = long retract, 0 = short retract = default> = Undo retraction according to stored setting +#if NUM_EXTRUDER > 1 + Extruder::current->retract(false, com->hasS() && com->S > 0); +#else + Extruder::current->retract(false, false); #endif -#if JSON_OUTPUT && SDSUPPORT - case 36: // M36 JSON File Info - if (com->hasString()) { - sd.JSONFileInfo(com->text); - } - break; + break; +#endif // FEATURE_RETRACTION + case 20: // G20 Units to inches + Printer::unitIsInches = 1; + break; + case 21: // G21 Units to mm + Printer::unitIsInches = 0; + break; + case 28: { //G28 Home all Axis one at a time +#if defined(SUPPORT_LASER) && SUPPORT_LASER + bool oldLaser = LaserDriver::laserOn; + LaserDriver::laserOn = false; +#endif + uint8_t homeAllAxis = (com->hasNoXYZ() && !com->hasE()); + if(com->hasE()) + Printer::currentPositionSteps[E_AXIS] = 0; + if(homeAllAxis || !com->hasNoXYZ()) + Printer::homeAxis(homeAllAxis || com->hasX(),homeAllAxis || com->hasY(),homeAllAxis || com->hasZ()); +#if defined(SUPPORT_LASER) && SUPPORT_LASER + LaserDriver::laserOn = oldLaser; #endif - case 42: //M42 -Change pin status via gcode - if (com->hasP()) - { - int pin_number = com->P; - for(uint8_t i = 0; i < (uint8_t)sizeof(sensitive_pins); i++) - { - if (pgm_read_byte(&sensitive_pins[i]) == pin_number) - { - pin_number = -1; + Printer::updateCurrentPosition(); + } + break; +#if FEATURE_Z_PROBE + case 29: { // G29 3 points, build average or distortion compensation +#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::zProbeHeight(),static_cast(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(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(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(ZPROBE_MIN_TEMPERATURE)),Extruder::current->id,false,true); +#endif +#endif + bool ok = true; + Printer::startProbing(true); + bool oldAutolevel = Printer::isAutolevelActive(); + Printer::setAutolevelActive(false); + float sum = 0, last,oldFeedrate = Printer::feedrate; + Printer::moveTo(EEPROM::zProbeX1(), EEPROM::zProbeY1(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed()); + sum = Printer::runZProbe(true,false,Z_PROBE_REPETITIONS,false); + if(sum == ILLEGAL_Z_PROBE) ok = false; + if(ok) { + Printer::moveTo(EEPROM::zProbeX2(), EEPROM::zProbeY2(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed()); + last = Printer::runZProbe(false,false); + if(last == ILLEGAL_Z_PROBE) ok = false; + sum+= last; + } + if(ok) { + Printer::moveTo(EEPROM::zProbeX3(), EEPROM::zProbeY3(), IGNORE_COORDINATE, IGNORE_COORDINATE, EEPROM::zProbeXYSpeed()); + last = Printer::runZProbe(false,true); + if(last == ILLEGAL_Z_PROBE) ok = false; + sum += last; + } + if(ok) { + sum *= 0.33333333333333; + Com::printFLN(Com::tZProbeAverage, sum); + if(com->hasS() && com->S) { +#if MAX_HARDWARE_ENDSTOP_Z +#if DRIVE_SYSTEM == DELTA + Printer::updateCurrentPosition(); + Printer::zLength += sum - Printer::currentPosition[Z_AXIS]; + Printer::updateDerivedParameter(); + Printer::homeAxis(true,true,true); +#else + Printer::currentPositionSteps[Z_AXIS] = sum * Printer::axisStepsPerMM[Z_AXIS]; + float zup = Printer::runZMaxProbe(); + if(zup == ILLEGAL_Z_PROBE) { + ok = false; + } else + Printer::zLength = zup + sum - ENDSTOP_Z_BACK_ON_HOME; +#endif // DELTA + Com::printInfoFLN(Com::tZProbeZReset); + Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength); +#else + Printer::currentPositionSteps[Z_AXIS] = sum * Printer::axisStepsPerMM[Z_AXIS]; + Com::printFLN(PSTR("Adjusted z origin")); +#endif // max z endstop + } + Printer::feedrate = oldFeedrate; + Printer::setAutolevelActive(oldAutolevel); + if(ok && com->hasS() && com->S == 2) + EEPROM::storeDataIntoEEPROM(); + } + Printer::updateCurrentPosition(true); + printCurrentPosition(PSTR("G29 ")); + Printer::finishProbing(); + Printer::feedrate = oldFeedrate; + if(!ok) { + GCode::fatalError(PSTR("G29 leveling failed!")); + break; + } +#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(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(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(ZPROBE_MIN_TEMPERATURE)),Extruder::current->id,false,true); +#endif +#endif + } + break; + case 30: + { // G30 single probe set Z0 + uint8_t p = (com->hasP() ? (uint8_t)com->P : 3); + if(Printer::runZProbe(p & 1,p & 2) == ILLEGAL_Z_PROBE) { + GCode::fatalError(PSTR("G29 leveling failed!")); + break; + } + Printer::updateCurrentPosition(p & 1); + } + break; + case 31: // G31 display hall sensor output + Endstops::update(); + Endstops::update(); + Com::printF(Com::tZProbeState); + Com::printF(Endstops::zProbe() ? Com::tHSpace : Com::tLSpace); + Com::println(); + break; +#if FEATURE_AUTOLEVEL + case 32: // G32 Auto-Bed leveling + { +#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::zProbeHeight(),static_cast(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(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(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(ZPROBE_MIN_TEMPERATURE)),Extruder::current->id,false,true); +#endif +#endif + if(!runBedLeveling(com)) { + GCode::fatalError(PSTR("G32 leveling failed!")); + break; + } +#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(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(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(ZPROBE_MIN_TEMPERATURE)),Extruder::current->id,false,true); +#endif +#endif + } + break; +#endif +#if DISTORTION_CORRECTION + case 33: { + if(com->hasL()) { // G33 L0 - List distortion matrix + Printer::distortion.showMatrix(); + } else if(com->hasR()) { // G33 R0 - Reset distortion matrix + Printer::distortion.resetCorrection(); + } else if(com->hasX() || com->hasY() || com->hasZ()) { // G33 X Y Z - Set correction for nearest point + if(com->hasX() && com->hasY() && com->hasZ()) { + Printer::distortion.set(com->X, com->Y, com->Z); + } else { + Com::printErrorFLN(PSTR("You need to define X, Y and Z to set a point!")); + } + } else { // G33 +#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::zProbeHeight(),static_cast(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(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(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(ZPROBE_MIN_TEMPERATURE)),Extruder::current->id,false,true); +#endif +#endif + float oldFeedrate = Printer::feedrate; + if(!Printer::measureDistortion()) { + GCode::fatalError(PSTR("G33 failed!")); + break; + } + 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(actTemp[i],i,false,false); + for(int i = 0; i < NUM_EXTRUDER; i++) + Extruder::setTemperatureForExtruder(actTemp[i],i,false, actTemp[i] > MAX_ROOM_TEMPERATURE); +#else + Extruder::setTemperatureForExtruder(actTemp[Extruder::current->id], Extruder::current->id, false, actTemp[Extruder::current->id] > MAX_ROOM_TEMPERATURE); +#endif +#endif + } + } + break; +#endif +#endif + case 90: // G90 + Printer::relativeCoordinateMode = false; + if(com->internalCommand) + Com::printInfoFLN(PSTR("Absolute positioning")); + break; + case 91: // G91 + Printer::relativeCoordinateMode = true; + if(com->internalCommand) + Com::printInfoFLN(PSTR("Relative positioning")); + break; + case 92: { // G92 + float xOff = Printer::coordinateOffset[X_AXIS]; + float yOff = Printer::coordinateOffset[Y_AXIS]; + float zOff = Printer::coordinateOffset[Z_AXIS]; + if(com->hasX()) xOff = Printer::convertToMM(com->X) - Printer::currentPosition[X_AXIS]; + if(com->hasY()) yOff = Printer::convertToMM(com->Y) - Printer::currentPosition[Y_AXIS]; + if(com->hasZ()) zOff = Printer::convertToMM(com->Z) - Printer::currentPosition[Z_AXIS]; + Printer::setOrigin(xOff, yOff, zOff); + if(com->hasE()) { + Printer::currentPositionSteps[E_AXIS] = Printer::convertToMM(com->E) * Printer::axisStepsPerMM[E_AXIS]; + } + } + break; +#if DRIVE_SYSTEM == DELTA + case 100: { // G100 Calibrate floor or rod radius + // Using manual control, adjust hot end to contact floor. + // G100 No action. Avoid accidental floor reset. + // G100 [X] [Y] [Z] set floor for argument passed in. Number ignored and may be absent. + // G100 R with X Y or Z flag error, sets only floor or radius, not both. + // G100 R[n] Add n to radius. Adjust to be above floor if necessary + // G100 R[0] set radius based on current z measurement. Moves to (0,0,0) + float currentZmm = Printer::currentPosition[Z_AXIS]; + if (currentZmm/Printer::zLength > 0.1) { + Com::printErrorFLN(PSTR("Calibration code is limited to bottom 10% of Z height")); break; } - } - if (pin_number > -1) - { - if(com->hasS()) - { - if(com->S >= 0 && com->S <= 255) - { - pinMode(pin_number, OUTPUT); - digitalWrite(pin_number, com->S); - analogWrite(pin_number, com->S); - Com::printF(Com::tSetOutputSpace, pin_number); - Com::printFLN(Com::tSpaceToSpace,(int)com->S); + if (com->hasR()) { + if (com->hasX() || com->hasY() || com->hasZ()) + Com::printErrorFLN(PSTR("Cannot set radius and floor at same time.")); + else if (com->R != 0) { + //add r to radius + if (abs(com->R) <= 10) EEPROM::incrementRodRadius(com->R); + else Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm.")); + } else { + // auto set radius. Head must be at 0,0 and touching + // Z offset will be corrected for. + if (Printer::currentPosition[X_AXIS] == 0 + && Printer::currentPosition[Y_AXIS] == 0) { + if(Printer::isLargeMachine()) { + // calculate radius assuming we are at surface + // If Z is greater than 0 it will get calculated out for correct radius + // Use either A or B tower as they acnhor x cartesian axis and always have + // Radius distance to center in simplest set up. + float h = Printer::deltaDiagonalStepsSquaredB.f; + unsigned long bSteps = Printer::currentNonlinearPositionSteps[B_TOWER]; + // The correct Rod Radius would put us here at z==0 and B height is + // square root (rod length squared minus rod radius squared) + // Reverse that to get calculated Rod Radius given B height + h -= RMath::sqr((float)bSteps); + h = sqrt(h); + EEPROM::setRodRadius(h*Printer::invAxisStepsPerMM[Z_AXIS]); + } else { + // calculate radius assuming we are at surface + // If Z is greater than 0 it will get calculated out for correct radius + // Use either A or B tower as they acnhor x cartesian axis and always have + // Radius distance to center in simplest set up. + unsigned long h = Printer::deltaDiagonalStepsSquaredB.l; + unsigned long bSteps = Printer::currentNonlinearPositionSteps[B_TOWER]; + // The correct Rod Radius would put us here at z==0 and B height is + // square root (rod length squared minus rod radius squared) + // Reverse that to get calculated Rod Radius given B height + h -= RMath::sqr(bSteps); + h = SQRT(h); + EEPROM::setRodRadius(h*Printer::invAxisStepsPerMM[Z_AXIS]); + } + } else + Com::printErrorFLN(PSTR("First move to touch at x,y=0,0 to auto-set radius.")); } - else - Com::printErrorFLN(PSTR("Illegal S value for M42")); + } else { + bool tooBig = false; + if (com->hasX()) { + if (abs(com->X) <= 10) + EEPROM::setTowerXFloor(com->X + currentZmm + Printer::xMin); + else tooBig = true; + } + if (com->hasY()) { + if (abs(com->Y) <= 10) + EEPROM::setTowerYFloor(com->Y + currentZmm + Printer::yMin); + else tooBig = true; + } + if (com->hasZ()) { + if (abs(com->Z) <= 10) + EEPROM::setTowerZFloor(com->Z + currentZmm + Printer::zMin); + else tooBig = true; + } + if (tooBig) + Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm.")); } - else - { - pinMode(pin_number, INPUT_PULLUP); - Com::printF(Com::tSpaceToSpace, pin_number); - Com::printFLN(Com::tSpaceIsSpace, digitalRead(pin_number)); + // after adjusting zero, physical position is out of sync with memory position + // this could cause jerky movement or push head into print surface. + // moving gets back into safe zero'ed position with respect to newle set floor or Radius. + Printer::moveTo(IGNORE_COORDINATE,IGNORE_COORDINATE,12.0,IGNORE_COORDINATE,IGNORE_COORDINATE); + break; + } + case 131: { // G131 Remove offset + float cx,cy,cz; + Printer::realPosition(cx,cy,cz); + float oldfeedrate = Printer::feedrate; + Printer::offsetX = 0; + Printer::offsetY = 0; + Printer::moveToReal(cx,cy,cz,IGNORE_COORDINATE,Printer::homingFeedrate[X_AXIS]); + Printer::feedrate = oldfeedrate; + Printer::updateCurrentPosition(); + } + break; + case 132: { // G132 Calibrate endstop offsets + // This has the probably unintended side effect of turning off leveling. + Printer::setAutolevelActive(false); // don't let transformations change result! + Printer::coordinateOffset[X_AXIS] = 0; + Printer::coordinateOffset[Y_AXIS] = 0; + Printer::coordinateOffset[Z_AXIS] = 0; + // I think this is coded incorrectly, as it depends on the biginning position of the + // of the hot end, and so should first move to x,y,z= 0,0,0, but as that may not + // be possible if the printer is not in the homes/zeroed state, the printer + // cannot safely move to 0 z coordinate without crashong into the print surface. + // so other than commenting, I'm not meddling. + // but you will always get different counts from different positions. + Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]); + int32_t m = RMath::max(Printer::stepsRemainingAtXHit,RMath::max(Printer::stepsRemainingAtYHit,Printer::stepsRemainingAtZHit)); + int32_t offx = m - Printer::stepsRemainingAtXHit; + int32_t offy = m - Printer::stepsRemainingAtYHit; + int32_t offz = m - Printer::stepsRemainingAtZHit; + Com::printFLN(Com::tTower1, offx); + Com::printFLN(Com::tTower2, offy); + Com::printFLN(Com::tTower3, offz); +#if EEPROM_MODE != 0 + if(com->hasS() && com->S > 0) { + EEPROM::setDeltaTowerXOffsetSteps(offx); + EEPROM::setDeltaTowerYOffsetSteps(offy); + EEPROM::setDeltaTowerZOffsetSteps(offz); } +#endif + PrintLine::moveRelativeDistanceInSteps(0, 0, -5*Printer::axisStepsPerMM[Z_AXIS], 0, Printer::homingFeedrate[Z_AXIS], true, true); + Printer::homeAxis(true,true,true); } - else - { - Com::printErrorFLN(PSTR("Pin can not be set by M42, is in sensitive pins! ")); + break; + case 133: { // G133 Measure steps to top + bool oldAuto = Printer::isAutolevelActive(); + Printer::setAutolevelActive(false); // don't let transformations change result! + Printer::currentPositionSteps[X_AXIS] = 0; + Printer::currentPositionSteps[Y_AXIS] = 0; + Printer::currentPositionSteps[Z_AXIS] = 0; + Printer::coordinateOffset[X_AXIS] = 0; + Printer::coordinateOffset[Y_AXIS] = 0; + Printer::coordinateOffset[Z_AXIS] = 0; + Printer::currentNonlinearPositionSteps[A_TOWER] = 0; + Printer::currentNonlinearPositionSteps[B_TOWER] = 0; + Printer::currentNonlinearPositionSteps[C_TOWER] = 0; + // similar to comment above, this will get a different answer from any different starting point + // so it is unclear how this is helpful. It must start at a well defined point. + Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]); + int32_t offx = HOME_DISTANCE_STEPS - Printer::stepsRemainingAtXHit; + int32_t offy = HOME_DISTANCE_STEPS - Printer::stepsRemainingAtYHit; + int32_t offz = HOME_DISTANCE_STEPS - Printer::stepsRemainingAtZHit; + Com::printFLN(Com::tTower1,offx); + Com::printFLN(Com::tTower2,offy); + Com::printFLN(Com::tTower3,offz); + Printer::setAutolevelActive(oldAuto); + PrintLine::moveRelativeDistanceInSteps(0, 0, Printer::axisStepsPerMM[Z_AXIS] * -ENDSTOP_Z_BACK_MOVE, 0, Printer::homingFeedrate[Z_AXIS] / ENDSTOP_X_RETEST_REDUCTION_FACTOR, true, false); + Printer::homeAxis(true,true,true); } - } - break; - case 80: // M80 - ATX Power On -#if PS_ON_PIN>-1 - Commands::waitUntilEndOfAllMoves(); - previousMillisCmd = HAL::timeInMilliseconds(); - SET_OUTPUT(PS_ON_PIN); //GND - Printer::setPowerOn(true); - WRITE(PS_ON_PIN, (POWER_INVERTING ? HIGH : LOW)); + break; + case 135: // G135 + Com::printF(PSTR("CompDelta:"),Printer::currentNonlinearPositionSteps[A_TOWER]); + Com::printF(Com::tComma,Printer::currentNonlinearPositionSteps[B_TOWER]); + Com::printFLN(Com::tComma,Printer::currentNonlinearPositionSteps[C_TOWER]); +#ifdef DEBUG_REAL_POSITION + Com::printF(PSTR("RealDelta:"),Printer::realDeltaPositionSteps[A_TOWER]); + Com::printF(Com::tComma,Printer::realDeltaPositionSteps[B_TOWER]); + Com::printFLN(Com::tComma,Printer::realDeltaPositionSteps[C_TOWER]); #endif - break; - case 81: // M81 - ATX Power Off -#if PS_ON_PIN>-1 - Commands::waitUntilEndOfAllMoves(); - SET_OUTPUT(PS_ON_PIN); //GND - Printer::setPowerOn(false); - WRITE(PS_ON_PIN,(POWER_INVERTING ? LOW : HIGH)); -#endif - break; - case 82: // M82 - Printer::relativeExtruderCoordinateMode = false; - break; - case 83: // M83 - Printer::relativeExtruderCoordinateMode = true; - break; - case 84: // M84 - if(com->hasS()) - { - stepperInactiveTime = com->S * 1000; - } - else - { - Commands::waitUntilEndOfAllMoves(); - Printer::kill(true); - } - break; - case 85: // M85 - if(com->hasS()) - maxInactiveTime = (int32_t)com->S * 1000; - else - maxInactiveTime = 0; - break; - case 92: // M92 - if(com->hasX()) Printer::axisStepsPerMM[X_AXIS] = com->X; - if(com->hasY()) Printer::axisStepsPerMM[Y_AXIS] = com->Y; - if(com->hasZ()) Printer::axisStepsPerMM[Z_AXIS] = com->Z; - Printer::updateDerivedParameter(); - if(com->hasE()) - { - Extruder::current->stepsPerMM = com->E; - Extruder::selectExtruderById(Extruder::current->id); - } - break; - case 99: // M99 S