diff options
Diffstat (limited to 'src/Movement/DeltaParameters.cpp')
| -rw-r--r-- | src/Movement/DeltaParameters.cpp | 93 |
1 files changed, 67 insertions, 26 deletions
diff --git a/src/Movement/DeltaParameters.cpp b/src/Movement/DeltaParameters.cpp index 1e808cc2..34f3a1c6 100644 --- a/src/Movement/DeltaParameters.cpp +++ b/src/Movement/DeltaParameters.cpp @@ -13,10 +13,11 @@ void DeltaParameters::Init() diagonal = 0.0; radius = 0.0; xCorrection = yCorrection = zCorrection = 0.0; + xTilt = yTilt = 0.0; printRadius = defaultPrintRadius; homedHeight = defaultDeltaHomedHeight; - for (size_t axis = 0; axis < AXES; ++axis) + for (size_t axis = 0; axis < DELTA_AXES; ++axis) { endstopAdjustments[axis] = 0.0; towerX[axis] = towerY[axis] = 0.0; @@ -50,7 +51,7 @@ void DeltaParameters::Recalc() // Calculate the base carriage height when the printer is homed, i.e. the carriages are at the endstops less the corrections const float tempHeight = diagonal; // any sensible height will do here - float machinePos[AXES]; + float machinePos[DELTA_AXES]; InverseTransform(tempHeight, tempHeight, tempHeight, machinePos); homedCarriageHeight = homedHeight + tempHeight - machinePos[Z_AXIS]; } @@ -67,14 +68,17 @@ void DeltaParameters::NormaliseEndstopAdjustments() homedCarriageHeight += eav; // no need for a full recalc, this is sufficient } -// Calculate the motor position for a single tower from a Cartesian coordinate -float DeltaParameters::Transform(const float machinePos[AXES], size_t axis) const +// Calculate the motor position for a single tower from a Cartesian coordinate. +float DeltaParameters::Transform(const float machinePos[DELTA_AXES], size_t axis) const { - return machinePos[Z_AXIS] - + sqrt(D2 - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis])); + return sqrt(D2 - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis])) + + machinePos[Z_AXIS] + + (machinePos[X_AXIS] * xTilt) + + (machinePos[Y_AXIS] * yTilt); } -void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machinePos[AXES]) const +// Calculate the Cartesian coordinates from the motor coordinates. +void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machinePos[DELTA_AXES]) const { const float Fa = coreFa + fsquare(Ha); const float Fb = coreFb + fsquare(Hb); @@ -103,7 +107,7 @@ void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machi float z = (minusHalfB - sqrtf(fsquare(minusHalfB) - A * C)) / A; machinePos[X_AXIS] = (U * z - S) / Q; machinePos[Y_AXIS] = (P - R * z) / Q; - machinePos[Z_AXIS] = z; + machinePos[Z_AXIS] = z - ((machinePos[X_AXIS] * xTilt) + (machinePos[Y_AXIS] * yTilt)); } // Compute the derivative of height with respect to a parameter at the specified motor endpoints. @@ -113,7 +117,8 @@ void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machi // 4 = X tower correction // 5 = Y tower correction // 6 = diagonal rod length -float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb, float hc) +// 7, 8 = X tilt, Y tilt. We scale these by the printable radius to get sensible values in the range -1..1 +floatc_t DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb, float hc) { const float perturb = 0.2; // perturbation amount in mm or degrees DeltaParameters hiParams(*this), loParams(*this); @@ -122,50 +127,71 @@ float DeltaParameters::ComputeDerivative(unsigned int deriv, float ha, float hb, case 0: case 1: case 2: + // Endstop corrections break; case 3: hiParams.radius += perturb; loParams.radius -= perturb; + hiParams.Recalc(); + loParams.Recalc(); break; case 4: hiParams.xCorrection += perturb; loParams.xCorrection -= perturb; + hiParams.Recalc(); + loParams.Recalc(); break; case 5: hiParams.yCorrection += perturb; loParams.yCorrection -= perturb; + hiParams.Recalc(); + loParams.Recalc(); break; case 6: hiParams.diagonal += perturb; loParams.diagonal -= perturb; + hiParams.Recalc(); + loParams.Recalc(); break; - } - hiParams.Recalc(); - loParams.Recalc(); + case 7: + case 8: + // X and Y tilt + break; + } - float newPos[AXES]; + float newPos[DELTA_AXES]; hiParams.InverseTransform((deriv == 0) ? ha + perturb : ha, (deriv == 1) ? hb + perturb : hb, (deriv == 2) ? hc + perturb : hc, newPos); - float zHi = newPos[Z_AXIS]; + if (deriv == 7) + { + return -newPos[X_AXIS]/printRadius; + } + if (deriv == 8) + { + return -newPos[Y_AXIS]/printRadius; + } + + const float zHi = newPos[Z_AXIS]; loParams.InverseTransform((deriv == 0) ? ha - perturb : ha, (deriv == 1) ? hb - perturb : hb, (deriv == 2) ? hc - perturb : hc, newPos); - float zLo = newPos[Z_AXIS]; + const float zLo = newPos[Z_AXIS]; - return (zHi - zLo)/(2 * perturb); + return ((floatc_t)zHi - (floatc_t)zLo)/(2 * perturb); } -// Perform 3, 4, 6 or 7-factor adjustment. +// Perform 3, 4, 6, 7, 8 or 9-factor adjustment. // The input vector contains the following parameters in this order: // X, Y and Z endstop adjustments -// If we are doing 4-factor adjustment, the next argument is the delta radius. Otherwise: -// X tower X position adjustment -// Y tower X position adjustment -// Z tower Y position adjustment -// Diagonal rod length adjustment -void DeltaParameters::Adjust(size_t numFactors, const float v[]) +// Delta radius +// X tower position adjustment +// Y tower position adjustment +// Diagonal rod length adjustment - omitted if doing 8-factor calibration (remainder are moved down) +// X tilt adjustment +// Y tilt adjustment +void DeltaParameters::Adjust(size_t numFactors, const floatc_t v[]) { const float oldCarriageHeightA = GetHomedCarriageHeight(A_AXIS); // save for later @@ -184,10 +210,21 @@ void DeltaParameters::Adjust(size_t numFactors, const float v[]) xCorrection += v[4]; yCorrection += v[5]; - if (numFactors == 7) + if (numFactors == 7 || numFactors == 9) { diagonal += v[6]; } + + if (numFactors == 8) + { + xTilt += v[6]/printRadius; + yTilt += v[7]/printRadius; + } + else if (numFactors == 9) + { + xTilt += v[7]/printRadius; + yTilt += v[8]/printRadius; + } } Recalc(); @@ -198,13 +235,17 @@ void DeltaParameters::Adjust(size_t numFactors, const float v[]) const float heightError = GetHomedCarriageHeight(A_AXIS) - oldCarriageHeightA - v[0]; homedHeight -= heightError; homedCarriageHeight -= heightError; + + // Note: if we adjusted the X and Y tilts, and there are any endstop adjustments, then the homed position won't be exactly in the centre + // and changing the tilt will therefore affect the homed height. We ignore this for now. If it is ever significant, a second sutocalibration + // run will correct it. } void DeltaParameters::PrintParameters(StringRef& reply) const { - reply.printf("Endstops X%.2f Y%.2f Z%.2f, height %.2f, diagonal %.2f, radius %.2f, xcorr %.2f, ycorr %.2f, zcorr %.2f\n", + reply.printf("Stops X%.3f Y%.3f Z%.3f height %.3f diagonal %.3f radius %.3f xcorr %.2f ycorr %.2f zcorr %.2f xtilt %.3f%% ytilt %.3f%%\n", endstopAdjustments[A_AXIS], endstopAdjustments[B_AXIS], endstopAdjustments[C_AXIS], homedHeight, diagonal, radius, - xCorrection, yCorrection, zCorrection); + xCorrection, yCorrection, zCorrection, xTilt * 100.0, yTilt * 100.0); } // End |