diff --git a/Direct-Calibrate.pdf b/Direct-Calibrate.pdf new file mode 100644 index 000000000..e981c5b5f Binary files /dev/null and b/Direct-Calibrate.pdf differ diff --git a/src/ArduinoAVR/Repetier/Commands.cpp b/src/ArduinoAVR/Repetier/Commands.cpp index 3998200c5..539968996 100644 --- a/src/ArduinoAVR/Repetier/Commands.cpp +++ b/src/ArduinoAVR/Repetier/Commands.cpp @@ -126,14 +126,19 @@ void Commands::waitUntilEndOfAllBuffers() #endif Commands::executeGCode(code); code->popCurrentCommand(); - } Commands::checkForPeriodicalActions(); + } + Commands::checkForPeriodicalActions(); UI_MEDIUM; } } -void Commands::printCurrentPosition() +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)) + { + Com::printErrorFLN(s); // flag where the error condition came from + } x += Printer::coordinateOffset[X_AXIS]; y += Printer::coordinateOffset[Y_AXIS]; z += Printer::coordinateOffset[Z_AXIS]; @@ -418,1096 +423,1236 @@ void microstepInit() #endif } - - /** - \brief Execute the command stored in com. + \brief Execute the Arc command stored in com. */ -void Commands::executeGCode(GCode *com) -{ - uint32_t codenum; //throw away variable -#ifdef INCLUDE_DEBUG_COMMUNICATION - if(Printer::debugCommunication()) - { - if(com->hasG() || (com->hasM() && com->M!=111)) - { - previousMillisCmd = HAL::timeInMilliseconds(); - return; - } - } -#endif - if(com->hasG()) - { - switch(com->G) - { - case 0: // G0 -> G1 - case 1: // G1 - if(com->hasS()) - Printer::setNoDestinationCheck(com->S!=0); - if(Printer::setDestinationStepsFromGCode(com)) // For X Y Z E F -#if NONLINEAR_SYSTEM - PrintLine::queueDeltaMove(ALWAYS_CHECK_ENDSTOPS, true, true); -#else - PrintLine::queueCartesianMove(ALWAYS_CHECK_ENDSTOPS,true); -#endif - break; #if ARC_SUPPORT - case 2: // CW Arc - case 3: // CCW Arc MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: - { - float position[Z_AXIS_ARRAY]; - Printer::realPosition(position[X_AXIS],position[Y_AXIS],position[Z_AXIS]); - if(!Printer::setDestinationStepsFromGCode(com)) break; // 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()) - { - /* - 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 - - 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 - - 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 - - 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 - - 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 - - */ - r = Printer::convertToMM(com->R); - // Calculate the change in position along each selected axis - double x = target[X_AXIS]-position[X_AXIS]; - double y = target[Y_AXIS]-position[Y_AXIS]; - - 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)) - { - Com::printErrorFLN(Com::tInvalidArc); - break; - } - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (com->G==3) - { - h_x2_div_d = -h_x2_div_d; - } +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()) + { + /* + 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. - /* 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. + 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 ---->| - T <-- Target position + 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 - ^ - 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 - | - | + Expanding the equations: - C <-- Current position */ + 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: - // 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) - { - h_x2_div_d = -h_x2_div_d; - r = -r; // Finished with r. Set to positive for mc_arc - } - // Complete the operation by calculating the actual center of the arc - offset[0] = 0.5*(x-(y*h_x2_div_d)); - offset[1] = 0.5*(y+(x*h_x2_div_d)); + 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 - } - else // Offset mode specific computations - { - r = hypot(offset[0], offset[1]); // Compute arc radius for arc - } - // Set clockwise/counter-clockwise sign for arc computations - uint8_t isclockwise = com->G == 2; - // Trace the arc - PrintLine::arc(position, target, offset,r, isclockwise); + Which we for size and speed reasons optimize to: - break; - } + 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); + // Calculate the change in position along each selected axis + double x = target[X_AXIS]-position[X_AXIS]; + double y = target[Y_AXIS]-position[Y_AXIS]; + + 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)) + { + 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) + { + 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. + + + 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! + \ | / + 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 */ + + + // 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) + { + h_x2_div_d = -h_x2_div_d; + r = -r; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + 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 + { + r = hypot(offset[0], offset[1]); // Compute arc radius for arc + } + // Set clockwise/counter-clockwise sign for arc computations + uint8_t isclockwise = com->G == 2; + // Trace the arc + PrintLine::arc(position, target, offset,r, isclockwise); +} #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(); - } - break; - case 20: // Units to inches - Printer::unitIsInches = 1; - break; - case 21: // Units to mm - Printer::unitIsInches = 0; - break; - case 28: //G28 Home all Axis one at a time - { - uint8_t home_all_axis = (com->hasNoXYZ() && !com->hasE()); - if(com->hasE()) + +/** + \brief Execute the G command stored in com. +*/ +void Commands::processGCode(GCode *com) +{ +uint32_t codenum; //throw away variable +switch(com->G) +{ + case 0: // G0 -> G1 + case 1: // G1 + 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)) { - Printer::currentPositionSteps[E_AXIS] = 0; - } - if(home_all_axis || !com->hasNoXYZ()) - Printer::homeAxis(home_all_axis || com->hasX(),home_all_axis || com->hasY(),home_all_axis || com->hasZ()); - Printer::updateCurrentPosition(); - } - break; + Com::printWarningFLN(PSTR("executeGCode / queueDeltaMove returns error")); + } +#else + PrintLine::queueCartesianMove(ALWAYS_CHECK_ENDSTOPS,true); +#endif + break; +#if ARC_SUPPORT + case 2: // CW Arc + case 3: // CCW Arc MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: + processArc(com); + 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(); + } + break; + 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 home_all_axis = (com->hasNoXYZ() && !com->hasE()); + if(com->hasE()) + { + Printer::currentPositionSteps[E_AXIS] = 0; + } + if(home_all_axis || !com->hasNoXYZ()) + Printer::homeAxis(home_all_axis || com->hasX(),home_all_axis || com->hasY(),home_all_axis || com->hasZ()); + Printer::updateCurrentPosition(); + } + break; #if FEATURE_Z_PROBE - case 29: // 3 points, build average - { - 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<0) break; - Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - last = Printer::runZProbe(false,false); - if(last<0) break; - sum+= last; - Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - last = Printer::runZProbe(false,true); - if(last<0) break; - sum+= last; - sum *= 0.33333333333333; - Com::printFLN(Com::tZProbeAverage,sum); - if(com->hasS() && com->S) - { + case 29: // G29 3 points, build average + { + 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<0) break; + Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); + last = Printer::runZProbe(false,false); + if(last<0) break; + sum+= last; + Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); + last = Printer::runZProbe(false,true); + if(last<0) 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); + 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; + 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); + 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(); - } - 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(); - } - break; - case 31: // G31 display hall sensor output - Com::printF(Com::tZProbeState); - Com::print(Printer::isZProbeHit() ? 'H' : 'L'); - Com::println(); - break; + 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 ")); + } + 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 + Com::printF(Com::tZProbeState); + Com::print(Printer::isZProbeHit() ? 'H' : 'L'); + Com::println(); + break; #if FEATURE_AUTOLEVEL - case 32: // G32 Auto-Bed leveling - { - GCode::executeFString(Com::tZProbeStartScript); - //bool iterate = com->hasP() && com->P>0; - Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0; - Printer::setAutolevelActive(false); // iterate - 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<0) break; - Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h2 = Printer::runZProbe(false,false); - if(h2<0) break; - Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); - h3 = Printer::runZProbe(false,true); - if(h3<0) break; - 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) - { + case 32: // G32 Auto-Bed leveling + { + GCode::executeFString(Com::tZProbeStartScript); + //bool iterate = com->hasP() && com->P>0; + Printer::coordinateOffset[X_AXIS] = Printer::coordinateOffset[Y_AXIS] = Printer::coordinateOffset[Z_AXIS] = 0; + Printer::setAutolevelActive(false); // iterate + 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<0) break; + Printer::moveTo(EEPROM::zProbeX2(),EEPROM::zProbeY2(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); + h2 = Printer::runZProbe(false,false); + if(h2<0) break; + Printer::moveTo(EEPROM::zProbeX3(),EEPROM::zProbeY3(),IGNORE_COORDINATE,IGNORE_COORDINATE,EEPROM::zProbeXYSpeed()); + h3 = Printer::runZProbe(false,true); + if(h3<0) break; + 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) + { #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]; + /* 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[2]-ENDSTOP_Z_BACK_ON_HOME; + int32_t zBottom = Printer::currentPositionSteps[Z_AXIS] = (h3+z)*Printer::axisStepsPerMM[Z_AXIS]; + Printer::zLength = Printer::runZMaxProbe()+zBottom*Printer::invAxisStepsPerMM[2]-ENDSTOP_Z_BACK_ON_HOME; #endif - Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength); + Com::printFLN(Com::tZProbePrinterHeight,Printer::zLength); #else #if DRIVE_SYSTEM!=DELTA - Printer::currentPositionSteps[Z_AXIS] = (h3+z)*Printer::axisStepsPerMM[Z_AXIS]; + Printer::currentPositionSteps[Z_AXIS] = (h3+z)*Printer::axisStepsPerMM[Z_AXIS]; #endif #endif - Printer::setAutolevelActive(true); - if(com->S == 2) - EEPROM::storeDataIntoEEPROM(); - } - Printer::setAutolevelActive(true); - Printer::updateDerivedParameter(); - Printer::updateCurrentPosition(true); - printCurrentPosition(); + Printer::setAutolevelActive(true); + if(com->S == 2) + EEPROM::storeDataIntoEEPROM(); + } + Printer::setAutolevelActive(true); + Printer::updateDerivedParameter(); + Printer::updateCurrentPosition(true); + printCurrentPosition(PSTR("G32 ")); #if DRIVE_SYSTEM==DELTA - Printer::homeAxis(true,true,true); -#endif - Printer::feedrate = oldFeedrate; - } - break; -#endif -#endif - case 90: // G90 - Printer::relativeCoordinateMode = false; - break; - case 91: // G91 - Printer::relativeCoordinateMode = true; - 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; + Printer::homeAxis(true,true,true); +#endif + Printer::feedrate = oldFeedrate; + } + break; +#endif +#endif + case 90: // G90 + Printer::relativeCoordinateMode = false; + break; + case 91: // G91 + Printer::relativeCoordinateMode = true; + 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 131: // Remove offset + 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::cartesianZMaxMM > 0.1) { - 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: // Calibrate endstop offsets + Com::printErrorFLN(PSTR("Calibration code is limited to bottom 10% of Z height")); + break; + } + if (com->hasR()) { - Printer::setAutolevelActive(false); // don't let transformations change result! - Printer::coordinateOffset[X_AXIS] = 0; - Printer::coordinateOffset[Y_AXIS] = 0; - Printer::coordinateOffset[Z_AXIS] = 0; - 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) + if (com->hasX() || com->hasY() || com->hasZ()) + Com::printErrorFLN(PSTR("Cannot set radius and floor at same time.")); + else if (com->R != 0) { - EEPROM::setDeltaTowerXOffsetSteps(offx); - EEPROM::setDeltaTowerYOffsetSteps(offy); - EEPROM::setDeltaTowerZOffsetSteps(offz); + //add r to radius + if (abs(com->R) <= 10) EEPROM::incrementRodRadius(com->R); + else Com::printErrorFLN(PSTR("Calibration movement is limited to 10mm.")); } -#endif - Printer::homeAxis(true,true,true); - } - break; - case 133: // Measure steps to top + else { - 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] = Printer::zMaxSteps; - 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] = Printer::zMaxSteps; - Printer::deltaMoveToTopEndstops(Printer::homingFeedrate[Z_AXIS]); - int32_t m = Printer::zMaxSteps*1.5; - 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); - Printer::setAutolevelActive(oldAuto); - Printer::homeAxis(true,true,true); - } - break; - /* case 134: - Com::printF(PSTR("CompDelta:"),Printer::currentDeltaPositionSteps[A_TOWER]); - Com::printF(Com::tComma,Printer::currentDeltaPositionSteps[B_TOWER]); - Com::printFLN(Com::tComma,Printer::currentDeltaPositionSteps[C_TOWER]); + // 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]); + } + } + 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 == EEPROM_ON + if(com->hasS() && com->S>0) + { + EEPROM::setDeltaTowerXOffsetSteps(offx); + EEPROM::setDeltaTowerYOffsetSteps(offy); + EEPROM::setDeltaTowerZOffsetSteps(offz); + } +#endif + 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); + Printer::homeAxis(true,true,true); + } + break; +/* case 134: // G134 + 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(); - break; + 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 - } - previousMillisCmd = HAL::timeInMilliseconds(); - } - - else if(com->hasM()) // Process M Code + default: + if(Printer::debugErrors()) { + Com::printF(Com::tUnknownCommand); + com->printCommand(); + } + } +} +/** + \brief Execute the G command stored in com. +*/ +void Commands::processMCode(GCode *com) +{ + uint32_t codenum; //throw away variable + switch( com->M ) + { - switch( com->M ) - { #if SDSUPPORT + case 20: // M20 - list SD card + sd.ls(); + 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; +#endif + case 42: //M42 -Change pin status via gcode + if (com->hasS() && com->hasP() && com->S>=0 && com->S<=255) + { + 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; + break; + } + } + if (pin_number > -1) + { + 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); + } + } + break; - case 20: // M20 - list SD card - sd.ls(); - 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; + case 80: // M80 - ATX Power On +#if PS_ON_PIN>-1 + Commands::waitUntilEndOfAllMoves(); + previousMillisCmd = HAL::timeInMilliseconds(); + SET_OUTPUT(PS_ON_PIN); //GND + WRITE(PS_ON_PIN, (POWER_INVERTING ? HIGH : LOW)); #endif - case 42: //M42 -Change pin status via gcode - if (com->hasS() && com->hasP() && com->S>=0 && com->S<=255) - { - 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; - break; - } - } - if (pin_number > -1) - { - 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); - } - } - break; - case 104: // M104 + break; + case 81: // M81 - ATX Power Off +#if PS_ON_PIN>-1 + Commands::waitUntilEndOfAllMoves(); + SET_OUTPUT(PS_ON_PIN); //GND + 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