/* * PolarKinematics.cpp * * Created on: 13 Oct 2017 * Author: David */ #include "PolarKinematics.h" #if SUPPORT_POLAR #include #include #include #include #include #if SUPPORT_OBJECT_MODEL // Object model table and functions // Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17. // Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM. // Macro to build a standard lambda function that includes the necessary type conversions #define OBJECT_MODEL_FUNC(...) OBJECT_MODEL_FUNC_BODY(PolarKinematics, __VA_ARGS__) constexpr ObjectModelTableEntry PolarKinematics::objectModelTable[] = { // Within each group, these entries must be in alphabetical order // 0. kinematics members { "name", OBJECT_MODEL_FUNC(self->GetName(true)), ObjectModelEntryFlags::none }, }; constexpr uint8_t PolarKinematics::objectModelTableDescriptor[] = { 1, 1 }; DEFINE_GET_OBJECT_MODEL_TABLE(PolarKinematics) #endif // Constructor PolarKinematics::PolarKinematics() noexcept : Kinematics(KinematicsType::polar, SegmentationType(true, false, false)), minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0), maxTurntableSpeed(ConvertSpeedFromMmPerSec(DefaultMaxTurntableSpeed)), maxTurntableAcceleration(ConvertAcceleration(DefaultMaxTurntableAcceleration)) { Recalc(); } // Return the name of the current kinematics. // If 'forStatusReport' is true then the string must be the one for that kinematics expected by DuetWebControl and PanelDue. // Otherwise it should be in a format suitable for printing. // For any new kinematics, the same string can be returned regardless of the parameter. const char *PolarKinematics::GetName(bool forStatusReport) const noexcept { return "Polar"; } // Set or report the parameters from a M665, M666 or M669 command // If 'mCode' is an M-code used to set parameters for the current kinematics (which should only ever be 665, 666, 667 or 669) // then search for parameters used to configure the current kinematics. If any are found, perform appropriate actions, // and return true if the changes affect the geometry. // If errors were discovered while processing parameters, put an appropriate error message in 'reply' and set 'error' to true. // If no relevant parameters are found, print the existing ones to 'reply' and return false. // If 'mCode' does not apply to this kinematics, call the base class version of this function, which will print a suitable error message. bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const StringRef& reply, bool& error) THROWS(GCodeException) { if (mCode == 669) { bool seen = false; if (gb.Seen('R')) { seen = true; float radiusLimits[2]; size_t numRadiusLimits = 2; gb.GetFloatArray(radiusLimits, numRadiusLimits, false); if (numRadiusLimits == 2) { minRadius = radiusLimits[0]; maxRadius = radiusLimits[1]; } else { minRadius = 0.0; maxRadius = radiusLimits[0]; } homedRadius = minRadius; // set up default } gb.TryGetFValue('H', homedRadius, seen); bool seenNonGeometry = TryConfigureSegmentation(gb); if (gb.Seen('A')) { maxTurntableAcceleration = gb.GetAcceleration(); seenNonGeometry = true; } if (gb.Seen('F')) { maxTurntableSpeed = gb.GetSpeedFromMm(true); seenNonGeometry = true; } if (seen) { Recalc(); } else if (!seenNonGeometry && !gb.Seen('K')) { Kinematics::Configure(mCode, gb, reply, error); reply.catf(", radius %.1f to %.1fmm, homed radius %.1fmm, max table acc. %.1fdeg/sec^2, max table speed %.1fdeg/sec", (double)minRadius, (double)maxRadius, (double)homedRadius, (double)InverseConvertAcceleration(maxTurntableAcceleration), (double)InverseConvertSpeedToMmPerSec(maxTurntableSpeed)); } return seen; } else { return Kinematics::Configure(mCode, gb, reply, error); } } // Convert Cartesian coordinates to motor positions measured in steps from reference position // 'machinePos' is a set of axis and extruder positions to convert // 'stepsPerMm' is as configured in M92. On a Scara or polar machine this would actually be steps per degree. // 'numAxes' is the number of machine axes to convert, which will always be at least 3 // 'motorPos' is the output vector of motor positions // Return true if successful, false if we were unable to convert bool PolarKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept { motorPos[0] = lrintf(fastSqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]); motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]); // Transform remaining axes linearly for (size_t axis = Z_AXIS; axis < numVisibleAxes; ++axis) { motorPos[axis] = lrintf(machinePos[axis] * stepsPerMm[axis]); } return true; } // Convert motor positions (measured in steps from reference position) to Cartesian coordinates // 'motorPos' is the input vector of motor positions // 'stepsPerMm' is as configured in M92. On a Scara or polar machine this would actually be steps per degree. // 'numDrives' is the number of machine drives to convert, which will always be at least 3 // 'machinePos' is the output set of converted axis and extruder positions void PolarKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept { const float angle = (motorPos[1] * DegreesToRadians)/stepsPerMm[1]; const float radius = (float)motorPos[0]/stepsPerMm[0]; machinePos[0] = radius * cosf(angle); machinePos[1] = radius * sinf(angle); // Convert any additional axes linearly for (size_t drive = Z_AXIS; drive < numVisibleAxes; ++drive) { machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; } } // Return true if the specified XY position is reachable by the print head reference point. bool PolarKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { if (axes.IsBitSet(X_AXIS) && axes.IsBitSet(Y_AXIS)) { const float r2 = fsquare(axesCoords[X_AXIS]) + fsquare(axesCoords[Y_AXIS]); if(r2 < minRadiusSquared || r2 > maxRadiusSquared) { return false; } } axes.ClearBit(X_AXIS); axes.ClearBit(Y_AXIS); return Kinematics::IsReachable(axesCoords, axes); } // Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed LimitPositionResult PolarKinematics::LimitPosition(float finalCoords[], const float * null initialCoords, size_t numAxes, AxesBitmap axesToLimit, bool isCoordinated, bool applyM208Limits) const noexcept { const bool m208Limited = (applyM208Limits) ? Kinematics::LimitPositionFromAxis(finalCoords, Z_AXIS, numAxes, axesToLimit) // call base class function to limit Z and higher axes : false; const float r2 = fsquare(finalCoords[X_AXIS]) + fsquare(finalCoords[Y_AXIS]); bool radiusLimited; if (r2 < minRadiusSquared) { radiusLimited = true; const float r = fastSqrtf(r2); if (r < 0.01) { finalCoords[X_AXIS] = minRadius; finalCoords[Y_AXIS] = 0.0; } else { finalCoords[X_AXIS] *= minRadius/r; finalCoords[Y_AXIS] *= minRadius/r; } } else if (r2 > maxRadiusSquared) { radiusLimited = true; const float r = fastSqrtf(r2); finalCoords[X_AXIS] *= maxRadius/r; finalCoords[Y_AXIS] *= maxRadius/r; } else { radiusLimited = false; } return (m208Limited || radiusLimited) ? LimitPositionResult::adjusted : LimitPositionResult::ok; } // Return the initial Cartesian coordinates we assume after switching to this kinematics void PolarKinematics::GetAssumedInitialPosition(size_t numAxes, float positions[]) const noexcept { for (size_t i = 0; i < numAxes; ++i) { positions[i] = 0.0; } } // Return the axes that we can assume are homed after executing a G92 command to set the specified axis coordinates AxesBitmap PolarKinematics::AxesAssumedHomed(AxesBitmap g92Axes) const noexcept { // If both X and Y have been specified then we know the positions the radius motor and the turntable, otherwise we don't if ((g92Axes & XyAxes) != XyAxes) { g92Axes &= ~XyAxes; } return g92Axes; } // Return the set of axes that must be homed prior to regular movement of the specified axes AxesBitmap PolarKinematics::MustBeHomedAxes(AxesBitmap axesMoving, bool disallowMovesBeforeHoming) const noexcept { if (axesMoving.Intersects(XyAxes)) { axesMoving |= XyAxes; } return axesMoving; } // This function is called when a request is made to home the axes in 'toBeHomed' and the axes in 'alreadyHomed' have already been homed. // If we can proceed with homing some axes, return the name of the homing file to be called. Optionally, update 'alreadyHomed' to indicate // that some additional axes should be considered not homed. // If we can't proceed because other axes need to be homed first, return nullptr and pass those axes back in 'mustBeHomedFirst'. AxesBitmap PolarKinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, const StringRef& filename) const noexcept { // Ask the base class which homing file we should call first const AxesBitmap ret = Kinematics::GetHomingFileName(toBeHomed, alreadyHomed, numVisibleAxes, filename); if (ret.IsNonEmpty()) { // Change the returned name if it is X or Y if (StringEqualsIgnoreCase(filename.c_str(), "homex.g")) { filename.copy(HomeRadiusFileName); } else if (StringEqualsIgnoreCase(filename.c_str(), "homey.g")) { filename.copy(HomeBedFileName); } } return ret; } // This function is called from the step ISR when an endstop switch is triggered during homing. // Return true if the entire homing move should be terminated, false if only the motor associated with the endstop switch should be stopped. bool PolarKinematics::QueryTerminateHomingMove(size_t axis) const noexcept { return false; } // This function is called from the step ISR when an endstop switch is triggered during homing after stopping just one motor or all motors. // Take the action needed to define the current position, normally by calling dda.SetDriveCoordinate() and return false. void PolarKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const noexcept { switch(axis) { case X_AXIS: // radius dda.SetDriveCoordinate(lrintf(homedRadius * stepsPerMm[axis]), axis); break; case Y_AXIS: // bed dda.SetDriveCoordinate(0, axis); break; default: const float hitPoint = (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) : reprap.GetPlatform().AxisMinimum(axis); dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); break; } } // Limit the speed and acceleration of a move to values that the mechanics can handle. // The speeds in Cartesian space have already been limited. void PolarKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept { int32_t turntableMovement = dda.DriveCoordinates()[1] - dda.GetPrevious()->DriveCoordinates()[1]; if (turntableMovement != 0) { const float stepsPerDegree = reprap.GetPlatform().DriveStepsPerUnit(1); if (continuousRotationShortcut) { const int32_t stepsPerRotation = lrintf(360.0 * stepsPerDegree); if (turntableMovement > stepsPerRotation/2) { turntableMovement -= stepsPerRotation; } else if (turntableMovement < -stepsPerRotation/2) { turntableMovement += stepsPerRotation; } } if (turntableMovement != 0) { const float stepRatio = dda.GetTotalDistance() * stepsPerDegree/labs(turntableMovement); dda.LimitSpeedAndAcceleration(stepRatio * maxTurntableSpeed, stepRatio * maxTurntableAcceleration); } } } // Return true if the specified axis is a continuous rotation axis bool PolarKinematics::IsContinuousRotationAxis(size_t axis) const noexcept { return axis == Y_AXIS || Kinematics::IsContinuousRotationAxis(axis); } // Return a bitmap of axes that move linearly in response to the correct combination of linear motor movements. // This is called to determine whether we can babystep the specified axis independently of regular motion. AxesBitmap PolarKinematics::GetLinearAxes() const noexcept { return AxesBitmap::MakeFromBits(Z_AXIS); } // Update the derived parameters after the master parameters have been changed void PolarKinematics::Recalc() { minRadiusSquared = (minRadius <= 0.0) ? 0.0 : fsquare(minRadius); maxRadiusSquared = fsquare(maxRadius); } #endif // SUPPORT_POLAR // End