diff options
Diffstat (limited to 'src/Movement/Kinematics/PolarKinematics.cpp')
| -rw-r--r-- | src/Movement/Kinematics/PolarKinematics.cpp | 260 |
1 files changed, 260 insertions, 0 deletions
diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp new file mode 100644 index 00000000..8d1eb8b8 --- /dev/null +++ b/src/Movement/Kinematics/PolarKinematics.cpp @@ -0,0 +1,260 @@ +/* + * PolarKinematics.cpp + * + * Created on: 13 Oct 2017 + * Author: David + */ + +#include "PolarKinematics.h" + +#include "RepRap.h" +#include "Platform.h" +#include "Storage/MassStorage.h" +#include "GCodes/GCodeBuffer.h" +#include "Movement/DDA.h" + +// Constructor +PolarKinematics::PolarKinematics() + : Kinematics(KinematicsType::polar, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true), + minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0), + maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(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 +{ + 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, StringRef& reply, bool& error) +{ + if (mCode == 669) + { + bool seenNonGeometry = false; + gb.TryGetFValue('S', segmentsPerSecond, seenNonGeometry); + gb.TryGetFValue('T', minSegmentLength, seenNonGeometry); + + 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); + gb.TryGetFValue('A', maxTurntableAcceleration, seenNonGeometry); + gb.TryGetFValue('F', maxTurntableSpeed, seenNonGeometry); + + if (seen || seenNonGeometry) + { + Recalc(); + } + else + { + reply.printf("Printer mode is Polar with radius %.1f to %.1fmm, homed radius %.1fmm, segments/sec %d, min. segment length %.2f", + (double)minRadius, (double)maxRadius, (double)homedRadius, + (int)segmentsPerSecond, (double)minSegmentLength); + } + 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 +{ + motorPos[0] = lrintf(sqrtf(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 +{ + 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 x, float y, bool isCoordinated) const +{ + const float r2 = fsquare(x) + fsquare(y); + return r2 >= minRadiusSquared && r2 <= maxRadiusSquared; +} + +// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed +bool PolarKinematics::LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const +{ + const bool m208Limited = Kinematics::LimitPositionFromAxis(position, Z_AXIS, numAxes, axesHomed); // call base class function to limit Z and higher axes + const float r2 = fsquare(position[0]) + fsquare(position[1]); + bool radiusLimited; + if (r2 < minRadiusSquared) + { + radiusLimited = true; + const float r = sqrtf(r2); + if (r < 0.01) + { + position[0] = minRadius; + position[1] = 0.0; + } + else + { + position[0] *= minRadius/r; + position[1] *= minRadius/r; + } + } + else if (r2 > maxRadiusSquared) + { + radiusLimited = true; + const float r = sqrtf(r2); + position[0] *= maxRadius/r; + position[1] *= maxRadius/r; + } + else + { + radiusLimited = false; + } + + return m208Limited || radiusLimited; +} + +// Return the initial Cartesian coordinates we assume after switching to this kinematics +void PolarKinematics::GetAssumedInitialPosition(size_t numAxes, float positions[]) const +{ + 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 +{ + // If both X and Y have been specified then we know the positions the radius motor and the turntable, otherwise we don't + const AxesBitmap xyAxes = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS); + if ((g92Axes & xyAxes) != xyAxes) + { + g92Axes &= ~xyAxes; + } + return g92Axes; +} + +// 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'. +const char* PolarKinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const +{ + // Ask the base class which homing file we should call first + const char* ret = Kinematics::GetHomingFileName(toBeHomed, alreadyHomed, numVisibleAxes, mustHomeFirst); + // Change the returned name if it is X or Y + if (ret == StandardHomingFileNames[X_AXIS]) + { + ret = HomeRadiusFileName; + } + else if (ret == StandardHomingFileNames[Y_AXIS]) + { + ret = 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 +{ + 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 +{ + 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) const +{ + const int32_t turntableMovement = labs(dda.DriveCoordinates()[1] - dda.GetPrevious()->DriveCoordinates()[1]); + if (turntableMovement != 0) + { + const float stepRatio = dda.GetTotalDistance() * reprap.GetPlatform().DriveStepsPerUnit(1)/turntableMovement; + dda.LimitSpeedAndAcceleration(stepRatio * maxTurntableSpeed, stepRatio * maxTurntableAcceleration); + } +} + +// 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); +} + +// End |