diff options
author | David Crocker <dcrocker@eschertech.com> | 2017-09-24 18:59:14 +0300 |
---|---|---|
committer | David Crocker <dcrocker@eschertech.com> | 2017-09-24 18:59:31 +0300 |
commit | 821db42ecc6767c87f009bdeae88005277731bc0 (patch) | |
tree | 5a3c28c926ad70a6e16ab88823eb78c4484c1ab9 /src | |
parent | 8d42bb182a8f28fa7eafa343fa302d5c49db1070 (diff) |
Version 1.20alpha5
Check arm angle limits when computing SCARA moves
Don't allow SCARA arm mode changes on coordinated moves
Diffstat (limited to 'src')
-rw-r--r-- | src/GCodes/GCodes.cpp | 13 | ||||
-rw-r--r-- | src/GCodes/GCodes.h | 11 | ||||
-rw-r--r-- | src/Movement/DDA.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CartesianKinematics.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CartesianKinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CoreXYKinematics.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CoreXYKinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CoreXYUKinematics.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CoreXYUKinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CoreXZKinematics.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/CoreXZKinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/Kinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/LinearDeltaKinematics.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/LinearDeltaKinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/ScaraKinematics.cpp | 15 | ||||
-rw-r--r-- | src/Movement/Kinematics/ScaraKinematics.h | 2 | ||||
-rw-r--r-- | src/Movement/Move.cpp | 7 | ||||
-rw-r--r-- | src/Movement/Move.h | 2 | ||||
-rw-r--r-- | src/Version.h | 2 |
19 files changed, 48 insertions, 28 deletions
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp index a576a26d..8b4f4502 100644 --- a/src/GCodes/GCodes.cpp +++ b/src/GCodes/GCodes.cpp @@ -504,6 +504,7 @@ void GCodes::Spin() } moveBuffer.feedRate = DefaultFeedrate * SecondsToMinutes; // ask for a good feed rate, we may have paused during a slow move moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -616,6 +617,7 @@ void GCodes::Spin() if (move.IsAccessibleProbePoint(x, y)) { moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -677,6 +679,7 @@ void GCodes::Spin() zProbeTriggered = false; platform.SetProbing(true); moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = ZProbeActive; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -720,6 +723,7 @@ void GCodes::Spin() // Move back up to the dive height moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -801,6 +805,7 @@ void GCodes::Spin() case GCodeState::probingAtPoint0: // Initial state when executing G30 with a P parameter. Start by moving to the dive height at the current position. moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -819,6 +824,7 @@ void GCodes::Spin() // Head is at the dive height but needs to be moved to the correct XY position. // The XY coordinates have already been stored. moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -877,6 +883,7 @@ void GCodes::Spin() zProbeTriggered = false; platform.SetProbing(true); moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = ZProbeActive; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -971,6 +978,7 @@ void GCodes::Spin() // Move back up to the dive height before we change anything, in particular before we adjust leadscrews moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = noFilePosition; @@ -1040,6 +1048,7 @@ void GCodes::Spin() moveBuffer.coords[Z_AXIS] += retractHop; currentZHop = retractHop; moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.isFirmwareRetraction = true; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition; @@ -1071,6 +1080,7 @@ void GCodes::Spin() } moveBuffer.feedRate = unRetractSpeed; moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.isFirmwareRetraction = true; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = (&gb == fileGCode) ? gb.MachineState().fileState.GetPosition() - fileInput->BytesCached() : noFilePosition; @@ -1813,6 +1823,7 @@ bool GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, int moveType) bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinated) { // Set up default move parameters + moveBuffer.isCoordinated = isCoordinated; moveBuffer.endStopsToCheck = 0; moveBuffer.moveType = 0; doingArcMove = false; @@ -2086,6 +2097,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise) moveBuffer.virtualExtruderPosition = virtualExtruderPosition; moveBuffer.endStopsToCheck = 0; moveBuffer.usePressureAdvance = true; + moveBuffer.isCoordinated = true; moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition; // Set up the arc centre coordinates and record which axes behave like an X axis. @@ -3599,6 +3611,7 @@ bool GCodes::RetractFilament(GCodeBuffer& gb, bool retract) moveBuffer.coords[i] = 0.0; } moveBuffer.moveType = 0; + moveBuffer.isCoordinated = false; moveBuffer.isFirmwareRetraction = true; moveBuffer.usePressureAdvance = false; moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition; diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h index e3806556..22bea4b9 100644 --- a/src/GCodes/GCodes.h +++ b/src/GCodes/GCodes.h @@ -88,11 +88,12 @@ public: IoBits_t ioBits; // I/O bits to set/clear at the start of this move #endif uint8_t moveType; // the S parameter from the G0 or G1 command, 0 for a normal move - bool isFirmwareRetraction; // true if this is a firmware retraction/un-retraction move - bool usePressureAdvance; // true if we want to us extruder pressure advance, if there is any extrusion - bool canPauseBefore; // true if we can pause before this move - bool canPauseAfter; // true if we can pause just after this move and successfully restart - bool hasExtrusion; // true if the move includes extrusion - only valid if the move was set up by SetupMove + uint8_t isFirmwareRetraction : 1; // true if this is a firmware retraction/un-retraction move + uint8_t usePressureAdvance : 1; // true if we want to us extruder pressure advance, if there is any extrusion + uint8_t canPauseBefore : 1; // true if we can pause before this move + uint8_t canPauseAfter : 1; // true if we can pause just after this move and successfully restart + uint8_t hasExtrusion : 1; // true if the move includes extrusion - only valid if the move was set up by SetupMove + uint8_t isCoordinated : 1; // true if this is a coordinates move }; GCodes(Platform& p); diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index 4a0bd8f7..ef92c444 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -225,7 +225,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) const Move& move = reprap.GetMove(); if (doMotorMapping) { - if (!move.CartesianToMotorSteps(nextMove.coords, endPoint)) // transform the axis coordinates if on a delta or CoreXY printer + if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, !nextMove.isCoordinated)) // transform the axis coordinates if on a delta or CoreXY printer { return false; // throw away the move if it couldn't be transformed } diff --git a/src/Movement/Kinematics/CartesianKinematics.cpp b/src/Movement/Kinematics/CartesianKinematics.cpp index dba9c2be..db884171 100644 --- a/src/Movement/Kinematics/CartesianKinematics.cpp +++ b/src/Movement/Kinematics/CartesianKinematics.cpp @@ -19,7 +19,7 @@ const char *CartesianKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates -bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const +bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const { for (size_t axis = 0; axis < numVisibleAxes; ++axis) { diff --git a/src/Movement/Kinematics/CartesianKinematics.h b/src/Movement/Kinematics/CartesianKinematics.h index eff75eb9..22051f6f 100644 --- a/src/Movement/Kinematics/CartesianKinematics.h +++ b/src/Movement/Kinematics/CartesianKinematics.h @@ -17,7 +17,7 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; HomingMode GetHomingMode() const override { return homeCartesianAxes; } bool QueryTerminateHomingMove(size_t axis) const override; diff --git a/src/Movement/Kinematics/CoreXYKinematics.cpp b/src/Movement/Kinematics/CoreXYKinematics.cpp index c8b2bdaf..6f35e321 100644 --- a/src/Movement/Kinematics/CoreXYKinematics.cpp +++ b/src/Movement/Kinematics/CoreXYKinematics.cpp @@ -18,7 +18,7 @@ const char *CoreXYKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates returning true if successful -bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const +bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const { motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]); motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]); diff --git a/src/Movement/Kinematics/CoreXYKinematics.h b/src/Movement/Kinematics/CoreXYKinematics.h index 0b426c55..4663acc1 100644 --- a/src/Movement/Kinematics/CoreXYKinematics.h +++ b/src/Movement/Kinematics/CoreXYKinematics.h @@ -17,7 +17,7 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool DriveIsShared(size_t drive) const override; }; diff --git a/src/Movement/Kinematics/CoreXYUKinematics.cpp b/src/Movement/Kinematics/CoreXYUKinematics.cpp index b961bebb..560037aa 100644 --- a/src/Movement/Kinematics/CoreXYUKinematics.cpp +++ b/src/Movement/Kinematics/CoreXYUKinematics.cpp @@ -55,7 +55,7 @@ bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef } // Convert Cartesian coordinates to motor coordinates -bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const +bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const { motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]); motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]); diff --git a/src/Movement/Kinematics/CoreXYUKinematics.h b/src/Movement/Kinematics/CoreXYUKinematics.h index 67034f13..77db9ae9 100644 --- a/src/Movement/Kinematics/CoreXYUKinematics.h +++ b/src/Movement/Kinematics/CoreXYUKinematics.h @@ -18,7 +18,7 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool DriveIsShared(size_t drive) const override; }; diff --git a/src/Movement/Kinematics/CoreXZKinematics.cpp b/src/Movement/Kinematics/CoreXZKinematics.cpp index 1d29c8fc..d81f13ca 100644 --- a/src/Movement/Kinematics/CoreXZKinematics.cpp +++ b/src/Movement/Kinematics/CoreXZKinematics.cpp @@ -18,7 +18,7 @@ const char *CoreXZKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates -bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const +bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const { motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Z_AXIS] * axisFactors[Z_AXIS])) * stepsPerMm[X_AXIS]); motorPos[Y_AXIS] = lrintf(machinePos[Y_AXIS] * stepsPerMm[Y_AXIS]); diff --git a/src/Movement/Kinematics/CoreXZKinematics.h b/src/Movement/Kinematics/CoreXZKinematics.h index 94b8a9f8..3a8d9344 100644 --- a/src/Movement/Kinematics/CoreXZKinematics.h +++ b/src/Movement/Kinematics/CoreXZKinematics.h @@ -17,7 +17,7 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); } bool DriveIsShared(size_t drive) const override; diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h index 8dfe3da6..3d960929 100644 --- a/src/Movement/Kinematics/Kinematics.h +++ b/src/Movement/Kinematics/Kinematics.h @@ -71,7 +71,7 @@ public: // '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 - virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const = 0; + virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const = 0; // Convert motor positions (measured in steps from reference position) to Cartesian coordinates // 'motorPos' is the input vector of motor positions diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp index 316c0ef6..8ff47f0e 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp +++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp @@ -134,7 +134,7 @@ void LinearDeltaKinematics::InverseTransform(float Ha, float Hb, float Hc, float } // Convert Cartesian coordinates to motor steps -bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const +bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const { //TODO return false if we can't transform the position for (size_t axis = 0; axis < min<size_t>(numVisibleAxes, DELTA_AXES); ++axis) diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h index 875bd126..6a3bdc3f 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.h +++ b/src/Movement/Kinematics/LinearDeltaKinematics.h @@ -26,7 +26,7 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool SupportsAutoCalibration() const override { return true; } bool DoAutoCalibration(size_t numFactors, const RandomProbePointSet& probePoints, StringRef& reply) override; diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp index 58438d25..a350f42b 100644 --- a/src/Movement/Kinematics/ScaraKinematics.cpp +++ b/src/Movement/Kinematics/ScaraKinematics.cpp @@ -32,7 +32,7 @@ const char *ScaraKinematics::GetName(bool forStatusReport) const // Convert Cartesian coordinates to motor coordinates // In the following, theta is the proximal arm angle relative to the X axis, psi is the distal arm angle relative to the proximal arm -bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const +bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const { // No need to limit x,y to reachable positions here, we already did that in class GCodes const float x = machinePos[X_AXIS] + xOffset; @@ -56,11 +56,11 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa bool switchedMode = false; for (;;) { - if (isDefaultArmMode) + if (isDefaultArmMode != switchedMode) { // The following equations choose arm mode 0 i.e. distal arm rotated anticlockwise relative to proximal arm theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y); - if (theta >= thetaLimits[0]) + if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1]) { break; } @@ -69,7 +69,7 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa { // The following equations choose arm mode 1 i.e. distal arm rotated clockwise relative to proximal arm theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y); - if (theta <= thetaLimits[1]) + if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1]) { psi = -psi; break; @@ -80,10 +80,15 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa { return false; // not reachable } - isDefaultArmMode = !isDefaultArmMode; switchedMode = true; } + // Now that we know we are going to do the move, update the arm mode + if (switchedMode) + { + isDefaultArmMode = !isDefaultArmMode; + } + //debugPrintf("psi = %.2f, theta = %.2f\n", psi * RadiansToDegrees, theta * RadiansToDegrees); motorPos[X_AXIS] = lrintf(theta * RadiansToDegrees * stepsPerMm[X_AXIS]); diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h index 248d9008..e229468d 100644 --- a/src/Movement/Kinematics/ScaraKinematics.h +++ b/src/Movement/Kinematics/ScaraKinematics.h @@ -28,7 +28,7 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool IsReachable(float x, float y) const override; bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed) const override; diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp index 31890527..f2cc927b 100644 --- a/src/Movement/Move.cpp +++ b/src/Movement/Move.cpp @@ -569,7 +569,7 @@ void Move::SetPositions(const float move[DRIVES]) void Move::EndPointToMachine(const float coords[], int32_t ep[], size_t numDrives) const { - if (CartesianToMotorSteps(coords, ep)) + if (CartesianToMotorSteps(coords, ep, true)) { const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t drive = numAxes; drive < numDrives; ++drive) @@ -594,9 +594,10 @@ void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes // Convert Cartesian coordinates to motor steps, axes only, returning true if successful. // Used to perform movement and G92 commands. -bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const +bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const { - const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos); + const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), + reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, allowModeChange); if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda)) { if (b) diff --git a/src/Movement/Move.h b/src/Movement/Move.h index 07d00845..9477115e 100644 --- a/src/Movement/Move.h +++ b/src/Movement/Move.h @@ -75,7 +75,7 @@ public: // Kinematics and related functions Kinematics& GetKinematics() const { return *kinematics; } bool SetKinematics(KinematicsType k); // Set kinematics, return true if successful - bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const; + bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const; // Convert Cartesian coordinates to delta motor coordinates, return true if successful void MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const; // Convert motor coordinates to machine coordinates diff --git a/src/Version.h b/src/Version.h index d0ee8670..1cf17174 100644 --- a/src/Version.h +++ b/src/Version.h @@ -9,7 +9,7 @@ #define SRC_VERSION_H_ #ifndef VERSION -# define VERSION "1.20alpha4" +# define VERSION "1.20alpha5" #endif #ifndef DATE |