diff options
Diffstat (limited to 'src/Movement')
21 files changed, 193 insertions, 462 deletions
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index 6e8f7c9c..6217d5fe 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -61,15 +61,15 @@ static size_t savedMovePointer = 0; #if DDA_LOG_PROBE_CHANGES size_t DDA::numLoggedProbePositions = 0; -int32_t DDA::loggedProbePositions[CART_AXES * MaxLoggedProbePositions]; +int32_t DDA::loggedProbePositions[XYZ_AXES * MaxLoggedProbePositions]; bool DDA::probeTriggered = false; void DDA::LogProbePosition() { if (numLoggedProbePositions < MaxLoggedProbePositions) { - int32_t *p = loggedProbePositions + (numLoggedProbePositions * CART_AXES); - for (size_t drive = 0; drive < CART_AXES; ++drive) + int32_t *p = loggedProbePositions + (numLoggedProbePositions * XYZ_AXES); + for (size_t drive = 0; drive < XYZ_AXES; ++drive) { DriveMovement& dm = ddm[drive]; if (dm.state == DMState::moving) @@ -146,11 +146,11 @@ void DDA::DebugPrintVector(const char *name, const float *vec, size_t len) const void DDA::DebugPrint() const { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); debugPrintf("DDA:"); if (endCoordinatesValid) { - float startCoordinates[MAX_AXES]; + float startCoordinates[MaxAxes]; for (size_t i = 0; i < numAxes; ++i) { startCoordinates[i] = endCoordinates[i] - (totalDistance * directionVector[i]); @@ -224,7 +224,7 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping) const bool isSpecialDeltaMove = (move.IsDeltaMode() && !doMotorMapping); float accelerations[DRIVES]; const float * const normalAccelerations = reprap.GetPlatform().Accelerations(); - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t drive = 0; drive < DRIVES; drive++) { accelerations[drive] = normalAccelerations[drive]; @@ -739,22 +739,21 @@ void DDA::CalcNewSpeeds() // This is called by Move::CurrentMoveCompleted to update the live coordinates from the move that has just finished bool DDA::FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords[DRIVES]) { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); - for (size_t drive = 0; drive < DRIVES; ++drive) { ep[drive] = endPoint[drive]; } if (endCoordinatesValid) { - for (size_t axis = 0; axis < numAxes; ++axis) + const size_t visibleAxes = reprap.GetGCodes().GetVisibleAxes(); + for (size_t axis = 0; axis < visibleAxes; ++axis) { endCoords[axis] = endCoordinates[axis]; } } // Extrusion amounts are always valid - for (size_t eDrive = numAxes; eDrive < DRIVES; ++eDrive) + for (size_t eDrive = reprap.GetGCodes().GetTotalAxes(); eDrive < DRIVES; ++eDrive) { endCoords[eDrive] += endCoordinates[eDrive]; } @@ -765,7 +764,7 @@ bool DDA::FetchEndPosition(volatile int32_t ep[DRIVES], volatile float endCoords void DDA::SetPositions(const float move[DRIVES], size_t numDrives) { reprap.GetMove().EndPointToMachine(move, endPoint, numDrives); - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); for (size_t axis = 0; axis < numAxes; ++axis) { endCoordinates[axis] = move[axis]; @@ -775,7 +774,7 @@ void DDA::SetPositions(const float move[DRIVES], size_t numDrives) // Get a Cartesian end coordinate from this move float DDA::GetEndCoordinate(size_t drive, bool disableDeltaMapping) -pre(disableDeltaMapping || drive < MAX_AXES) +pre(disableDeltaMapping || drive < MaxAxes) { if (disableDeltaMapping) { @@ -783,10 +782,10 @@ pre(disableDeltaMapping || drive < MAX_AXES) } else { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); - if (drive < numAxes && !endCoordinatesValid) + const size_t visibleAxes = reprap.GetGCodes().GetVisibleAxes(); + if (drive < visibleAxes && !endCoordinatesValid) { - reprap.GetMove().MotorStepsToCartesian(endPoint, numAxes, endCoordinates); + reprap.GetMove().MotorStepsToCartesian(endPoint, visibleAxes, reprap.GetGCodes().GetTotalAxes(), endCoordinates); endCoordinatesValid = true; } return endCoordinates[drive]; @@ -905,7 +904,7 @@ void DDA::Prepare() goingSlow = false; firstDM = nullptr; - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t drive = 0; drive < DRIVES; ++drive) { DriveMovement& dm = ddm[drive]; @@ -1027,7 +1026,7 @@ float DDA::NormaliseXYZ() // First calculate the magnitude of the vector. If there is more than one X axis, take an average of their movements (they should be equal). float magSquared = 0.0; unsigned int numXaxes = 0; - for (size_t d = 0; d < MAX_AXES; ++d) + for (size_t d = 0; d < MaxAxes; ++d) { if (((1 << d) & xAxes) != 0) { @@ -1129,7 +1128,7 @@ void DDA::CheckEndstops(Platform& platform) } #endif - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t drive = 0; drive < numAxes; ++drive) { if ((endStopsToCheck & (1 << drive)) != 0) @@ -1200,7 +1199,7 @@ pre(state == frozen) if (firstDM != nullptr) { unsigned int extrusions = 0, retractions = 0; // bitmaps of extruding and retracting drives - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t i = 0; i < DRIVES; ++i) { DriveMovement& dm = ddm[i]; @@ -1376,7 +1375,7 @@ void DDA::StopDrive(size_t drive) { endPoint[drive] -= dm.GetNetStepsLeft(); dm.state = DMState::idle; - if (drive < reprap.GetGCodes().GetNumAxes()) + if (drive < reprap.GetGCodes().GetTotalAxes()) { endCoordinatesValid = false; // the XYZ position is no longer valid } diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h index ecb2f632..49f21bb0 100644 --- a/src/Movement/DDA.h +++ b/src/Movement/DDA.h @@ -91,7 +91,7 @@ public: #if DDA_LOG_PROBE_CHANGES static const size_t MaxLoggedProbePositions = 40; static size_t numLoggedProbePositions; - static int32_t loggedProbePositions[CART_AXES * MaxLoggedProbePositions]; + static int32_t loggedProbePositions[XYZ_AXES * MaxLoggedProbePositions]; #endif private: diff --git a/src/Movement/DeltaProbe.cpp b/src/Movement/DeltaProbe.cpp deleted file mode 100644 index 37a2f0d8..00000000 --- a/src/Movement/DeltaProbe.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* - * DeltaProbe.cpp - * - * Created on: 20 Apr 2015 - * Author: David - */ - -#include "DeltaProbe.h" -#include "DDA.h" -#include "Platform.h" -#include "RepRap.h" - -// Set up to probe -bool DeltaProbe::Init(float frequency, float amplitude, float rate, float height) -{ -debugPrintf("Start probe f=%.1f a=%.2f r=%.2f h=%.1f\n", frequency, amplitude, rate, height); - // Sanity check the inputs (we check the max amplitude later) - if (frequency < 50.0 || frequency > 1000.0 || amplitude < 0.02 || rate < 0.1 || rate > 10.0 || height < 0.5) - { - return false; - } - -debugPrintf("ok so far\n"); - // Calculate the number of steps for the peak to peak amplitude - const float zRate = reprap.GetPlatform().DriveStepsPerUnit(Z_AXIS); - normalSteps = (size_t)(amplitude * zRate); - if (normalSteps > MaxSteps) - { - return false; - } - -debugPrintf("normalSteps=%u\n", normalSteps); - // Build the tables of step times for sinusoidal motion - const float recipOmega = (float)DDA::stepClockRate/(frequency * 2.0 * PI); - - for (size_t i = 0; i < normalSteps - 1; ++i) - { - normalStepTable[i] = acos(1.0 - (float)(2 * (i + 1))/(float)normalSteps) * recipOmega; - } - - for (size_t i = 0; i < normalSteps; ++i) - { - incStepTable[i] = acos(1.0 - (float)(2 * (i + 1))/(float)(normalSteps + 1)) * recipOmega; - } - - halfCycleTime = (uint32_t)((float)DDA::stepClockRate/(2.0 * frequency)); - incStepTable[normalSteps] = normalStepTable[normalSteps - 1] = halfCycleTime; - - halfCyclesPerIncrement = 2 * (unsigned int)((frequency / (rate * zRate)) + 0.5); - if (halfCyclesPerIncrement < 4) - { - halfCyclesPerIncrement = 4; - } - maxIncrements = height * zRate; - -const float peakAccel = fsquare(2.0 * PI * frequency) * amplitude * 0.5; -debugPrintf("halfCycleTime=%u halfCyclesPerIncrement=%u peak accel=%.1f\n", halfCycleTime, halfCyclesPerIncrement, peakAccel); -debugPrintf("normalTable="); -for (unsigned int i = 0; i < normalSteps; ++i) -{ - debugPrintf(" %u", normalStepTable[i]); -} -debugPrintf(" incStepTable="); -for (unsigned int i = 0; i <= normalSteps; ++i) -{ - debugPrintf(" %u", incStepTable[i]); -} -debugPrintf("\n"); - return true; -} - -// Start probing, and return the time that the next step is due -uint32_t DeltaProbe::Start() -{ - // Initialise the dynamic values - stepsDone = 0; - halfCycleCount = 0; - numIncrements = 0; - incrementing = false; - state = State::normal; - return normalStepTable[0]; -} - -bool DeltaProbe::GetDirection() const -{ - return (halfCycleCount & 1) ? FORWARDS : BACKWARDS; -} - -// Calculate the next step time. Returns 0xFFFFFFFF to stop. -uint32_t DeltaProbe::CalcNextStepTime() -{ - if (state == State::stopped || state == State::overran) - { - return 0xFFFFFFFF; - } - - ++stepsDone; - if (stepsDone == ((incrementing) ? normalSteps + 1 : normalSteps)) - { - stepsDone = 0; - ++halfCycleCount; - if (state == State::stopping && (halfCycleCount & 1) == 0) - { - state = State::stopped; - return 0xFFFFFFFF; - } - - if (incrementing) - { - ++numIncrements; - incrementing = false; - } - - if (halfCycleCount == halfCyclesPerIncrement) - { - if (numIncrements == maxIncrements) - { - state = State::overran; // another increment is due, but we have already gone down as far as we were asked to - return 0xFFFFFFFF; - } - halfCycleCount = 0; - incrementing = true; - } - } - - return (incrementing) - ? (halfCyclesPerIncrement * numIncrements * halfCycleTime) + incStepTable[stepsDone] - : (halfCyclesPerIncrement * numIncrements * halfCycleTime) + normalStepTable[stepsDone]; -} - -void DeltaProbe::Trigger() -{ - if (state == State::normal) - { - state = State::stopping; - } -} - -// End diff --git a/src/Movement/DeltaProbe.h b/src/Movement/DeltaProbe.h deleted file mode 100644 index 90b2fb49..00000000 --- a/src/Movement/DeltaProbe.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * DeltaProbe.h - * - * Created on: 20 Apr 2015 - * Author: David - */ - -#ifndef DELTAPROBE_H_ -#define DELTAPROBE_H_ - -#include "RepRapFirmware.h" - -// Class to hold the parameters for my new Z probing method -class DeltaProbe -{ - enum class State { normal, stopping, stopped, overran }; - - // Fixed parameters - static const unsigned int MaxSteps = 30; // 15 corresponds to 0.375mm p-p movement @ 80 steps/mm - - // Static parameters, set up before we start probing and unchanged during probing - unsigned int normalSteps; // the number of steps we use to achieve the requested amplitude - unsigned int halfCyclesPerIncrement; // how many half cycles between lowering the head by 1 step - unsigned int maxIncrements; // max number of steps we lower the head - uint32_t halfCycleTime; // how many interrupt clocks per quarter cycle - uint32_t normalStepTable[MaxSteps]; // table of step times for the first half cycle, in interrupt clocks from start - uint32_t incStepTable[MaxSteps + 1]; // table of step times for the first half cycle, when we are moving down a step - - // Dynamic parameters, to track the progress of the probe - unsigned int stepsDone; // how many steps since the start of this quarter cycle - unsigned int halfCycleCount; // how many quarter cycles since we started or lowered the head - unsigned int numIncrements; // how many steps we have lowered the head since we started - bool incrementing; // true if we are lowering the head 2 step in this half cycle - State state; // what state the probe is in - -public: - bool Init(float frequency, float amplitude, float rate, float height); // Get ready to probe - uint32_t Start(); // start the process, return the next step time - bool GetDirection() const; // get the direction for the current step - uint32_t CalcNextStepTime(); // calculate when the next step is due - void Trigger(); // cease probing - bool Finished() const { return state == State::stopped || state == State::overran; } - bool Overran() const { return state == State::overran; } -}; - -#endif /* DELTAPROBE_H_ */ diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp index f9b6061b..e208443a 100644 --- a/src/Movement/DriveMovement.cpp +++ b/src/Movement/DriveMovement.cpp @@ -85,7 +85,7 @@ void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, bo mp.cart.twoCsquaredTimesMmPerStepDivA = (uint64_t)(((float)DDA::stepClockRate * (float)DDA::stepClockRate)/(stepsPerMm * dda.acceleration)) * 2; // Calculate the pressure advance parameter - const float compensationTime = (doCompensation && dv > 0.0) ? reprap.GetPlatform().GetPressureAdvance(drive - reprap.GetGCodes().GetNumAxes()) : 0.0; + const float compensationTime = (doCompensation && dv > 0.0) ? reprap.GetPlatform().GetPressureAdvance(drive - reprap.GetGCodes().GetTotalAxes()) : 0.0; const uint32_t compensationClocks = (uint32_t)(compensationTime * DDA::stepClockRate); // Calculate the net total step count to allow for compensation. It may be negative. diff --git a/src/Movement/Kinematics/CartesianKinematics.cpp b/src/Movement/Kinematics/CartesianKinematics.cpp index a6a75994..5aa8a1f9 100644 --- a/src/Movement/Kinematics/CartesianKinematics.cpp +++ b/src/Movement/Kinematics/CartesianKinematics.cpp @@ -18,9 +18,9 @@ const char *CartesianKinematics::GetName(bool forStatusReport) const } // Convert Cartesian coordinates to motor coordinates -bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numAxes, int32_t motorPos[]) const +bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const { - for (size_t axis = 0; axis < numAxes; ++axis) + for (size_t axis = 0; axis < numVisibleAxes; ++axis) { motorPos[axis] = (int32_t)roundf(machinePos[axis] * stepsPerMm[axis]); } @@ -28,10 +28,10 @@ bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const } // Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves. -void CartesianKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const +void CartesianKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { - // Convert all axes and the extruders - for (size_t drive = 0; drive < numDrives; ++drive) + // Convert all axes + for (size_t drive = 0; drive < numVisibleAxes; ++drive) { machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; } diff --git a/src/Movement/Kinematics/CartesianKinematics.h b/src/Movement/Kinematics/CartesianKinematics.h index 58a4397b..d2bd6118 100644 --- a/src/Movement/Kinematics/CartesianKinematics.h +++ b/src/Movement/Kinematics/CartesianKinematics.h @@ -17,8 +17,8 @@ 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 numAxes, int32_t motorPos[]) const override; - void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) 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 false; } }; diff --git a/src/Movement/Kinematics/CoreBaseKinematics.cpp b/src/Movement/Kinematics/CoreBaseKinematics.cpp index 1054d285..15013557 100644 --- a/src/Movement/Kinematics/CoreBaseKinematics.cpp +++ b/src/Movement/Kinematics/CoreBaseKinematics.cpp @@ -10,16 +10,16 @@ CoreBaseKinematics::CoreBaseKinematics(KinematicsType t) : Kinematics(t) { - for (size_t axis = 0; axis < CART_AXES; ++axis) + for (size_t axis = 0; axis < XYZ_AXES; ++axis) { axisFactors[axis] = 1.0; } } // Convert Cartesian coordinates to motor coordinates -bool CoreBaseKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numAxes, int32_t motorPos[]) const +bool CoreBaseKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const { - for (size_t axis = 0; axis < numAxes; ++axis) + for (size_t axis = 0; axis < numVisibleAxes; ++axis) { motorPos[axis] = (int32_t)roundf(MotorFactor(axis, machinePos) * stepsPerMm[axis]); } @@ -28,12 +28,12 @@ bool CoreBaseKinematics::CartesianToMotorSteps(const float machinePos[], const f // Set the parameters from a M665, M666 or M669 command // Return true if we changed any parameters. Set 'error' true if there was an error, otherwise leave it alone. -bool CoreBaseKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/ +bool CoreBaseKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/ { if (mCode == 667) { bool seen = false; - for (size_t axis = 0; axis < CART_AXES; ++axis) + for (size_t axis = 0; axis < XYZ_AXES; ++axis) { if (gb.Seen(GCodes::axisLetters[axis])) { @@ -44,7 +44,7 @@ bool CoreBaseKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& if (!seen && !gb.Seen('S')) { reply.printf("Printer mode is %s with axis factors", GetName()); - for (size_t axis = 0; axis < CART_AXES; ++axis) + for (size_t axis = 0; axis < XYZ_AXES; ++axis) { reply.catf(" %c:%f", GCodes::axisLetters[axis], axisFactors[axis]); } @@ -53,7 +53,7 @@ bool CoreBaseKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& } else { - return Kinematics::SetOrReportParameters(mCode, gb, reply, error); + return Kinematics::Configure(mCode, gb, reply, error); } } diff --git a/src/Movement/Kinematics/CoreBaseKinematics.h b/src/Movement/Kinematics/CoreBaseKinematics.h index c532f0ba..1d684830 100644 --- a/src/Movement/Kinematics/CoreBaseKinematics.h +++ b/src/Movement/Kinematics/CoreBaseKinematics.h @@ -16,8 +16,8 @@ public: CoreBaseKinematics(KinematicsType t); // Overridden base class functions. See Kinematics.h for descriptions. - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numAxes, int32_t motorPos[]) const override final; - bool SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override final; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override final; + bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override final; bool SupportsAutoCalibration() const override final { return false; } protected: @@ -25,7 +25,7 @@ protected: // The default implementation just returns directionVector[drive] but this needs to be overridden for CoreXY and CoreXZ printers. virtual float MotorFactor(size_t drive, const float directionVector[]) const = 0; - float axisFactors[CART_AXES]; + float axisFactors[XYZ_AXES]; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREBASEKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreXYKinematics.cpp b/src/Movement/Kinematics/CoreXYKinematics.cpp index 583b370d..55258509 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 motor coordinates to machine coordinates. Used after homing and after individual motor moves. -void CoreXYKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const +void CoreXYKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { // Convert the axes machinePos[X_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Y_AXIS]) - (motorPos[Y_AXIS] * stepsPerMm[X_AXIS])) @@ -27,8 +27,8 @@ void CoreXYKinematics::MotorStepsToCartesian(const int32_t motorPos[], const flo /(2 * axisFactors[Y_AXIS] * stepsPerMm[X_AXIS] * stepsPerMm[Y_AXIS]); machinePos[Z_AXIS] = motorPos[Z_AXIS]/stepsPerMm[Z_AXIS]; - // Convert any additional axes and the extruders - for (size_t drive = MIN_AXES; drive < numDrives; ++drive) + // Convert any additional axes + for (size_t drive = XYZ_AXES; drive < numVisibleAxes; ++drive) { machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; } diff --git a/src/Movement/Kinematics/CoreXYKinematics.h b/src/Movement/Kinematics/CoreXYKinematics.h index b68fc3cc..d9166904 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; - void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const override; + void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; protected: float MotorFactor(size_t drive, const float directionVector[]) const override; diff --git a/src/Movement/Kinematics/CoreXZKinematics.cpp b/src/Movement/Kinematics/CoreXZKinematics.cpp index a72a49a5..f9baf985 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 motor coordinates to machine coordinates. Used after homing and after individual motor moves. -void CoreXZKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const +void CoreXZKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { machinePos[X_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Z_AXIS]) - (motorPos[Z_AXIS] * stepsPerMm[X_AXIS])) /(2 * axisFactors[X_AXIS] * stepsPerMm[X_AXIS] * stepsPerMm[Z_AXIS]); @@ -26,8 +26,8 @@ void CoreXZKinematics::MotorStepsToCartesian(const int32_t motorPos[], const flo machinePos[Z_AXIS] = ((motorPos[X_AXIS] * stepsPerMm[Z_AXIS]) + (motorPos[Z_AXIS] * stepsPerMm[X_AXIS])) /(2 * axisFactors[Z_AXIS] * stepsPerMm[X_AXIS] * stepsPerMm[Z_AXIS]); - // Convert any additional axes and the extruders - for (size_t drive = MIN_AXES; drive < numDrives; ++drive) + // Convert any additional axes linearly + for (size_t drive = XYZ_AXES; drive < numVisibleAxes; ++drive) { machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; } diff --git a/src/Movement/Kinematics/CoreXZKinematics.h b/src/Movement/Kinematics/CoreXZKinematics.h index 1bdb3074..5809f9f3 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; - void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const override; + void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; uint16_t AxesToHomeBeforeProbing() const override { return (1 << X_AXIS) | (1 << Y_AXIS) | (1 << Z_AXIS); } protected: diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp index 139a283a..058aef71 100644 --- a/src/Movement/Kinematics/Kinematics.cpp +++ b/src/Movement/Kinematics/Kinematics.cpp @@ -28,7 +28,7 @@ Kinematics::Kinematics(KinematicsType t, float segsPerSecond, float minSegLength // Set or report the parameters from a M665, M666 or M669 command // This is the fallback function for when the derived class doesn't use the specified M-code -bool Kinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) +bool Kinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) { reply.printf("M%u parameters do not apply to %s kinematics", mCode, GetName()); error = true; @@ -45,10 +45,10 @@ bool Kinematics::IsReachable(float x, float y) const // Limit the Cartesian position that the user wants to move to // This default implementation just applies the rectangular limits set up by M208 to those axes that have been homed. -void Kinematics::LimitPosition(float coords[], size_t numAxes, uint16_t axesHomed) const +void Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, uint16_t axesHomed) const { const Platform& platform = reprap.GetPlatform(); - for (size_t axis = 0; axis < numAxes; axis++) + for (size_t axis = 0; axis < numVisibleAxes; axis++) { if ((axesHomed & (1 << axis)) != 0) { diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h index 7cec8338..75f5b327 100644 --- a/src/Movement/Kinematics/Kinematics.h +++ b/src/Movement/Kinematics/Kinematics.h @@ -44,11 +44,11 @@ public: // 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. + // then search for parameters used to configure the current kinematics. If any are found, perform appropriate actions and return true. // 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. - virtual bool SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error); + // If 'mCode' does not apply to this kinematics, call the base class version of this function, which will print a suitable error message. + virtual bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error); // Convert Cartesian coordinates to motor positions measured in steps from reference position // 'machinePos' is a set of axis and extruder positions to convert @@ -56,14 +56,14 @@ 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 numAxes, int32_t motorPos[]) const = 0; + virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const = 0; // 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 - virtual void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const = 0; + virtual void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const = 0; // Return true if the kinematics supports auto calibration based on bed probing. // Normally returns false, but overridden for delta kinematics. @@ -91,7 +91,7 @@ public: // Limit the Cartesian position that the user wants to move to // The default implementation just applies the rectangular limits set up by M208 to those axes that have been homed. - virtual void LimitPosition(float coords[], size_t numAxes, uint16_t axesHomed) const; + virtual void LimitPosition(float coords[], size_t numVisibleAxes, uint16_t axesHomed) const; // Return the set of axes that must have been homed before bed probing is allowed // The default implementation requires just X and Y, but some kinematics require additional axes to be homed (e.g. delta, CoreXZ) @@ -100,10 +100,16 @@ public: // Return the initial Cartesian coordinates we assume after switching to this kinematics virtual void GetAssumedInitialPosition(size_t numAxes, float positions[]) const; - // Override this one if any axes do not use the line motion (e.g. for segmentation-free delta motion) + // Override this one if any axes do not use the linear motion code (e.g. for segmentation-free delta motion) virtual MotionType GetMotionType(size_t axis) const { return MotionType::linear; } - // Override the virtual destructor if your constructor allocates any dynamic memory + // Override this if the number of homing buttons (excluding the home all button) is not the same as the number of visible axes (e.g. on a delta printer) + virtual size_t NumHomingButtons(size_t numVisibleAxes) const { return numVisibleAxes; } + + // Override this if the homing buttons are not named after the axes (e.g. SCARA printer) + virtual const char* HomingButtonNames() const { return "XYZUVW"; } + + // Override this virtual destructor if your constructor allocates any dynamic memory virtual ~Kinematics() { } // Factory function to create a particular kinematics object and return a pointer to it. diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp index 6e084be0..c109134c 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp +++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp @@ -131,16 +131,16 @@ 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 numAxes, int32_t motorPos[]) const +bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const { //TODO return false if we can't transform the position - for (size_t axis = 0; axis < min<size_t>(numAxes, DELTA_AXES); ++axis) + for (size_t axis = 0; axis < min<size_t>(numVisibleAxes, DELTA_AXES); ++axis) { motorPos[axis] = (int32_t)roundf(Transform(machinePos, axis) * stepsPerMm[axis]); } // Transform any additional axes linearly - for (size_t axis = DELTA_AXES; axis < numAxes; ++axis) + for (size_t axis = DELTA_AXES; axis < numVisibleAxes; ++axis) { motorPos[axis] = (int32_t)roundf(machinePos[axis] * stepsPerMm[axis]); } @@ -148,12 +148,12 @@ bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], cons } // Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves. -void LinearDeltaKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const +void LinearDeltaKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { InverseTransform(motorPos[A_AXIS]/stepsPerMm[A_AXIS], motorPos[B_AXIS]/stepsPerMm[B_AXIS], motorPos[C_AXIS]/stepsPerMm[C_AXIS], machinePos); // Convert any additional axes linearly - for (size_t drive = DELTA_AXES; drive < numDrives; ++drive) + for (size_t drive = DELTA_AXES; drive < numVisibleAxes; ++drive) { machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; } @@ -166,7 +166,7 @@ bool LinearDeltaKinematics::IsReachable(float x, float y) const } // Limit the Cartesian position that the user wants to move to -void LinearDeltaKinematics::LimitPosition(float coords[], size_t numAxes, uint16_t axesHomed) const +void LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, uint16_t axesHomed) const { const uint16_t allAxes = (1u << X_AXIS) | (1u << Y_AXIS) | (1u << Z_AXIS); if ((axesHomed & allAxes) == allAxes) @@ -541,7 +541,7 @@ float LinearDeltaKinematics::GetTiltCorrection(size_t axis) const // Set the parameters from a M665, M666 or M669 command // Return true if we changed any parameters. Set 'error' true if there was an error, otherwise leave it alone. -bool LinearDeltaKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/ +bool LinearDeltaKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/ { switch(mCode) { @@ -643,7 +643,7 @@ bool LinearDeltaKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffe } default: - return Kinematics::SetOrReportParameters(mCode, gb, reply, error); + return Kinematics::Configure(mCode, gb, reply, error); } } diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h index 1bb0ca4f..54a8371b 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.h +++ b/src/Movement/Kinematics/LinearDeltaKinematics.h @@ -33,19 +33,20 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numAxes, int32_t motorPos[]) const override; - void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) 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; + 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; } void DoAutoCalibration(size_t numFactors, const RandomProbePointSet& probePoints, StringRef& reply) override; void SetCalibrationDefaults() override { Init(); } bool WriteCalibrationParameters(FileStore *f) const override; float GetTiltCorrection(size_t axis) const override; bool IsReachable(float x, float y) const override; - void LimitPosition(float coords[], size_t numAxes, uint16_t axesHomed) const override; + void LimitPosition(float coords[], size_t numVisibleAxes, uint16_t axesHomed) const override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; uint16_t AxesToHomeBeforeProbing() const override { return (1 << X_AXIS) | (1 << Y_AXIS) | (1 << Z_AXIS); } MotionType GetMotionType(size_t axis) const override; + size_t NumHomingButtons(size_t numVisibleAxes) const override { return 0; } // Public functions specific to this class float GetDiagonalSquared() const { return D2; } diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp index c6b29f11..f9b67772 100644 --- a/src/Movement/Kinematics/ScaraKinematics.cpp +++ b/src/Movement/Kinematics/ScaraKinematics.cpp @@ -27,7 +27,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 X axis -bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numAxes, int32_t motorPos[]) const +bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const { // No need to limit x,y to reachable positions here, we already did that in class GCodes const float x = machinePos[X_AXIS]; @@ -85,12 +85,18 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa motorPos[X_AXIS] = theta * RadiansToDegrees * stepsPerMm[X_AXIS]; motorPos[Y_AXIS] = (psi * RadiansToDegrees * stepsPerMm[Y_AXIS]) - (crosstalk[0] * motorPos[X_AXIS]); motorPos[Z_AXIS] = (int32_t)((machinePos[Z_AXIS] * stepsPerMm[Z_AXIS]) - (motorPos[X_AXIS] * crosstalk[1]) - (motorPos[Y_AXIS] * crosstalk[2])); + + // Transform any additional axes linearly + for (size_t axis = XYZ_AXES; axis < numVisibleAxes; ++axis) + { + motorPos[axis] = (int32_t)roundf(machinePos[axis] * stepsPerMm[axis]); + } return true; } // Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves. // For Scara, the X and Y components of stepsPerMm are actually steps per degree angle. -void ScaraKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) const +void ScaraKinematics::MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { const float arm1Angle = ((float)motorPos[X_AXIS]/stepsPerMm[X_AXIS]) * DegreesToRadians; const float arm2Angle = (((float)motorPos[Y_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[0]))/stepsPerMm[Y_AXIS]) * DegreesToRadians; @@ -100,11 +106,17 @@ void ScaraKinematics::MotorStepsToCartesian(const int32_t motorPos[], const floa // On some machines (e.g. Helios), the X and/or Y arm motors also affect the Z height machinePos[Z_AXIS] = ((float)motorPos[Z_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[1]) + ((float)motorPos[Y_AXIS] * crosstalk[2]))/stepsPerMm[Z_AXIS]; + + // Convert any additional axes linearly + for (size_t drive = XYZ_AXES; drive < numVisibleAxes; ++drive) + { + machinePos[drive] = motorPos[drive]/stepsPerMm[drive]; + } } // Set the parameters from a M665, M666 or M669 command // Return true if we changed any parameters. Set 'error' true if there was an error, otherwise leave it alone. -bool ScaraKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/ +bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) /*override*/ { if (mCode == 669) { @@ -143,7 +155,7 @@ bool ScaraKinematics::SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, } else { - return Kinematics::SetOrReportParameters(mCode, gb, reply, error); + return Kinematics::Configure(mCode, gb, reply, error); } } @@ -158,7 +170,7 @@ bool ScaraKinematics::IsReachable(float x, float y) const // Limit the Cartesian position that the user wants to move to // TODO take account of arm angle limits -void ScaraKinematics::LimitPosition(float coords[], size_t numAxes, uint16_t axesHomed) const +void ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, uint16_t axesHomed) const { float& x = coords[X_AXIS]; float& y = coords[Y_AXIS]; diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h index 4c22c832..8e0c13d8 100644 --- a/src/Movement/Kinematics/ScaraKinematics.h +++ b/src/Movement/Kinematics/ScaraKinematics.h @@ -27,13 +27,14 @@ public: // Overridden base class functions. See Kinematics.h for descriptions. const char *GetName(bool forStatusReport) const override; - bool SetOrReportParameters(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override; - bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numAxes, int32_t motorPos[]) const override; - void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numDrives, float machinePos[]) 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; + void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override; bool SupportsAutoCalibration() const override { return false; } bool IsReachable(float x, float y) const override; void LimitPosition(float position[], size_t numAxes, uint16_t axesHomed) const override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; + const char* HomingButtonNames() const override { return "PDZUVW"; } private: static constexpr float DefaultSegmentsPerSecond = 200.0; diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp index de7a2658..e8c09e76 100644 --- a/src/Movement/Move.cpp +++ b/src/Movement/Move.cpp @@ -30,8 +30,6 @@ Move::Move() : currentDda(NULL), scheduledMoves(0), completedMoves(0) void Move::Init() { - deltaProbing = false; - // Empty the ring ddaRingGetPointer = ddaRingCheckPointer = ddaRingAddPointer; DDA *dda = ddaRingAddPointer; @@ -183,88 +181,85 @@ void Move::Spin() } } - if (!deltaProbing) + // See whether we need to kick off a move + if (currentDda == nullptr) { - // See whether we need to kick off a move - if (currentDda == nullptr) + // No DDA is executing, so start executing a new one if possible + if (idleCount > 10) // better to have a few moves in the queue so that we can do lookahead { - // No DDA is executing, so start executing a new one if possible - if (idleCount > 10) // better to have a few moves in the queue so that we can do lookahead + DDA *dda = ddaRingGetPointer; + if (dda->GetState() == DDA::provisional) + { + dda->Prepare(); + } + if (dda->GetState() == DDA::frozen) { - DDA *dda = ddaRingGetPointer; - if (dda->GetState() == DDA::provisional) + if (simulationMode != 0) { - dda->Prepare(); + currentDda = dda; // pretend we are executing this move } - if (dda->GetState() == DDA::frozen) + else { - if (simulationMode != 0) + Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure + if (StartNextMove(Platform::GetInterruptClocks())) // start the next move { - currentDda = dda; // pretend we are executing this move + Interrupt(); } - else - { - Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure - if (StartNextMove(Platform::GetInterruptClocks())) // start the next move - { - Interrupt(); - } - } - iState = IdleState::busy; - } - else if (!simulationMode != 0 && iState == IdleState::busy && !reprap.GetGCodes().IsPaused() && idleTimeout > 0.0) - { - lastMoveTime = reprap.GetPlatform().Time(); // record when we first noticed that the machine was idle - iState = IdleState::timing; - } - else if (!simulationMode != 0 && iState == IdleState::timing && reprap.GetPlatform().Time() - lastMoveTime >= idleTimeout) - { - reprap.GetPlatform().SetDriversIdle(); // put all drives in idle hold - iState = IdleState::idle; } + iState = IdleState::busy; + } + else if (!simulationMode != 0 && iState == IdleState::busy && !reprap.GetGCodes().IsPaused() && idleTimeout > 0.0) + { + lastMoveTime = reprap.GetPlatform().Time(); // record when we first noticed that the machine was idle + iState = IdleState::timing; + } + else if (!simulationMode != 0 && iState == IdleState::timing && reprap.GetPlatform().Time() - lastMoveTime >= idleTimeout) + { + reprap.GetPlatform().SetDriversIdle(); // put all drives in idle hold + iState = IdleState::idle; } } + } - DDA *cdda = currentDda; // currentDda is volatile, so copy it - if (cdda != nullptr) + DDA *cdda = currentDda; // currentDda is volatile, so copy it + if (cdda != nullptr) + { + // See whether we need to prepare any moves + int32_t preparedTime = 0; + uint32_t preparedCount = 0; + DDA::DDAState st; + while ((st = cdda->GetState()) == DDA::completed || st == DDA::executing || st == DDA::frozen) { - // See whether we need to prepare any moves - int32_t preparedTime = 0; - uint32_t preparedCount = 0; - DDA::DDAState st; - while ((st = cdda->GetState()) == DDA::completed || st == DDA::executing || st == DDA::frozen) + preparedTime += cdda->GetTimeLeft(); + ++preparedCount; + cdda = cdda->GetNext(); + if (cdda == ddaRingAddPointer) { - preparedTime += cdda->GetTimeLeft(); - ++preparedCount; - cdda = cdda->GetNext(); - if (cdda == ddaRingAddPointer) - { - break; - } + break; } + } - // If the number of prepared moves will execute in less than the minimum time, prepare another move - while (st == DDA::provisional - && preparedTime < (int32_t)(DDA::stepClockRate/8) // prepare moves one eighth of a second ahead of when they will be needed - && preparedCount < DdaRingLength/2 // but don't prepare more than half the ring - ) - { - cdda->Prepare(); - preparedTime += cdda->GetTimeLeft(); - ++preparedCount; - cdda = cdda->GetNext(); - st = cdda->GetState(); - } + // If the number of prepared moves will execute in less than the minimum time, prepare another move + while (st == DDA::provisional + && preparedTime < (int32_t)(DDA::stepClockRate/8) // prepare moves one eighth of a second ahead of when they will be needed + && preparedCount < DdaRingLength/2 // but don't prepare more than half the ring + ) + { + cdda->Prepare(); + preparedTime += cdda->GetTimeLeft(); + ++preparedCount; + cdda = cdda->GetNext(); + st = cdda->GetState(); + } - // If we are simulating and the move pipeline is reasonably full, simulate completion of the current move - if (simulationMode != 0 && idleCount >= 10) - { - // Simulate completion of the current move + // If we are simulating and the move pipeline is reasonably full, simulate completion of the current move + if (simulationMode != 0 && idleCount >= 10) + { + // Simulate completion of the current move //DEBUG //currentDda->DebugPrint(); - simulationTime += currentDda->CalcTime(); - CurrentMoveCompleted(); - } + simulationTime += currentDda->CalcTime(); + CurrentMoveCompleted(); } } @@ -355,7 +350,7 @@ FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate, ui if (ddaRingAddPointer != savedDdaRingAddPointer) { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); // We are going to skip some moves. dda points to the last move we are going to print. for (size_t axis = 0; axis < numAxes; ++axis) @@ -444,8 +439,8 @@ void Move::Diagnostics(MessageType mtype) char ch = ' '; for (size_t i = 0; i < DDA::numLoggedProbePositions; ++i) { - float xyzPos[CART_AXES]; - MotorStepsToCartesian(DDA::loggedProbePositions + (CART_AXES * i), CART_AXES, xyzPos); + float xyzPos[XYZ_AXES]; + MotorStepsToCartesian(DDA::loggedProbePositions + (XYZ_AXES * i), XYZ_AXES, XYZ_AXES, xyzPos); p.MessageF(mtype, "%c%.2f,%.2f", ch, xyzPos[X_AXIS], xyzPos[Y_AXIS]); ch = ','; } @@ -480,7 +475,7 @@ void Move::EndPointToMachine(const float coords[], int32_t ep[], size_t numDrive { if (CartesianToMotorSteps(coords, ep)) { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t drive = numAxes; drive < numDrives; ++drive) { ep[drive] = MotorEndPointToMachine(drive, coords[drive]); @@ -496,16 +491,16 @@ int32_t Move::MotorEndPointToMachine(size_t drive, float coord) // Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves. // This is computationally expensive on a delta or SCARA machine, so only call it when necessary, and never from the step ISR. -void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numDrives, float machinePos[]) const +void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { - kinematics->MotorStepsToCartesian(motorPos, reprap.GetPlatform().GetDriveStepsPerUnit(), numDrives, machinePos); + kinematics->MotorStepsToCartesian(motorPos, reprap.GetPlatform().GetDriveStepsPerUnit(), numVisibleAxes, numTotalAxes, machinePos); } // 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[MAX_AXES], int32_t motorPos[MAX_AXES]) const +bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const { - const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), reprap.GetGCodes().GetNumAxes(), motorPos); + const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos); if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda)) { if (b) @@ -520,7 +515,7 @@ bool Move::CartesianToMotorSteps(const float machinePos[MAX_AXES], int32_t motor return b; } -void Move::AxisAndBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes, bool useBedCompensation) const +void Move::AxisAndBedTransform(float xyzPoint[MaxAxes], uint32_t xAxes, bool useBedCompensation) const { AxisTransform(xyzPoint); if (useBedCompensation) @@ -529,14 +524,14 @@ void Move::AxisAndBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes, bool us } } -void Move::InverseAxisAndBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const +void Move::InverseAxisAndBedTransform(float xyzPoint[MaxAxes], uint32_t xAxes) const { InverseBedTransform(xyzPoint, xAxes); InverseAxisTransform(xyzPoint); } // Do the Axis transform BEFORE the bed transform -void Move::AxisTransform(float xyzPoint[MAX_AXES]) const +void Move::AxisTransform(float xyzPoint[MaxAxes]) const { //TODO should we transform U axis instead of/as well as X if we are in dual carriage mode? xyzPoint[X_AXIS] += tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS]; @@ -544,7 +539,7 @@ void Move::AxisTransform(float xyzPoint[MAX_AXES]) const } // Invert the Axis transform AFTER the bed transform -void Move::InverseAxisTransform(float xyzPoint[MAX_AXES]) const +void Move::InverseAxisTransform(float xyzPoint[MaxAxes]) const { //TODO should we transform U axis instead of/as well as X if we are in dual carriage mode? xyzPoint[Y_AXIS] -= tanYZ*xyzPoint[Z_AXIS]; @@ -552,12 +547,12 @@ void Move::InverseAxisTransform(float xyzPoint[MAX_AXES]) const } // Do the bed transform AFTER the axis transform -void Move::BedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const +void Move::BedTransform(float xyzPoint[MaxAxes], uint32_t xAxes) const { if (!useTaper || xyzPoint[Z_AXIS] < taperHeight) { float zCorrection = 0.0; - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); unsigned int numXAxes = 0; // Transform the Z coordinate based on the average correction for each axis used as an X axis. @@ -589,10 +584,10 @@ void Move::BedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const } // Invert the bed transform BEFORE the axis transform -void Move::InverseBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const +void Move::InverseBedTransform(float xyzPoint[MaxAxes], uint32_t xAxes) const { float zCorrection = 0.0; - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); unsigned int numXAxes = 0; // Transform the Z coordinate based on the average correction for each axis used as an X axis. @@ -759,49 +754,6 @@ void Move::AdjustMotorPositions(const float_t adjustment[], size_t numMotors) liveCoordinatesValid = false; // force the live XYZ position to be recalculated } -static void ShortDelay() -{ - for (unsigned int i = 0; i < 10; ++i) - { - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - } -} - -// This is the function that is called by the timer interrupt to step the motors when we are using the experimental delta probe. -// The movements are quite slow so it is not time-critical. -void Move::DeltaProbeInterrupt() -{ - bool again; - do - { - if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit) - { - deltaProbe.Trigger(); - } - - const bool dir = deltaProbe.GetDirection(); - Platform& platform = reprap.GetPlatform(); - platform.SetDirection(X_AXIS, dir); - platform.SetDirection(Y_AXIS, dir); - platform.SetDirection(Z_AXIS, dir); - ShortDelay(); - const uint32_t steppersMoving = platform.GetDriversBitmap(X_AXIS) | platform.GetDriversBitmap(Y_AXIS) | platform.GetDriversBitmap(Z_AXIS); - Platform::StepDriversHigh(steppersMoving); - ShortDelay(); - Platform::StepDriversLow(); - const uint32_t tim = deltaProbe.CalcNextStepTime(); - again = (tim != 0xFFFFFFFF && platform.ScheduleInterrupt(tim + deltaProbingStartTime)); - } while (again); -} - // This is called from the step ISR when the current move has been completed void Move::CurrentMoveCompleted() { @@ -837,7 +789,7 @@ bool Move::TryStartNextMove(uint32_t startTime) // This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'. void Move::HitLowStop(size_t axis, DDA* hitDDA) { - if (axis < reprap.GetGCodes().GetNumAxes() && !IsDeltaMode()) // should always be true + if (axis < reprap.GetGCodes().GetTotalAxes() && !IsDeltaMode()) // should always be true { JustHomed(axis, reprap.GetPlatform().AxisMinimum(axis), hitDDA); } @@ -846,7 +798,7 @@ void Move::HitLowStop(size_t axis, DDA* hitDDA) // This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'. void Move::HitHighStop(size_t axis, DDA* hitDDA) { - if (axis < reprap.GetGCodes().GetNumAxes()) // should always be true + if (axis < reprap.GetGCodes().GetTotalAxes()) // should always be true { const float hitPoint = (IsDeltaMode()) ? ((LinearDeltaKinematics*)kinematics)->GetHomedCarriageHeight(axis) // this is a delta printer, so the motor is at the homed carriage height for this drive @@ -860,13 +812,13 @@ void Move::JustHomed(size_t axisHomed, float hitPoint, DDA* hitDDA) { if (IsCoreXYAxis(axisHomed)) { - float tempCoordinates[CART_AXES]; - for (size_t axis = 0; axis < CART_AXES; ++axis) + float tempCoordinates[XYZ_AXES]; + for (size_t axis = 0; axis < XYZ_AXES; ++axis) { tempCoordinates[axis] = hitDDA->GetEndCoordinate(axis, false); } tempCoordinates[axisHomed] = hitPoint; - hitDDA->SetPositions(tempCoordinates, CART_AXES); + hitDDA->SetPositions(tempCoordinates, XYZ_AXES); } else { @@ -887,7 +839,7 @@ void Move::ZProbeTriggered(DDA* hitDDA) void Move::GetCurrentMachinePosition(float m[DRIVES], bool disableMotorMapping) const { DDA * const lastQueuedMove = ddaRingAddPointer->GetPrevious(); - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); for (size_t i = 0; i < DRIVES; i++) { if (i < numAxes) @@ -930,7 +882,7 @@ void Move::GetCurrentUserPosition(float m[DRIVES], uint8_t moveType, uint32_t xA void Move::LiveCoordinates(float m[DRIVES], uint32_t xAxes) { // The live coordinates and live endpoints are modified by the ISR, so be careful to get a self-consistent set of them - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); // do this before we disable interrupts + const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes(); // do this before we disable interrupts cpu_irq_disable(); if (liveCoordinatesValid) { @@ -940,19 +892,20 @@ void Move::LiveCoordinates(float m[DRIVES], uint32_t xAxes) } else { + const size_t numTotalAxes = reprap.GetGCodes().GetTotalAxes(); // do this before we disable interrupts // Only the extruder coordinates are valid, so we need to convert the motor endpoints to coordinates - memcpy(m + numAxes, const_cast<const float *>(liveCoordinates + numAxes), sizeof(m[0]) * (DRIVES - numAxes)); - int32_t tempEndPoints[MAX_AXES]; + memcpy(m + numTotalAxes, const_cast<const float *>(liveCoordinates + numTotalAxes), sizeof(m[0]) * (DRIVES - numTotalAxes)); + int32_t tempEndPoints[MaxAxes]; memcpy(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints)); cpu_irq_enable(); - MotorStepsToCartesian(tempEndPoints, numAxes, m); // this is slow, so do it with interrupts enabled + MotorStepsToCartesian(tempEndPoints, numVisibleAxes, numTotalAxes, m); // this is slow, so do it with interrupts enabled // If the ISR has not updated the endpoints, store the live coordinates back so that we don't need to do it again cpu_irq_disable(); if (memcmp(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints)) == 0) { - memcpy(const_cast<float *>(liveCoordinates), m, sizeof(m[0]) * numAxes); + memcpy(const_cast<float *>(liveCoordinates), m, sizeof(m[0]) * numVisibleAxes); liveCoordinatesValid = true; } cpu_irq_enable(); @@ -969,13 +922,13 @@ void Move::SetLiveCoordinates(const float coords[DRIVES]) liveCoordinates[drive] = coords[drive]; } liveCoordinatesValid = true; - EndPointToMachine(coords, const_cast<int32_t *>(liveEndPoints), reprap.GetGCodes().GetNumAxes()); + EndPointToMachine(coords, const_cast<int32_t *>(liveEndPoints), reprap.GetGCodes().GetVisibleAxes()); } void Move::ResetExtruderPositions() { cpu_irq_disable(); - for (size_t eDrive = reprap.GetGCodes().GetNumAxes(); eDrive < DRIVES; eDrive++) + for (size_t eDrive = reprap.GetGCodes().GetTotalAxes(); eDrive < DRIVES; eDrive++) { liveCoordinates[eDrive] = 0.0; } @@ -1055,47 +1008,4 @@ bool Move::IsCoreXYAxis(size_t axis) const } } -// Do a delta probe returning -1 if still probing, 0 if failed, 1 if success -int Move::DoDeltaProbe(float frequency, float amplitude, float rate, float distance) -{ - if (deltaProbing) - { - if (deltaProbe.Finished()) - { - deltaProbing = false; - return (deltaProbe.Overran()) ? 0 : 1; - } - } - else - { - if (currentDda != nullptr || !DDARingEmpty()) - { - return 0; - } - if (!deltaProbe.Init(frequency, amplitude, rate, distance)) - { - return 0; - } - - const uint32_t firstInterruptTime = deltaProbe.Start(); - if (firstInterruptTime != 0xFFFFFFFF) - { - Platform& platform = reprap.GetPlatform(); - platform.EnableDrive(X_AXIS); - platform.EnableDrive(Y_AXIS); - platform.EnableDrive(Z_AXIS); - deltaProbing = true; - iState = IdleState::busy; - const irqflags_t flags = cpu_irq_save(); - deltaProbingStartTime = platform.GetInterruptClocks(); - if (platform.ScheduleInterrupt(firstInterruptTime + deltaProbingStartTime)) - { - Interrupt(); - } - cpu_irq_restore(flags); - } - } - return -1; -} - // End diff --git a/src/Movement/Move.h b/src/Movement/Move.h index 06d99816..e2eeac41 100644 --- a/src/Movement/Move.h +++ b/src/Movement/Move.h @@ -15,7 +15,6 @@ #include "BedProbing/RandomProbePointSet.h" #include "BedProbing/Grid.h" #include "Kinematics/Kinematics.h" -#include "DeltaProbe.h" #ifdef DUET_NG const unsigned int DdaRingLength = 30; @@ -67,9 +66,9 @@ 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[MAX_AXES], int32_t motorPos[MAX_AXES]) const; + bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const; // Convert Cartesian coordinates to delta motor coordinates, return true if successful - void MotorStepsToCartesian(const int32_t motorPos[], size_t numDrives, float machinePos[]) const; + void MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const; // Convert motor coordinates to machine coordinates void EndPointToMachine(const float coords[], int32_t ep[], size_t numDrives) const; void AdjustMotorPositions(const float_t adjustment[], size_t numMotors); // Perform motor endpoint adjustment @@ -93,8 +92,6 @@ public: FilePosition PausePrint(float positions[DRIVES], float& pausedFeedRate, uint32_t xAxes); // Pause the print as soon as we can bool NoLiveMovement() const; // Is a move running, or are there any queued? - int DoDeltaProbe(float frequency, float amplitude, float rate, float distance); - bool IsExtruding() const; // Is filament being extruded? uint32_t GetScheduledMoves() const { return scheduledMoves; } // How many moves have been scheduled? @@ -110,12 +107,11 @@ private: enum class IdleState : uint8_t { idle, busy, timing }; bool StartNextMove(uint32_t startTime); // start the next move, returning true if Step() needs to be called immediately - void BedTransform(float move[MAX_AXES], uint32_t xAxes) const; // Take a position and apply the bed compensations - void InverseBedTransform(float move[MAX_AXES], uint32_t xAxes) const; // Go from a bed-transformed point back to user coordinates - void AxisTransform(float move[MAX_AXES]) const; // Take a position and apply the axis-angle compensations - void InverseAxisTransform(float move[MAX_AXES]) const; // Go from an axis transformed point back to user coordinates + void BedTransform(float move[MaxAxes], uint32_t xAxes) const; // Take a position and apply the bed compensations + void InverseBedTransform(float move[MaxAxes], uint32_t xAxes) const; // Go from a bed-transformed point back to user coordinates + void AxisTransform(float move[MaxAxes]) const; // Take a position and apply the axis-angle compensations + void InverseAxisTransform(float move[MaxAxes]) const; // Go from an axis transformed point back to user coordinates void JustHomed(size_t axis, float hitPoint, DDA* hitDDA); // Deal with setting positions after a drive has been homed - void DeltaProbeInterrupt(); // Step ISR when using the experimental delta probe bool DDARingAdd(); // Add a processed look-ahead entry to the DDA ring DDA* DDARingGet(); // Get the next DDA ring entry to be run @@ -158,11 +154,6 @@ private: unsigned int stepErrors; // count of step errors, for diagnostics uint32_t scheduledMoves; // Move counters for the code queue volatile uint32_t completedMoves; // This one is modified by an ISR, hence volatile - - // Parameters for the experimental accoustic delta probe - DeltaProbe deltaProbe; // Delta probing state - uint32_t deltaProbingStartTime; - bool deltaProbing; }; //****************************************************************************************************** @@ -202,10 +193,6 @@ inline void Move::Interrupt() { } while (currentDda->Step()); } - else if (deltaProbing) - { - DeltaProbeInterrupt(); - } } #endif /* MOVE_H_ */ |