diff options
Diffstat (limited to 'src/Movement')
23 files changed, 1095 insertions, 435 deletions
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index a76a25c6..896e588c 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -26,17 +26,30 @@ struct MoveParameters float accelDistance; float steadyDistance; float decelDistance; + float requestedSpeed; float startSpeed; float topSpeed; float endSpeed; + float targetNextSpeed; + uint32_t endstopChecks; + uint16_t flags; MoveParameters() { - accelDistance = steadyDistance = decelDistance = startSpeed = topSpeed = endSpeed = 0.0; + accelDistance = steadyDistance = decelDistance = requestedSpeed = startSpeed = topSpeed = endSpeed = targetNextSpeed = 0.0; + endstopChecks = 0; + flags = 0; + } + + void DebugPrint() const + { + reprap.GetPlatform().MessageF(DebugMessage, "%f,%f,%f,%f,%f,%f,%f,%f,%08" PRIX32 ",%04x\n", + (double)accelDistance, (double)steadyDistance, (double)decelDistance, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed, + (double)targetNextSpeed, endstopChecks, flags); } }; -const size_t NumSavedMoves = 256; +const size_t NumSavedMoves = 128; static MoveParameters savedMoves[NumSavedMoves]; static size_t savedMovePointer = 0; @@ -47,8 +60,7 @@ static size_t savedMovePointer = 0; // Print the saved moved in CSV format for (size_t i = 0; i < NumSavedMoves; ++i) { - const MoveParameters& m = savedMoves[savedMovePointer]; - reprap.GetPlatform().MessageF(DebugMessage, "%f,%f,%f,%f,%f,%f\n", m.accelDistance, m.steadyDistance, m.decelDistance, m.startSpeed, m.topSpeed, m.endSpeed); + savedMoves[savedMovePointer].DebugPrint(); savedMovePointer = (savedMovePointer + 1) % NumSavedMoves; } } @@ -132,22 +144,21 @@ inline void DDA::InsertDM(DriveMovement *dm) *dmp = dm; } -// Remove this drive from the list of drives with steps due, and return its DM or nullptr if not there +// Remove this drive from the list of drives with steps due // Called from the step ISR only. -DriveMovement *DDA::RemoveDM(size_t drive) +void DDA::RemoveDM(size_t drive) { DriveMovement **dmp = &firstDM; while (*dmp != nullptr) { - DriveMovement *dm = *dmp; + DriveMovement * const dm = *dmp; if (dm->drive == drive) { (*dmp) = dm->nextDM; - return dm; + break; } dmp = &(dm->nextDM); } - return nullptr; } void DDA::DebugPrintVector(const char *name, const float *vec, size_t len) const @@ -225,7 +236,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) const Move& move = reprap.GetMove(); if (doMotorMapping) { - if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, !nextMove.isCoordinated)) // 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 } @@ -237,7 +248,6 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) isDeltaMovement = false; } - isPrintingMove = false; xyMoving = false; bool extruding = false; // we set this true if extrusion was commanded, even if it is too small to do bool realMove = false; @@ -338,6 +348,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) isPrintingMove = xyMoving && extruding; usePressureAdvance = nextMove.usePressureAdvance; virtualExtruderPosition = nextMove.virtualExtruderPosition; + proportionRemaining = nextMove.proportionRemaining; hadLookaheadUnderrun = false; isLeadscrewAdjustmentMove = false; goingSlow = false; @@ -417,41 +428,13 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) // for diagonal moves. On a delta, this is not OK and any movement in the XY plane should be limited to the X/Y axis values, which we assume to be equal. if (doMotorMapping) { - switch (reprap.GetMove().GetKinematics().GetKinematicsType()) - { - case KinematicsType::cartesian: - // On a Cartesian printer the axes accelerate independently - break; - - case KinematicsType::coreXY: - case KinematicsType::coreXYU: - // Preferably we would specialise these, but for now fall through to the default implementation - default: - // On other types of printer, the relationship between motor movement and head movement is complex. - // Limit all moves to the lower of X and Y speeds and accelerations. - { - const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); - const float * const maxSpeeds = reprap.GetPlatform().MaxFeedrates(); - const float maxSpeed = min<float>(maxSpeeds[X_AXIS], maxSpeeds[Y_AXIS]); - if (requestedSpeed * xyFactor > maxSpeed) - { - requestedSpeed = maxSpeed/xyFactor; - } - - const float maxAcceleration = min<float>(normalAccelerations[X_AXIS], normalAccelerations[Y_AXIS]); - if (acceleration * xyFactor > maxAcceleration) - { - acceleration = maxAcceleration/xyFactor; - } - } - break; - } + reprap.GetMove().GetKinematics().LimitSpeedAndAcceleration(*this, normalisedDirectionVector); // give the kinematics the chance to further restrict the speed and acceleration } // 7. Calculate the provisional accelerate and decelerate distances and the top speed endSpeed = 0.0; // until the next move asks us to adjust it - if (prev->state != provisional || isPrintingMove != prev->isPrintingMove) + if (prev->state != provisional || isPrintingMove != prev->isPrintingMove || xyMoving != prev->xyMoving) { // There is no previous move that we can adjust, so this move must start at zero speed. startSpeed = 0.0; @@ -460,7 +443,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping) { // Try to meld this move to the previous move to avoid stop/start // Assuming that this move ends with zero speed, calculate the maximum possible starting speed: u^2 = v^2 - 2as - prev->targetNextSpeed = min<float>(sqrtf(acceleration * totalDistance * 2.0), requestedSpeed); + prev->targetNextSpeed = sqrtf(acceleration * totalDistance * 2.0); DoLookahead(prev); startSpeed = prev->targetNextSpeed; } @@ -562,61 +545,73 @@ inline bool DDA::IsDecelerationMove() const && prev->decelDistance > 0.0); // if the previous move has no deceleration phase then no point in adjus6ting it } -// Try to increase the ending speed of this move to allow the next move to start at targetNextSpeed +// Return true if there is no reason to delay preparing this move +bool DDA::IsGoodToPrepare() const +{ + return endSpeed >= topSpeed; // if it never decelerates, we can't improve it +} + +// Try to increase the ending speed of this move to allow the next move to start at targetNextSpeed. +// Only called if this move ands the next one are both printing moves. /*static*/ void DDA::DoLookahead(DDA *laDDA) pre(state == provisional) { // if (reprap.Debug(moduleDda)) debugPrintf("Adjusting, %f\n", laDDA->targetNextSpeed); unsigned int laDepth = 0; - bool goingUp = true; + bool recurse = true; for(;;) // this loop is used to nest lookahead without making recursive calls { - bool recurse = false; - if (goingUp) + if (recurse) { // We have been asked to adjust the end speed of this move to match the next move starting at targetNextSpeed + if (laDDA->targetNextSpeed > laDDA->requestedSpeed) + { + laDDA->targetNextSpeed = laDDA->requestedSpeed; + } if (laDDA->topSpeed >= laDDA->requestedSpeed) { // This move already reaches its top speed, so just need to adjust the deceleration part - laDDA->endSpeed = laDDA->requestedSpeed; // remove the deceleration phase - laDDA->CalcNewSpeeds(); // put it back if necessary + laDDA->endSpeed = laDDA->targetNextSpeed; // ideally, maintain constant speed between the two moves + laDDA->CalcNewSpeeds(); // adjust it if necessary + recurse = false; } else if (laDDA->IsDecelerationMove()) { // This is a deceleration-only move, so we may have to adjust the previous move as well to get optimum behaviour - if (laDDA->prev->state == provisional) + if (laDDA->prev->state == provisional && laDDA->prev->isPrintingMove == laDDA->isPrintingMove && laDDA->prev->xyMoving == laDDA->xyMoving) { - laDDA->endSpeed = laDDA->requestedSpeed; + laDDA->endSpeed = laDDA->targetNextSpeed; laDDA->CalcNewSpeeds(); const float maxStartSpeed = sqrtf(fsquare(laDDA->endSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); - if (maxStartSpeed < laDDA->requestedSpeed) - { - laDDA->prev->targetNextSpeed = maxStartSpeed; - // Still provisionally a decelerate-only move - } - else - { - laDDA->prev->targetNextSpeed = laDDA->requestedSpeed; - } - recurse = true; + laDDA->prev->targetNextSpeed = min<float>(maxStartSpeed, laDDA->requestedSpeed); + // leave 'recurse' true } else { // This move is a deceleration-only move but we can't adjust the previous one laDDA->hadLookaheadUnderrun = true; - laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), - laDDA->requestedSpeed); + laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), laDDA->requestedSpeed); laDDA->CalcNewSpeeds(); + recurse = false; } } else { // This move doesn't reach its requested speed, but it isn't a deceleration-only move // Set its end speed to the minimum of the requested speed and the highest we can reach - laDDA->endSpeed = min<float>(sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)), - laDDA->requestedSpeed); + const float maxReachableSpeed = sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); + if (maxReachableSpeed >= laDDA->targetNextSpeed) + { + laDDA->endSpeed = laDDA->targetNextSpeed; + } + else + { + // Looks like this is an acceleration segment, so to ensure smooth acceleration we should reduce targetNextSpeed to endSpeed as well + laDDA->targetNextSpeed = laDDA->endSpeed = maxReachableSpeed; + } laDDA->CalcNewSpeeds(); + recurse = false; } } else @@ -646,7 +641,7 @@ pre(state == provisional) } else { - // Either just stopped going up. or going down + // Either just stopped going up, or going down laDDA->RecalculateMove(); if (laDepth == 0) @@ -657,7 +652,6 @@ pre(state == provisional) laDDA = laDDA->next; --laDepth; - goingUp = false; } } } @@ -758,18 +752,15 @@ float DDA::AdvanceBabyStepping(float amount) pdm->state = DMState::moving; } - if (pdm != nullptr) + if (steps >= 0) { - if (steps >= 0) - { - pdm->direction = true; - pdm->totalSteps = (uint32_t)steps; - } - else - { - pdm->direction = false; - pdm->totalSteps = (uint32_t)(-steps); - } + pdm->direction = true; + pdm->totalSteps = (uint32_t)steps; + } + else + { + pdm->direction = false; + pdm->totalSteps = (uint32_t)(-steps); } } } @@ -785,19 +776,19 @@ float DDA::AdvanceBabyStepping(float amount) // This may cause a move that we intended to be a deceleration-only move to have a tiny acceleration segment at the start void DDA::RecalculateMove() { - accelDistance = (fsquare(requestedSpeed) - fsquare(startSpeed))/(2.0 * acceleration); - decelDistance = (fsquare(requestedSpeed) - fsquare(endSpeed))/(2.0 * acceleration); - if (accelDistance + decelDistance >= totalDistance) + const float accelDiff = fsquare(requestedSpeed) - fsquare(startSpeed); + const float decelDiff = fsquare(requestedSpeed) - fsquare(endSpeed); + if (accelDiff + decelDiff >= acceleration * totalDistance * 2) { - // This move has no steady-speed phase, so it's accelerate-decelerate or accelerate-only move. + // This move has no steady-speed phase, so it's accelerate-decelerate or accelerate-only or decelerate-only move. // If V is the peak speed, then (V^2 - u^2)/2a + (V^2 - v^2)/2a = distance. // So (2V^2 - u^2 - v^2)/2a = distance // So V^2 = a * distance + 0.5(u^2 + v^2) - float vsquared = (acceleration * totalDistance) + 0.5 * (fsquare(startSpeed) + fsquare(endSpeed)); + const float vsquared = (acceleration * totalDistance) + 0.5 * (fsquare(startSpeed) + fsquare(endSpeed)); // Calculate accelerate distance from: V^2 = u^2 + 2as if (vsquared >= 0.0) { - accelDistance = max<float>((vsquared - fsquare(startSpeed))/(2.0 * acceleration), 0.0); + accelDistance = max<float>((vsquared - fsquare(startSpeed))/(2 * acceleration), 0.0); decelDistance = totalDistance - accelDistance; topSpeed = sqrtf(vsquared); } @@ -811,20 +802,22 @@ void DDA::RecalculateMove() accelDistance = totalDistance; decelDistance = 0.0; topSpeed = endSpeed; - acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/(2.0 * totalDistance); + acceleration = (fsquare(endSpeed) - fsquare(startSpeed))/(2 * totalDistance); } else { accelDistance = 0.0; decelDistance = totalDistance; topSpeed = startSpeed; - acceleration = (fsquare(startSpeed) - fsquare(endSpeed))/(2.0 * totalDistance); + acceleration = (fsquare(startSpeed) - fsquare(endSpeed))/(2 * totalDistance); } } } else { topSpeed = requestedSpeed; + accelDistance = accelDiff/(2 * acceleration); + decelDistance = decelDiff/(2 * acceleration); } if (canPauseAfter && endSpeed != 0.0) @@ -839,6 +832,12 @@ void DDA::RecalculateMove() } } } + + // We need to set the number of clocks needed here because we use it before the move has been frozen + const float accelStopTime = (topSpeed - startSpeed)/acceleration; + const float decelStartTime = accelStopTime + (totalDistance - accelDistance - decelDistance)/topSpeed; + const float totalTime = decelStartTime + (topSpeed - endSpeed)/acceleration; + clocksNeeded = (uint32_t)(totalTime * stepClockRate); } // Decide what speed we would really like this move to end at. @@ -963,12 +962,6 @@ pre(disableDeltaMapping || drive < MaxAxes) } } -// Calculate the time needed for this move -float DDA::CalcTime() const -{ - return (float)clocksNeeded/stepClockRate; -} - // Prepare this DDA for execution. // This must not be called with interrupts disabled, because it calls Platform::EnableDrive. void DDA::Prepare(uint8_t simMode) @@ -976,98 +969,26 @@ void DDA::Prepare(uint8_t simMode) PrepParams params; params.decelStartDistance = totalDistance - decelDistance; - // Convert the accelerate/decelerate distances to times - const float accelStopTime = (topSpeed - startSpeed)/acceleration; - const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed; - const float totalTime = decelStartTime + (topSpeed - endSpeed)/acceleration; - - clocksNeeded = (uint32_t)(totalTime * stepClockRate); - if (simMode == 0) { if (isDeltaMovement) { - // This code used to be in DDA::Init but we moved it here so that we can implement babystepping in it, - // also it speeds up simulation because we no longer execute this code when simulating. - // However, this code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move. + // This code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move. // Therefore the Move code must not store a new move in that entry until this one has been prepared! (It took me ages to track this down.) - // Ideally we would store the initial X any Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build. - a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]); + // Ideally we would store the initial X and Y coordinates in the DDA, but we need to be economical with memory in the Duet 06/085 build. cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc); - - const float initialX = prev->GetEndCoordinate(X_AXIS, false); - const float initialY = prev->GetEndCoordinate(Y_AXIS, false); - const LinearDeltaKinematics& dparams = (const LinearDeltaKinematics&)reprap.GetMove().GetKinematics(); - const float diagonalSquared = dparams.GetDiagonalSquared(); - const float a2b2D2 = a2plusb2 * diagonalSquared; - - for (size_t drive = 0; drive < DELTA_AXES; ++drive) - { - const float A = initialX - dparams.GetTowerX(drive); - const float B = initialY - dparams.GetTowerY(drive); - const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); - DriveMovement* const pdm = pddm[drive]; - if (pdm != nullptr) - { - const float aAplusbB = A * directionVector[X_AXIS] + B * directionVector[Y_AXIS]; - const float dSquaredMinusAsquaredMinusBsquared = diagonalSquared - fsquare(A) - fsquare(B); - const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared); - pdm->mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2); - pdm->mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2); - pdm->mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = - (int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2)); - - // Calculate the distance at which we need to reverse direction. - if (a2plusb2 <= 0.0) - { - // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. - pdm->direction = (directionVector[Z_AXIS] >= 0.0); - pdm->reverseStartStep = pdm->totalSteps + 1; - } - else - { - // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed, - // the other root corresponds to the carriages being above the bed. - const float drev = ((directionVector[Z_AXIS] * sqrtf(a2b2D2 - fsquare(A * directionVector[Y_AXIS] - B * directionVector[X_AXIS]))) - - aAplusbB)/a2plusb2; - if (drev > 0.0 && drev < totalDistance) // if the reversal point is within range - { - // Calculate how many steps we need to move up before reversing - const float hrev = directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev)); - const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); - - // We may be almost at the peak height already, in which case we don't really have a reversal. - if (numStepsUp < 1 || (pdm->direction && (uint32_t)numStepsUp <= pdm->totalSteps)) - { - pdm->reverseStartStep = pdm->totalSteps + 1; - } - else - { - pdm->reverseStartStep = (uint32_t)numStepsUp + 1; - - // Correct the initial direction and the total number of steps - if (pdm->direction) - { - // Net movement is up, so we will go up a bit and then down by a lesser amount - pdm->totalSteps = (2 * numStepsUp) - pdm->totalSteps; - } - else - { - // Net movement is down, so we will go up first and then down by a greater amount - pdm->direction = true; - pdm->totalSteps = (2 * numStepsUp) + pdm->totalSteps; - } - } - } - else - { - pdm->reverseStartStep = pdm->totalSteps + 1; - } - } - } - } + params.a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]); + params.initialX = prev->GetEndCoordinate(X_AXIS, false); + params.initialY = prev->GetEndCoordinate(Y_AXIS, false); + params.dparams = static_cast<const LinearDeltaKinematics*>(&(reprap.GetMove().GetKinematics())); + params.diagonalSquared = params.dparams->GetDiagonalSquared(); + params.a2b2D2 = params.a2plusb2 * params.diagonalSquared; } + // Convert the accelerate/decelerate distances to times + const float accelStopTime = (topSpeed - startSpeed)/acceleration; + const float decelStartTime = accelStopTime + (params.decelStartDistance - accelDistance)/topSpeed; + params.startSpeedTimesCdivA = (uint32_t)((startSpeed * stepClockRate)/acceleration); params.topSpeedTimesCdivA = (uint32_t)((topSpeed * stepClockRate)/acceleration); params.decelStartClocks = (uint32_t)(decelStartTime * stepClockRate); @@ -1138,7 +1059,7 @@ void DDA::Prepare(uint8_t simMode) // Prepare for the first step pdm->nextStep = 0; pdm->nextStepTime = 0; - pdm->stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step + pdm->stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step pdm->stepsTillRecalc = 0; // so that we don't skip the calculation const bool stepsToDo = (isDeltaMovement && drive < numAxes) ? pdm->CalcNextStepTimeDelta(*this, false) @@ -1164,9 +1085,13 @@ void DDA::Prepare(uint8_t simMode) m.accelDistance = accelDistance; m.decelDistance = decelDistance; m.steadyDistance = totalDistance - accelDistance - decelDistance; + m.requestedSpeed = requestedSpeed; m.startSpeed = startSpeed; m.topSpeed = topSpeed; m.endSpeed = endSpeed; + m.targetNextSpeed = targetNextSpeed; + m.endstopChecks = endStopsToCheck; + m.flags = flags; savedMovePointer = (savedMovePointer + 1) % NumSavedMoves; #endif } @@ -1285,7 +1210,7 @@ void DDA::CheckEndstops(Platform& platform) case EndStopHit::lowHit: MoveAborted(); // set the state to completed and recalculate the endpoints reprap.GetGCodes().MoveStoppedByZProbe(); - break; + return; case EndStopHit::lowNear: ReduceHomingSpeed(); @@ -1334,12 +1259,19 @@ void DDA::CheckEndstops(Platform& platform) { case EndStopHit::lowHit: case EndStopHit::highHit: + if ((endStopsToCheck & UseSpecialEndstop) != 0) // use only one (probably non-default) endstop while probing a tool offset + { + MoveAborted(); + return; + } + else { ClearBit(endStopsToCheck, drive); // clear this check so that we can check for more const Kinematics& kin = reprap.GetMove().GetKinematics(); if (endStopsToCheck == 0 || kin.QueryTerminateHomingMove(drive)) { MoveAborted(); // no more endstops to check, or this axis uses shared motors, so stop the entire move + return; } else { @@ -1354,8 +1286,7 @@ void DDA::CheckEndstops(Platform& platform) break; case EndStopHit::lowNear: - // Only reduce homing speed if there are no more axes to be homed. - // This allows us to home X and Y simultaneously. + // Only reduce homing speed if there are no more axes to be homed. This allows us to home X and Y simultaneously. if (endStopsToCheck == MakeBitmap<AxesBitmap>(drive)) { ReduceHomingSpeed(); @@ -1453,7 +1384,7 @@ pre(state == frozen) return true; // schedule another interrupt immediately } -extern uint32_t maxReps; +uint32_t DDA::maxReps = 0; // this holds he maximum ISR loop count // This is called by the interrupt service routine to execute steps. // It returns true if it needs to be called again on the DDA of the new current move, otherwise false. @@ -1564,16 +1495,17 @@ bool DDA::Step() return false; } -// Stop a drive and re-calculate the corresponding endpoint +// Stop a drive and re-calculate the corresponding endpoint. +// For extruder drivers, we need to be able to calculate how much of the extrusion was completed after calling this. void DDA::StopDrive(size_t drive) { DriveMovement* const pdm = pddm[drive]; - if (pdm->state == DMState::moving) + if (pdm != nullptr && pdm->state == DMState::moving) { - endPoint[drive] -= pdm->GetNetStepsLeft(); pdm->state = DMState::idle; if (drive < reprap.GetGCodes().GetTotalAxes()) { + endPoint[drive] -= pdm->GetNetStepsLeft(); endCoordinatesValid = false; // the XYZ position is no longer valid } RemoveDM(drive); @@ -1585,7 +1517,8 @@ void DDA::StopDrive(size_t drive) } // This is called when we abort a move because we have hit an endstop. -// It adjusts the end points of the current move to account for how far through the move we got. +// It stop all drives and adjusts the end points of the current move to account for how far through the move we got. +// The caller must call MoveCompleted at some point after calling this. void DDA::MoveAborted() { if (state == executing) @@ -1598,6 +1531,40 @@ void DDA::MoveAborted() state = completed; } +// Return the approximate (to within 1%) proportion of extrusion for the complete multi-segment move that remains to be done. +// The move was either not started or was aborted. +float DDA::GetProportionDone() const +{ + // Get the proportion of extrusion already done at the start of this segment + unsigned int proportionDone = (filePos != noFilePosition && filePos == prev->filePos) + ? 256 - prev->proportionRemaining + : 0; + if (state == completed) + { + // The move was aborted, so subtract how much was done + const unsigned int proportionDoneAtEnd = 256 - proportionRemaining; + if (proportionDoneAtEnd > proportionDone) + { + int32_t taken = 0, left = 0; + for (size_t drive = reprap.GetGCodes().GetTotalAxes(); drive < DRIVES; ++drive) + { + const DriveMovement* const pdm = pddm[drive]; + if (pdm != nullptr) // if this extruder is active + { + taken += pdm->GetNetStepsTaken(); + left += pdm->GetNetStepsLeft(); + } + } + const int32_t total = taken + left; + if (total > 0) // if the move has net extrusion + { + proportionDone += (((proportionDoneAtEnd - proportionDone) * taken) + (total/2)) / total; + } + } + } + return (float)proportionDone/256.0; +} + // Reduce the speed of this move to the indicated speed. // This is called from the ISR, so interrupts are disabled and nothing else can mess with us. // As this is only called for homing moves and with very low speeds, we assume that we don't need acceleration or deceleration phases. @@ -1611,7 +1578,7 @@ void DDA::ReduceHomingSpeed() for (size_t drive = 0; drive < DRIVES; ++drive) { DriveMovement* const pdm = pddm[drive]; - if (pdm->state == DMState::moving) + if (pdm != nullptr && pdm->state == DMState::moving) { pdm->ReduceSpeed(*this, factor); RemoveDM(pdm->drive); @@ -1657,4 +1624,16 @@ int32_t DDA::GetStepsTaken(size_t drive) const return (dmp != nullptr) ? dmp->GetNetStepsTaken() : 0; } +void DDA::LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration) +{ + if (requestedSpeed > maxSpeed) + { + requestedSpeed = maxSpeed; + } + if (acceleration > maxAcceleration) + { + acceleration = maxAcceleration; + } +} + // End diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h index ebb560dc..83a8fc9c 100644 --- a/src/Movement/DDA.h +++ b/src/Movement/DDA.h @@ -41,14 +41,13 @@ public: bool Init(GCodes::RawMove &nextMove, bool doMotorMapping) __attribute__ ((hot)); // Set up a new move, returning true if it represents real movement bool Init(const float_t steps[DRIVES]); // Set up a raw (unmapped) motor move void Init(); // Set up initial positions for machine startup - bool Start(uint32_t tim); // Start executing the DDA, i.e. move the move. + bool Start(uint32_t tim) __attribute__ ((hot)); // Start executing the DDA, i.e. move the move. bool Step() __attribute__ ((hot)); // Take one step of the DDA, called by timed interrupt. void SetNext(DDA *n) { next = n; } void SetPrevious(DDA *p) { prev = p; } void Complete() { state = completed; } bool Free(); void Prepare(uint8_t simMode) __attribute__ ((hot)); // Calculate all the values and freeze this DDA - float CalcTime() const; // Calculate the time needed for this move (used for simulation) bool HasStepError() const; bool CanPauseAfter() const { return canPauseAfter; } bool CanPauseBefore() const { return canPauseBefore; } @@ -71,8 +70,15 @@ public: bool IsHomingAxes() const { return (endStopsToCheck & HomeAxes) != 0; } uint32_t GetXAxes() const { return xAxes; } uint32_t GetYAxes() const { return yAxes; } + float GetTotalDistance() const { return totalDistance; } + void LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration); // Limit the speed an acceleration of this move int32_t GetStepsTaken(size_t drive) const; + float GetProportionDone() const; // Return the proportion of extrusion for the complete multi-segment move already done + + void MoveAborted(); + + bool IsGoodToPrepare() const; #if SUPPORT_IOBITS uint32_t GetMoveStartTime() const { return moveStartTime; } @@ -91,14 +97,14 @@ public: // the calculation can just be managed in time at speeds of 15000mm/min (step interval 50us), but not at 20000mm/min (step interval 37.5us). // Therefore, where the step interval falls below 60us, we don't calculate on every step. // Note: the above measurements were taken some time ago, before some firmware optimisations. -#ifdef DUET_NG +#if SAM4E || SAM4S static constexpr int32_t MinCalcIntervalDelta = (40 * stepClockRate)/1000000; // the smallest sensible interval between calculations (40us) in step timer clocks static constexpr int32_t MinCalcIntervalCartesian = (40 * stepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t minInterruptInterval = 6; // about 2us minimum interval between interrupts, in clocks + static constexpr uint32_t minInterruptInterval = 6; // about 6us minimum interval between interrupts, in step clocks #else static constexpr int32_t MinCalcIntervalDelta = (60 * stepClockRate)/1000000; // the smallest sensible interval between calculations (60us) in step timer clocks static constexpr int32_t MinCalcIntervalCartesian = (60 * stepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t minInterruptInterval = 6; // about 2us minimum interval between interrupts, in clocks + static constexpr uint32_t minInterruptInterval = 4; // about 6us minimum interval between interrupts, in step clocks #endif static void PrintMoves(); // print saved moves for debugging @@ -109,14 +115,15 @@ public: static int32_t loggedProbePositions[XYZ_AXES * MaxLoggedProbePositions]; #endif + static uint32_t maxReps; + private: void RecalculateMove() __attribute__ ((hot)); void CalcNewSpeeds() __attribute__ ((hot)); void ReduceHomingSpeed(); // called to reduce homing speed when a near-endstop is triggered void StopDrive(size_t drive); // stop movement of a drive and recalculate the endpoint - void MoveAborted(); void InsertDM(DriveMovement *dm) __attribute__ ((hot)); - DriveMovement *RemoveDM(size_t drive) __attribute__ ((hot)); + void RemoveDM(size_t drive); void ReleaseDMs(); bool IsDecelerationMove() const; // return true if this move is or have been might have been intended to be a deceleration-only move void DebugPrintVector(const char *name, const float *vec, size_t len) const; @@ -135,16 +142,25 @@ private: DDA *prev; // The previous one in the ring volatile DDAState state; // What state this DDA is in - uint8_t endCoordinatesValid : 1; // True if endCoordinates can be relied on - uint8_t isDeltaMovement : 1; // True if this is a delta printer movement - uint8_t canPauseAfter : 1; // True if we can pause at the end of this move - uint8_t canPauseBefore : 1; // True if we can pause just before this move - uint8_t isPrintingMove : 1; // True if this move includes XY movement and extrusion - uint8_t usePressureAdvance : 1; // True if pressure advance should be applied to any forward extrusion - uint8_t hadLookaheadUnderrun : 1; // True if the lookahead queue was not long enough to optimise this move - uint8_t xyMoving : 1; // True if movement along an X axis or the Y axis was requested, even it if's too small to do - uint8_t goingSlow : 1; // True if we have slowed the movement because the Z probe is approaching its threshold - uint8_t isLeadscrewAdjustmentMove : 1; // True if this is a leadscrews adjustment move + uint8_t proportionRemaining; // What proportion of the extrusion in the G1 or G0 move of which this is a part remains to be done after this segment is complete + // - we use a uint8_t instead of a float to save space because it only affects the extrusion amount, so ~0.4% error is acceptable + union + { + struct + { + uint8_t endCoordinatesValid : 1; // True if endCoordinates can be relied on + uint8_t isDeltaMovement : 1; // True if this is a delta printer movement + uint8_t canPauseAfter : 1; // True if we can pause at the end of this move + uint8_t canPauseBefore : 1; // True if we can pause just before this move + uint8_t isPrintingMove : 1; // True if this move includes XY movement and extrusion + uint8_t usePressureAdvance : 1; // True if pressure advance should be applied to any forward extrusion + uint8_t hadLookaheadUnderrun : 1; // True if the lookahead queue was not long enough to optimise this move + uint8_t xyMoving : 1; // True if movement along an X axis or the Y axis was requested, even it if's too small to do + uint8_t goingSlow : 1; // True if we have slowed the movement because the Z probe is approaching its threshold + uint8_t isLeadscrewAdjustmentMove : 1; // True if this is a leadscrews adjustment move + }; + uint16_t flags; // so that we can print all the flags at once for debugging + }; EndstopChecks endStopsToCheck; // Which endstops we are checking on this move AxesBitmap xAxes; // Which axes are behaving as X axes @@ -161,7 +177,6 @@ private: float virtualExtruderPosition; // the virtual extruder position at the end of this move, used for pause/resume // These are used only in delta calculations - float a2plusb2; // Sum of the squares of the X and Y movement fractions int32_t cKc; // The Z movement fraction multiplied by Kc and converted to integer // These vary depending on how we connect the move with its predecessor and successor, but remain constant while the move is being executed diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp index a2aa8228..04b98c05 100644 --- a/src/Movement/DriveMovement.cpp +++ b/src/Movement/DriveMovement.cpp @@ -10,6 +10,7 @@ #include "Move.h" #include "RepRap.h" #include "Libraries/Math/Isqrt.h" +#include "Kinematics/LinearDeltaKinematics.h" // Static members @@ -46,16 +47,6 @@ DriveMovement *DriveMovement::Allocate(size_t drive, DMState st) return dm; } -void DriveMovement::Release(DriveMovement *item) -{ - if (item != nullptr) - { - item->nextDM = freeList; - freeList = item; - ++numFree; - } -} - // Constructors DriveMovement::DriveMovement(DriveMovement *next) : nextDM(next) { @@ -102,6 +93,64 @@ void DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& param void DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) { const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); + const float A = params.initialX - params.dparams->GetTowerX(drive); + const float B = params.initialY - params.dparams->GetTowerY(drive); + const float aAplusbB = A * dda.directionVector[X_AXIS] + B * dda.directionVector[Y_AXIS]; + const float dSquaredMinusAsquaredMinusBsquared = params.diagonalSquared - fsquare(A) - fsquare(B); + const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared); + mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2); + mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2); + mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = + (int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2)); + + // Calculate the distance at which we need to reverse direction. + if (params.a2plusb2 <= 0.0) + { + // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. + direction = (dda.directionVector[Z_AXIS] >= 0.0); + reverseStartStep = totalSteps + 1; + } + else + { + // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed, + // the other root corresponds to the carriages being above the bed. + const float drev = ((dda.directionVector[Z_AXIS] * sqrtf(params.a2b2D2 - fsquare(A * dda.directionVector[Y_AXIS] - B * dda.directionVector[X_AXIS]))) + - aAplusbB)/params.a2plusb2; + if (drev > 0.0 && drev < dda.totalDistance) // if the reversal point is within range + { + // Calculate how many steps we need to move up before reversing + const float hrev = dda.directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev)); + const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); + + // We may be almost at the peak height already, in which case we don't really have a reversal. + if (numStepsUp < 1 || (direction && (uint32_t)numStepsUp <= totalSteps)) + { + reverseStartStep = totalSteps + 1; + } + else + { + reverseStartStep = (uint32_t)numStepsUp + 1; + + // Correct the initial direction and the total number of steps + if (direction) + { + // Net movement is up, so we will go up a bit and then down by a lesser amount + totalSteps = (2 * numStepsUp) - totalSteps; + } + else + { + // Net movement is down, so we will go up first and then down by a greater amount + direction = true; + totalSteps = (2 * numStepsUp) + totalSteps; + } + } + } + else + { + reverseStartStep = totalSteps + 1; + } + } + mp.delta.twoCsquaredTimesMmPerStepDivAK = (uint32_t)((float)DDA::stepClockRateSquared/(stepsPerMm * dda.acceleration * (K2/2))); // Acceleration phase parameters diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h index 36b44f95..788f495d 100644 --- a/src/Movement/DriveMovement.h +++ b/src/Movement/DriveMovement.h @@ -10,16 +10,29 @@ #include "RepRapFirmware.h" +class LinearDeltaKinematics; + // Struct for passing parameters to the DriveMovement Prepare methods struct PrepParams { + // Parameters used for all types of motion float decelStartDistance; uint32_t startSpeedTimesCdivA; uint32_t topSpeedTimesCdivA; uint32_t decelStartClocks; uint32_t topSpeedTimesCdivAPlusDecelStartClocks; uint32_t accelClocksMinusAccelDistanceTimesCdivTopSpeed; + + // Parameters used only for extruders float compFactor; + + // Parameters used only for delta moves + float initialX; + float initialY; + const LinearDeltaKinematics *dparams; + float diagonalSquared; + float a2plusb2; // sum of the squares of the X and Y movement fractions + float a2b2D2; }; enum class DMState : uint8_t @@ -35,11 +48,11 @@ class DriveMovement public: DriveMovement(DriveMovement *next); - bool CalcNextStepTimeCartesian(const DDA &dda, bool live); - bool CalcNextStepTimeDelta(const DDA &dda, bool live); - void PrepareCartesianAxis(const DDA& dda, const PrepParams& params); - void PrepareDeltaAxis(const DDA& dda, const PrepParams& params); - void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation); + bool CalcNextStepTimeCartesian(const DDA &dda, bool live) __attribute__ ((hot)); + bool CalcNextStepTimeDelta(const DDA &dda, bool live) __attribute__ ((hot)); + void PrepareCartesianAxis(const DDA& dda, const PrepParams& params) __attribute__ ((hot)); + void PrepareDeltaAxis(const DDA& dda, const PrepParams& params) __attribute__ ((hot)); + void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation) __attribute__ ((hot)); void ReduceSpeed(const DDA& dda, float inverseSpeedFactor); void DebugPrint(char c, bool withDelta) const; int32_t GetNetStepsLeft() const; @@ -53,8 +66,8 @@ public: static void Release(DriveMovement *item); private: - bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live); - bool CalcNextStepTimeDeltaFull(const DDA &dda, bool live); + bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live) __attribute__ ((hot)); + bool CalcNextStepTimeDeltaFull(const DDA &dda, bool live) __attribute__ ((hot)); static DriveMovement *freeList; static int numFree; @@ -144,6 +157,7 @@ inline bool DriveMovement::CalcNextStepTimeCartesian(const DDA &dda, bool live) // Calculate the time since the start of the move when the next step for the specified DriveMovement is due // Return true if there are more steps to do. When finished, leave nextStep == totalSteps + 1. +// We inline this part to speed things up when we are doing double/quad/octal stepping. inline bool DriveMovement::CalcNextStepTimeDelta(const DDA &dda, bool live) { ++nextStep; @@ -201,4 +215,12 @@ inline int32_t DriveMovement::GetNetStepsTaken() const return (direction) ? netStepsTaken : -netStepsTaken; } +// This is inlined because it is only called from one place +inline void DriveMovement::Release(DriveMovement *item) +{ + item->nextDM = freeList; + freeList = item; + ++numFree; +} + #endif /* DRIVEMOVEMENT_H_ */ diff --git a/src/Movement/Kinematics/CartesianKinematics.cpp b/src/Movement/Kinematics/CartesianKinematics.cpp index db884171..f9a889b1 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[], bool allowModeChange) const +bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { for (size_t axis = 0; axis < numVisibleAxes; ++axis) { @@ -50,7 +50,14 @@ bool CartesianKinematics::QueryTerminateHomingMove(size_t axis) const void CartesianKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const { const float hitPoint = (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) : reprap.GetPlatform().AxisMinimum(axis); - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); +} + +// 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 CartesianKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // The axes of a Cartesian printer move independently, so there is nothing to do here } // End diff --git a/src/Movement/Kinematics/CartesianKinematics.h b/src/Movement/Kinematics/CartesianKinematics.h index 22051f6f..6b578282 100644 --- a/src/Movement/Kinematics/CartesianKinematics.h +++ b/src/Movement/Kinematics/CartesianKinematics.h @@ -17,11 +17,12 @@ 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[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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; void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_CARTESIANKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreBaseKinematics.cpp b/src/Movement/Kinematics/CoreBaseKinematics.cpp index 92bb81b5..f6b74b5e 100644 --- a/src/Movement/Kinematics/CoreBaseKinematics.cpp +++ b/src/Movement/Kinematics/CoreBaseKinematics.cpp @@ -74,7 +74,7 @@ void CoreBaseKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, cons } else { - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); } } diff --git a/src/Movement/Kinematics/CoreXYKinematics.cpp b/src/Movement/Kinematics/CoreXYKinematics.cpp index 6f35e321..86d32a33 100644 --- a/src/Movement/Kinematics/CoreXYKinematics.cpp +++ b/src/Movement/Kinematics/CoreXYKinematics.cpp @@ -6,6 +6,7 @@ */ #include "CoreXYKinematics.h" +#include "Movement/DDA.h" CoreXYKinematics::CoreXYKinematics() : CoreBaseKinematics(KinematicsType::coreXY) { @@ -18,7 +19,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[], bool allowModeChange) const +bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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]); @@ -55,4 +56,24 @@ bool CoreXYKinematics::DriveIsShared(size_t drive) const return drive == X_AXIS || drive == Y_AXIS; } +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds along individual Cartesian axes have already been limited before this is called. +void CoreXYKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + const float vecX = normalisedDirectionVector[0]; + const float vecY = normalisedDirectionVector[1]; + const float vecMax = max<float>(fabs(vecX + vecY), fabs(vecX - vecY)); // pick the case for the motor that is working hardest + if (vecMax > 0.01) // avoid division by zero or near-zero + { + const Platform& platform = reprap.GetPlatform(); + const float aX = platform.Acceleration(0); + const float aY = platform.Acceleration(1); + const float vX = platform.MaxFeedrate(0); + const float vY = platform.MaxFeedrate(1); + const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMax * (fabs(vecX) * aY + fabs(vecY) * aX)); + const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMax * (fabs(vecX) * vY + fabs(vecY) * vX)); + dda.LimitSpeedAndAcceleration(vMax, aMax); + } +} + // End diff --git a/src/Movement/Kinematics/CoreXYKinematics.h b/src/Movement/Kinematics/CoreXYKinematics.h index 4663acc1..711dd3dc 100644 --- a/src/Movement/Kinematics/CoreXYKinematics.h +++ b/src/Movement/Kinematics/CoreXYKinematics.h @@ -17,9 +17,10 @@ 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[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREXYKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreXYUKinematics.cpp b/src/Movement/Kinematics/CoreXYUKinematics.cpp index 560037aa..ca327652 100644 --- a/src/Movement/Kinematics/CoreXYUKinematics.cpp +++ b/src/Movement/Kinematics/CoreXYUKinematics.cpp @@ -7,6 +7,7 @@ #include "CoreXYUKinematics.h" #include "GCodes/GCodes.h" +#include "Movement/DDA.h" const size_t CoreXYU_AXES = 5; const size_t U_AXIS = 3; // X2 @@ -55,7 +56,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[], bool allowModeChange) const +bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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]); @@ -102,4 +103,41 @@ bool CoreXYUKinematics::DriveIsShared(size_t drive) const || drive == V_AXIS; // V doesn't have endstop switches, but include it here just in case } +// Limit the speed and acceleration of a move to values that the mechanics can handle. +// The speeds along individual Cartesian axes have already been limited before this is called. +void CoreXYUKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + const float vecX = normalisedDirectionVector[0]; + const float vecY = normalisedDirectionVector[1]; + + // Limit the XY motor accelerations + const float vecMaxXY = max<float>(fabs(vecX + vecY), fabs(vecX - vecY)); // pick the case for the motor that is working hardest + if (vecMaxXY > 0.01) // avoid division by zero or near-zero + { + const Platform& platform = reprap.GetPlatform(); + const float aX = platform.Acceleration(0); + const float aY = platform.Acceleration(1); + const float vX = platform.MaxFeedrate(0); + const float vY = platform.MaxFeedrate(1); + const float aMax = (fabs(vecX) + fabs(vecY)) * aX * aY/(vecMaxXY * (fabs(vecX) * aY + fabs(vecY) * aX)); + const float vMax = (fabs(vecX) + fabs(vecY)) * vX * vY/(vecMaxXY * (fabs(vecX) * vY + fabs(vecY) * vX)); + dda.LimitSpeedAndAcceleration(vMax, aMax); + } + + // Limit the UV motor accelerations + const float vecU = normalisedDirectionVector[3]; + const float vecMaxUV = max<float>(fabs(vecU + vecY), fabs(vecY - vecY)); // pick the case for the motor that is working hardest + if (vecMaxUV > 0.01) // avoid division by zero or near-zero + { + const Platform& platform = reprap.GetPlatform(); + const float aU = platform.Acceleration(3); + const float aY = platform.Acceleration(1); + const float vU = platform.MaxFeedrate(3); + const float vY = platform.MaxFeedrate(1); + const float aMax = (fabs(vecX) + fabs(vecY)) * aU * aY/(vecMaxUV * (fabs(vecX) * aY + fabs(vecY) * aU)); + const float vMax = (fabs(vecX) + fabs(vecY)) * vU * vY/(vecMaxUV * (fabs(vecX) * vY + fabs(vecY) * vU)); + dda.LimitSpeedAndAcceleration(vMax, aMax); + } +} + // End diff --git a/src/Movement/Kinematics/CoreXYUKinematics.h b/src/Movement/Kinematics/CoreXYUKinematics.h index 77db9ae9..47a867b0 100644 --- a/src/Movement/Kinematics/CoreXYUKinematics.h +++ b/src/Movement/Kinematics/CoreXYUKinematics.h @@ -18,9 +18,10 @@ 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[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREXYKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/CoreXZKinematics.cpp b/src/Movement/Kinematics/CoreXZKinematics.cpp index d81f13ca..4144aa47 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[], bool allowModeChange) const +bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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]); @@ -56,4 +56,13 @@ bool CoreXZKinematics::DriveIsShared(size_t drive) const return drive == X_AXIS || drive == Z_AXIS; } +// 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 CoreXZKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // Ideally we would do something here, but for now we don't. + // This could mean that a simultaneous X and Z movement with Z at maximum speed up and X at 3x that speed would be under-powered, + // but the workaround in that case would be just to lower the maximum Z speed a little, which won't affect printing speed significantly. +} + // End diff --git a/src/Movement/Kinematics/CoreXZKinematics.h b/src/Movement/Kinematics/CoreXZKinematics.h index 3a8d9344..9f9078ef 100644 --- a/src/Movement/Kinematics/CoreXZKinematics.h +++ b/src/Movement/Kinematics/CoreXZKinematics.h @@ -17,11 +17,12 @@ 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[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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; bool SupportsAutoCalibration() const override { return false; } + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; }; #endif /* SRC_MOVEMENT_KINEMATICS_COREXZKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp index 484f7d35..d85f8d24 100644 --- a/src/Movement/Kinematics/Kinematics.cpp +++ b/src/Movement/Kinematics/Kinematics.cpp @@ -12,6 +12,7 @@ #include "CoreXZKinematics.h" #include "ScaraKinematics.h" #include "CoreXYUKinematics.h" +#include "PolarKinematics.h" #include "RepRap.h" #include "Platform.h" @@ -42,15 +43,15 @@ bool Kinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply // Return true if the specified XY position is reachable by the print head reference point. // This default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 command. -bool Kinematics::IsReachable(float x, float y) const +bool Kinematics::IsReachable(float x, float y, bool isCoordinated) const { const Platform& platform = reprap.GetPlatform(); return x >= platform.AxisMinimum(X_AXIS) && y >= platform.AxisMinimum(Y_AXIS) && x <= platform.AxisMaximum(X_AXIS) && y <= platform.AxisMaximum(Y_AXIS); } -// Limit the Cartesian position that the user wants to move to +// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed // This default implementation just applies the rectangular limits set up by M208 to those axes that have been homed. -bool Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const +bool Kinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const { return LimitPositionFromAxis(coords, 0, numVisibleAxes, axesHomed); } @@ -139,6 +140,8 @@ const char* Kinematics::GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alrea return new ScaraKinematics(); case KinematicsType::coreXYU: return new CoreXYUKinematics(); + case KinematicsType::polar: + return new PolarKinematics(); } } diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h index 3d960929..fa9b2e25 100644 --- a/src/Movement/Kinematics/Kinematics.h +++ b/src/Movement/Kinematics/Kinematics.h @@ -26,6 +26,8 @@ enum class KinematicsType : uint8_t linearDelta, scara, coreXYU, + hangprinter, + polar, unknown // this one must be last! }; @@ -52,7 +54,7 @@ public: // 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 fore printing. + // Otherwise it should be in a format suitable for printing. // For any new kinematics, the same string can be returned regardless of the parameter. virtual const char *GetName(bool forStatusReport = false) const = 0; @@ -71,7 +73,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[], bool allowModeChange) const = 0; + virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const = 0; // Convert motor positions (measured in steps from reference position) to Cartesian coordinates // 'motorPos' is the input vector of motor positions @@ -102,12 +104,12 @@ public: virtual float GetTiltCorrection(size_t axis) const { return 0.0; } // Return true if the specified XY position is reachable by the print head reference point. - // The default implementation assumes a rectangular reachable area, so it just used the bed dimensions give in the M208 commands. - virtual bool IsReachable(float x, float y) const; + // The default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 commands. + virtual bool IsReachable(float x, float y, bool isCoordinated) const; // Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed // The default implementation just applies the rectangular limits set up by M208 to those axes that have been homed. - virtual bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const; + virtual bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) 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) @@ -123,7 +125,7 @@ public: 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"; } + virtual const char* HomingButtonNames() const { return "XYZUVWABC"; } // 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 @@ -149,6 +151,10 @@ public: // Write any calibration data that we need to resume a print after power fail, returning true if successful. Override where necessary. virtual bool WriteResumeSettings(FileStore *f) const { return true; } + // Limit the speed and acceleration of a move to values that the mechanics can handle. + // The speeds along individual Cartesian axes have already been limited before this is called. + virtual void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const = 0; + // Override this virtual destructor if your constructor allocates any dynamic memory virtual ~Kinematics() { } diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp index 28ed94e4..ecfc093d 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[], bool allowModeChange) const +bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const { //TODO return false if we can't transform the position for (size_t axis = 0; axis < min<size_t>(numVisibleAxes, DELTA_AXES); ++axis) @@ -163,20 +163,20 @@ void LinearDeltaKinematics::MotorStepsToCartesian(const int32_t motorPos[], cons } // Return true if the specified XY position is reachable by the print head reference point. -bool LinearDeltaKinematics::IsReachable(float x, float y) const +bool LinearDeltaKinematics::IsReachable(float x, float y, bool isCoordinated) const { return fsquare(x) + fsquare(y) < printRadiusSquared; } // Limit the Cartesian position that the user wants to move to -bool LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const +bool LinearDeltaKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const { const AxesBitmap allAxes = MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); bool limited = false; if ((axesHomed & allAxes) == allAxes) { // If axes have been homed on a delta printer and this isn't a homing move, check for movements outside limits. - // Skip this check if axes have not been homed, so that extruder-only moved are allowed before homing + // Skip this check if axes have not been homed, so that extruder-only moves are allowed before homing // Constrain the move to be within the build radius const float diagonalSquared = fsquare(coords[X_AXIS]) + fsquare(coords[Y_AXIS]); if (diagonalSquared > printRadiusSquared) @@ -735,7 +735,22 @@ void LinearDeltaKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, c if (highEnd) { const float hitPoint = GetHomedCarriageHeight(axis); - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); + } +} + +// 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 LinearDeltaKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // Limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration + const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + if (xyFactor > 0.01) + { + const Platform& platform = reprap.GetPlatform(); + const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS)); + const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS)); + dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor); } } diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h index 6a3bdc3f..bc09cb1c 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.h +++ b/src/Movement/Kinematics/LinearDeltaKinematics.h @@ -26,15 +26,15 @@ 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[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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; 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; - bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const override; + bool IsReachable(float x, float y, bool isCoordinated) const override; + bool LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); } MotionType GetMotionType(size_t axis) const override; @@ -45,6 +45,7 @@ public: bool QueryTerminateHomingMove(size_t axis) const override; void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; bool WriteResumeSettings(FileStore *f) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; // Public functions specific to this class float GetDiagonalSquared() const { return D2; } 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 diff --git a/src/Movement/Kinematics/PolarKinematics.h b/src/Movement/Kinematics/PolarKinematics.h new file mode 100644 index 00000000..fe473d9b --- /dev/null +++ b/src/Movement/Kinematics/PolarKinematics.h @@ -0,0 +1,51 @@ +/* + * PolarKinematics.h + * + * Created on: 13 Oct 2017 + * Author: David + */ + +#ifndef SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ +#define SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ + +#include "Kinematics.h" + +class PolarKinematics : public Kinematics +{ +public: + PolarKinematics(); + + // 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[], bool isCoordinated) 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, bool isCoordinated) const override; + bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const override; + void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; + const char* HomingButtonNames() const override { return "RTZUVWABC"; } + HomingMode GetHomingMode() const override { return homeIndividualMotors; } + AxesBitmap AxesAssumedHomed(AxesBitmap g92Axes) const override; + const char* GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const override; + bool QueryTerminateHomingMove(size_t axis) const override; + void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; + +private: + static constexpr float DefaultSegmentsPerSecond = 100.0; + static constexpr float DefaultMinSegmentSize = 0.2; + static constexpr float DefaultMaxRadius = 150.0; + static constexpr float DefaultMaxTurntableSpeed = 30.0; // degrees per second + static constexpr float DefaultMaxTurntableAcceleration = 30.0; // degrees per second per second + static constexpr const char *HomeRadiusFileName = "homeradius.g"; + static constexpr const char *HomeBedFileName = "homebed.g"; + + void Recalc(); + + float minRadius, maxRadius, homedRadius; + float maxTurntableSpeed, maxTurntableAcceleration; + + float minRadiusSquared, maxRadiusSquared; +}; + +#endif /* SRC_MOVEMENT_KINEMATICS_POLARKINEMATICS_H_ */ diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp index a350f42b..9a5580b0 100644 --- a/src/Movement/Kinematics/ScaraKinematics.cpp +++ b/src/Movement/Kinematics/ScaraKinematics.cpp @@ -12,14 +12,16 @@ #include "GCodes/GCodeBuffer.h" #include "Movement/DDA.h" +#include <limits> + ScaraKinematics::ScaraKinematics() : ZLeadscrewKinematics(KinematicsType::scara, DefaultSegmentsPerSecond, DefaultMinSegmentSize, true), proximalArmLength(DefaultProximalArmLength), distalArmLength(DefaultDistalArmLength), xOffset(0.0), yOffset(0.0) { thetaLimits[0] = DefaultMinTheta; thetaLimits[1] = DefaultMaxTheta; - phiLimits[0] = DefaultMinPhi; - phiLimits[1] = DefaultMaxPhi; + psiLimits[0] = DefaultMinPsi; + psiLimits[1] = DefaultMaxPsi; crosstalk[0] = crosstalk[1] = crosstalk[2] = 0.0; Recalc(); } @@ -30,11 +32,11 @@ const char *ScaraKinematics::GetName(bool forStatusReport) const return "Scara"; } -// 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[], bool allowModeChange) const +// Calculate theta, psi and the new arm mode form a target position. +// If the position is not reachable because it is out of radius limits, set theta and psi to NaN and return false. +// Otherwise set theta and psi to the required values and return true if they are in range. +bool ScaraKinematics::CalculateThetaAndPsi(const float machinePos[], bool isCoordinated, float& theta, float& psi, bool& armMode) 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; const float y = machinePos[Y_AXIS] + yOffset; const float cosPsi = (fsquare(x) + fsquare(y) - proximalArmLengthSquared - distalArmLengthSquared) / twoPd; @@ -43,40 +45,46 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa const float square = 1.0f - fsquare(cosPsi); if (square < 0.01f) { + theta = psi = std::numeric_limits<float>::quiet_NaN(); return false; // not reachable } + psi = acosf(cosPsi); const float sinPsi = sqrtf(square); - float psi = acosf(cosPsi); const float SCARA_K1 = proximalArmLength + distalArmLength * cosPsi; const float SCARA_K2 = distalArmLength * sinPsi; - float theta; // Try the current arm mode, then the other one bool switchedMode = false; for (;;) { - if (isDefaultArmMode != switchedMode) + if (armMode != 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] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1]) + if (psi >= psiLimits[0] && psi <= psiLimits[1]) { - break; + theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y); + if (theta >= thetaLimits[0] && theta <= thetaLimits[1]) + { + break; + } } } else { // 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[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1]) + if ((-psi) >= psiLimits[0] && (-psi) <= psiLimits[1]) { - psi = -psi; - break; + theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y); + if (theta >= thetaLimits[0] && theta <= thetaLimits[1]) + { + psi = -psi; + break; + } } } - if (switchedMode) + if (isCoordinated || switchedMode) { return false; // not reachable } @@ -86,7 +94,31 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa // Now that we know we are going to do the move, update the arm mode if (switchedMode) { - isDefaultArmMode = !isDefaultArmMode; + armMode = !armMode; + } + + return true; +} + +// Convert Cartesian coordinates to motor coordinates, returning true if successful +// 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[], bool isCoordinated) const +{ + float theta, psi; + if (machinePos[0] == cachedX && machinePos[1] == cachedY) + { + theta = cachedTheta; + psi = cachedPsi; + currentArmMode = cachedArmMode; + } + else + { + bool armMode = currentArmMode; + if (!CalculateThetaAndPsi(machinePos, isCoordinated, theta, psi, armMode)) + { + return false; + } + currentArmMode = armMode; } //debugPrintf("psi = %.2f, theta = %.2f\n", psi * RadiansToDegrees, theta * RadiansToDegrees); @@ -107,11 +139,11 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa // 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 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; + const float psi = ((float)motorPos[X_AXIS]/stepsPerMm[X_AXIS]) * DegreesToRadians; + const float theta = (((float)motorPos[Y_AXIS] + ((float)motorPos[X_AXIS] * crosstalk[0]))/stepsPerMm[Y_AXIS]) * DegreesToRadians; - machinePos[X_AXIS] = (cosf(arm1Angle) * proximalArmLength + cosf(arm1Angle + arm2Angle) * distalArmLength) - xOffset; - machinePos[Y_AXIS] = (sinf(arm1Angle) * proximalArmLength + sinf(arm1Angle + arm2Angle) * distalArmLength) - yOffset; + machinePos[X_AXIS] = (cosf(psi) * proximalArmLength + cosf(psi + theta) * distalArmLength) - xOffset; + machinePos[Y_AXIS] = (sinf(psi) * proximalArmLength + sinf(psi + theta) * distalArmLength) - yOffset; // 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]; @@ -139,14 +171,17 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& gb.TryGetFValue('Y', yOffset, seen); if (gb.TryGetFloatArray('A', 2, thetaLimits, reply, seen)) { + error = true; return true; } - if (gb.TryGetFloatArray('B', 2, phiLimits, reply, seen)) + if (gb.TryGetFloatArray('B', 2, psiLimits, reply, seen)) { + error = true; return true; } if (gb.TryGetFloatArray('C', 3, crosstalk, reply, seen)) { + error = true; return true; } @@ -159,7 +194,7 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply.printf("Printer mode is Scara with proximal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL ", distal arm %.2fmm range %.1f to %.1f" DEGREE_SYMBOL ", crosstalk %.1f:%.1f:%.1f, bed origin (%.1f, %.1f), segments/sec %d, min. segment length %.2f", (double)proximalArmLength, (double)thetaLimits[0], (double)thetaLimits[1], - (double)distalArmLength, (double)phiLimits[0], (double)phiLimits[1], + (double)distalArmLength, (double)psiLimits[0], (double)psiLimits[1], (double)crosstalk[0], (double)crosstalk[1], (double)crosstalk[2], (double)xOffset, (double)yOffset, (int)segmentsPerSecond, (double)minSegmentLength); @@ -173,57 +208,95 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& } // Return true if the specified XY position is reachable by the print head reference point. -// TODO add an arm mode parameter? -bool ScaraKinematics::IsReachable(float x, float y) const +bool ScaraKinematics::IsReachable(float x, float y, bool isCoordinated) const { - // TODO The following isn't quite right, in particular it doesn't take account of the maximum arm travel - const float r = sqrtf(fsquare(x + xOffset) + fsquare(y + yOffset)); - return r >= minRadius && r <= maxRadius && (x + xOffset) > 0.0; + // Check the M208 limits first + float coords[2] = {x, y}; + if (Kinematics::LimitPosition(coords, 2, LowestNBits<AxesBitmap>(2), isCoordinated)) + { + return false; + } + + // See if we can transform the position + float theta, psi; + bool armMode = currentArmMode; + const bool reachable = CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode); + if (reachable) + { + // Save the original and transformed coordinates so that we don't need to calculate them again if we are commanded to move to this position + cachedX = x; + cachedY = y; + cachedTheta = theta; + cachedPsi = psi; + cachedArmMode = armMode; + } + + return reachable; } -// Limit the Cartesian position that the user wants to move to -// TODO take account of arm angle limits -bool ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed) const +// Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed +bool ScaraKinematics::LimitPosition(float coords[], size_t numVisibleAxes, AxesBitmap axesHomed, bool isCoordinated) const { // First limit all axes according to M208 - const bool m208Limited = Kinematics::LimitPosition(coords, numVisibleAxes, axesHomed); - - // Now check that the XY position is within radius limits, in case the M208 limits are too generous - bool radiusLimited = false; - float x = coords[X_AXIS] + xOffset; - float y = coords[Y_AXIS] + yOffset; - const float r2 = fsquare(x) + fsquare(y); - if (r2 < minRadiusSquared) + const bool m208Limited = Kinematics::LimitPosition(coords, numVisibleAxes, axesHomed, isCoordinated); + + float theta, psi; + bool armMode = currentArmMode; + if (CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode)) { - const float r = sqrtf(r2); - // The user may have specified x=0 y=0 so allow for this - if (r < 1.0) + // Save the original and transformed coordinates so that we don't need to calculate them again if we are commanded to move to this position + cachedX = coords[0]; + cachedY = coords[1]; + cachedTheta = theta; + cachedPsi = psi; + cachedArmMode = armMode; + return m208Limited; + } + + // The requested position was not reachable + if (isnan(theta)) + { + // We are radius-limited + float x = coords[X_AXIS] + xOffset; + float y = coords[Y_AXIS] + yOffset; + const float r = sqrtf(fsquare(x) + fsquare(y)); + if (r < minRadius) { - x = minRadius; - y = 0.0; + // Radius is too small. The user may have specified x=0 y=0 so allow for this. + if (r < 1.0) + { + x = minRadius; + y = 0.0; + } + else + { + x *= minRadius/r; + y *= minRadius/r; + } } else { - x *= minRadius/r; - y *= minRadius/r; + // Radius must be too large + x *= maxRadius/r; + y *= maxRadius/r; } - radiusLimited = true; - } - else if (r2 > maxRadiusSquared) - { - const float r = sqrtf(r2); - x *= maxRadius/r; - y *= maxRadius/r; - radiusLimited = true; - } - if (radiusLimited) - { coords[X_AXIS] = x - xOffset; coords[Y_AXIS] = y - yOffset; } - return m208Limited || radiusLimited; + // Recalculate theta and psi, but don't allow arm mode changes this time + if (!CalculateThetaAndPsi(coords, true, theta, psi, armMode) && !isnan(theta)) + { + // Radius is in range but at least one arm angle isn't + cachedTheta = theta = constrain<float>(theta, thetaLimits[0], thetaLimits[1]); + cachedPsi = psi = constrain<float>(psi, psiLimits[0], psiLimits[1]); + cachedX = coords[X_AXIS] = (cosf(psi) * proximalArmLength + cosf(psi + theta) * distalArmLength) - xOffset; + cachedY = coords[Y_AXIS] = (sinf(psi) * proximalArmLength + sinf(psi + theta) * distalArmLength) - yOffset; + cachedArmMode = currentArmMode; + } + + return true; } // Return the initial Cartesian coordinates we assume after switching to this kinematics @@ -302,16 +375,33 @@ bool ScaraKinematics::QueryTerminateHomingMove(size_t axis) const // 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. +//TODO this doesn't appear to take account of the crosstalk factors yet void ScaraKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const { const float hitPoint = (axis == 0) ? ((highEnd) ? thetaLimits[1] : thetaLimits[0]) // proximal joint homing switch : (axis == 1) - ? ((highEnd) ? phiLimits[1] : phiLimits[0]) // distal joint homing switch + ? ((highEnd) ? psiLimits[1] : psiLimits[0]) // distal joint homing switch : (highEnd) ? reprap.GetPlatform().AxisMaximum(axis) // other axis (e.g. Z) high end homing switch : reprap.GetPlatform().AxisMinimum(axis); // other axis (e.g. Z) low end homing switch - dda.SetDriveCoordinate(hitPoint * stepsPerMm[axis], axis); + dda.SetDriveCoordinate(lrintf(hitPoint * stepsPerMm[axis]), axis); +} + +// 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 ScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const +{ + // For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration. + // Limiting the angular rates of the arms would be better. + const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + if (xyFactor > 0.01) + { + const Platform& platform = reprap.GetPlatform(); + const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS)); + const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS)); + dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor); + } } // Recalculate the derived parameters @@ -321,20 +411,22 @@ void ScaraKinematics::Recalc() distalArmLengthSquared = fsquare(distalArmLength); twoPd = proximalArmLength * distalArmLength * 2.0f; - minRadius = (proximalArmLength + distalArmLength * min<float>(cosf(phiLimits[0] * DegreesToRadians), cosf(phiLimits[1] * DegreesToRadians))) * 1.005; - if (phiLimits[0] < 0.0 && phiLimits[1] > 0.0) + minRadius = (proximalArmLength + distalArmLength * min<float>(cosf(psiLimits[0] * DegreesToRadians), cosf(psiLimits[1] * DegreesToRadians))) * 1.005; + if (psiLimits[0] < 0.0 && psiLimits[1] > 0.0) { // Zero distal arm angle is reachable maxRadius = proximalArmLength + distalArmLength; } else { - const float minAngle = min<float>(fabs(phiLimits[0]), fabs(phiLimits[1])) * DegreesToRadians; + const float minAngle = min<float>(fabs(psiLimits[0]), fabs(psiLimits[1])) * DegreesToRadians; maxRadius = sqrtf(proximalArmLengthSquared + distalArmLengthSquared + (twoPd * cosf(minAngle))); } maxRadius *= 0.995; minRadiusSquared = fsquare(minRadius); maxRadiusSquared = fsquare(maxRadius); + + cachedX = cachedY = std::numeric_limits<float>::quiet_NaN(); // make sure that the cached values won't match any coordinates } // End diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h index e229468d..43e1807b 100644 --- a/src/Movement/Kinematics/ScaraKinematics.h +++ b/src/Movement/Kinematics/ScaraKinematics.h @@ -28,18 +28,19 @@ 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[], bool allowModeChange) const override; + bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) 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; + bool IsReachable(float x, float y, bool isCoordinated) const override; + bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed, bool isCoordinated) const override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const override; size_t NumHomingButtons(size_t numVisibleAxes) const override; - const char* HomingButtonNames() const override { return "PDZUVW"; } + const char* HomingButtonNames() const override { return "PDZUVWABC"; } HomingMode GetHomingMode() const override { return homeIndividualMotors; } AxesBitmap AxesAssumedHomed(AxesBitmap g92Axes) const override; const char* GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, AxesBitmap& mustHomeFirst) const override; bool QueryTerminateHomingMove(size_t axis) const override; void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const override; + void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector) const override; private: static constexpr float DefaultSegmentsPerSecond = 100.0; @@ -48,19 +49,20 @@ private: static constexpr float DefaultDistalArmLength = 100.0; static constexpr float DefaultMinTheta = -90.0; // minimum proximal joint angle static constexpr float DefaultMaxTheta = 90.0; // maximum proximal joint angle - static constexpr float DefaultMinPhi = -135.0; // minimum distal joint angle - static constexpr float DefaultMaxPhi = 135.0; // maximum distal joint angle + static constexpr float DefaultMinPsi = -135.0; // minimum distal joint angle + static constexpr float DefaultMaxPsi = 135.0; // maximum distal joint angle static constexpr const char *HomeProximalFileName = "homeproximal.g"; static constexpr const char *HomeDistalFileName = "homedistal.g"; void Recalc(); + bool CalculateThetaAndPsi(const float machinePos[], bool isCoordinated, float& theta, float& psi, bool& armMode) const; // Primary parameters float proximalArmLength; float distalArmLength; float thetaLimits[2]; // minimum proximal joint angle - float phiLimits[2]; // minimum distal joint angle + float psiLimits[2]; // minimum distal joint angle float crosstalk[3]; // if we rotate the distal arm motor, for each full rotation the Z height goes up by this amount float xOffset; // where bed X=0 is relative to the proximal joint float yOffset; // where bed Y=0 is relative to the proximal joint @@ -73,7 +75,8 @@ private: float twoPd; // State variables - mutable bool isDefaultArmMode; // this should be moved into class Move when it knows about different arm modes + mutable float cachedX, cachedY, cachedTheta, cachedPsi; + mutable bool currentArmMode, cachedArmMode; }; #endif /* SRC_MOVEMENT_KINEMATICS_SCARAKINEMATICS_H_ */ diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp index 8096ae79..4304db5d 100644 --- a/src/Movement/Move.cpp +++ b/src/Movement/Move.cpp @@ -7,13 +7,12 @@ #include "Move.h" #include "Platform.h" -#include "RepRap.h" -#include "Kinematics/LinearDeltaKinematics.h" // temporary -Move::Move() : currentDda(nullptr), scheduledMoves(0), completedMoves(0) -{ - active = false; +static constexpr uint32_t UsualMinimumPreparedTime = DDA::stepClockRate/10; // 100ms +static constexpr uint32_t AbsoluteMinimumPreparedTime = DDA::stepClockRate/20; // 50ms +Move::Move() : currentDda(nullptr), active(false), scheduledMoves(0), completedMoves(0) +{ kinematics = Kinematics::Create(KinematicsType::cartesian); // default to Cartesian // Build the DDA ring @@ -21,7 +20,7 @@ Move::Move() : currentDda(nullptr), scheduledMoves(0), completedMoves(0) ddaRingGetPointer = ddaRingAddPointer = dda; for (size_t i = 1; i < DdaRingLength; i++) { - DDA *oldDda = dda; + DDA * const oldDda = dda; dda = new DDA(dda); oldDda->SetPrevious(dda); } @@ -74,13 +73,13 @@ void Move::Init() longWait = millis(); idleTimeout = DefaultIdleTimeout; - iState = IdleState::idle; + moveState = MoveState::idle; idleCount = 0; simulationMode = 0; simulationTime = 0.0; longestGcodeWaitInterval = 0; - waitingForMove = specialMoveAvailable = false; + specialMoveAvailable = false; active = true; } @@ -119,9 +118,10 @@ void Move::Spin() ++idleCount; } - // Check for DDA errors to print if Move debug is enabled + // Recycle the DDAs for completed moves, checking for DDA errors to print if Move debug is enabled while (ddaRingCheckPointer->GetState() == DDA::completed) { + // Check for step errors and record/print them if we have any, before we lose the DMs if (ddaRingCheckPointer->HasStepError()) { if (reprap.Debug(moduleMove)) @@ -131,6 +131,8 @@ void Move::Spin() ++stepErrors; reprap.GetPlatform().LogError(ErrorCode::BadMove); } + + // Now release the DMs and check for underrun if (ddaRingCheckPointer->Free()) { ++numLookaheadUnderruns; @@ -139,21 +141,21 @@ void Move::Spin() } // See if we can add another move to the ring - if ( + bool canAddMove = ( #if SUPPORT_ROLAND - !reprap.GetRoland()->Active() && + !reprap.GetRoland()->Active() && #endif - ddaRingAddPointer->GetState() == DDA::empty - && ddaRingAddPointer->GetNext()->GetState() != DDA::provisional // function Prepare needs to access the endpoints in the previous move, so don't change them - && DriveMovement::NumFree() >= (int)DRIVES // check that we won't run out of DMs - ) + ddaRingAddPointer->GetState() == DDA::empty + && ddaRingAddPointer->GetNext()->GetState() != DDA::provisional // function Prepare needs to access the endpoints in the previous move, so don't change them + && DriveMovement::NumFree() >= (int)DRIVES // check that we won't run out of DMs + ); + if (canAddMove) { // In order to react faster to speed and extrusion rate changes, only add more moves if the total duration of - // all un-frozen moves is less than 2 seconds, or the total duration of all but the first un-frozen move is - // less than 0.5 seconds. + // all un-frozen moves is less than 2 seconds, or the total duration of all but the first un-frozen move is less than 0.5 seconds. const DDA *dda = ddaRingAddPointer; - float unPreparedTime = 0.0; - float prevMoveTime = 0.0; + uint32_t unPreparedTime = 0; + uint32_t prevMoveTime = 0; for(;;) { dda = dda->GetPrevious(); @@ -162,119 +164,124 @@ void Move::Spin() break; } unPreparedTime += prevMoveTime; - prevMoveTime = dda->CalcTime(); + prevMoveTime = dda->GetClocksNeeded(); } - if (unPreparedTime < 0.5 || unPreparedTime + prevMoveTime < 2.0) + canAddMove = (unPreparedTime < DDA::stepClockRate/2 || unPreparedTime + prevMoveTime < 2 * DDA::stepClockRate); + } + + if (canAddMove) + { + // OK to add another move. First check if a special move is available. + if (specialMoveAvailable) { - // First check for a special move - if (specialMoveAvailable) + if (simulationMode < 2) { - if (simulationMode < 2) + if (ddaRingAddPointer->Init(specialMoveCoords)) { - if (ddaRingAddPointer->Init(specialMoveCoords)) + ddaRingAddPointer = ddaRingAddPointer->GetNext(); + if (moveState == MoveState::idle || moveState == MoveState::timing) { - ddaRingAddPointer = ddaRingAddPointer->GetNext(); - idleCount = 0; + // We were previously idle, so we have a state change + moveState = MoveState::collecting; + const uint32_t now = millis(); + const uint32_t timeWaiting = now - lastStateChangeTime; + if (timeWaiting > longestGcodeWaitInterval) + { + longestGcodeWaitInterval = timeWaiting; + } + lastStateChangeTime = now; } } - specialMoveAvailable = false; } - else + specialMoveAvailable = false; + } + else + { + // If there's a G Code move available, add it to the DDA ring for processing. + GCodes::RawMove nextMove; + if (reprap.GetGCodes().ReadMove(nextMove)) // if we have a new move { - // If there's a G Code move available, add it to the DDA ring for processing. - GCodes::RawMove nextMove; - if (reprap.GetGCodes().ReadMove(nextMove)) // if we have a new move + if (simulationMode < 2) // in simulation mode 2 and higher, we don't process incoming moves beyond this point { - if (waitingForMove) +#if 0 // disabled this because it causes jerky movements on the SCARA printer + // Add on the extrusion left over from last time. + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); + for (size_t drive = numAxes; drive < DRIVES; ++drive) { - waitingForMove = false; - const uint32_t timeWaiting = millis() - gcodeWaitStartTime; - if (timeWaiting > longestGcodeWaitInterval) - { - longestGcodeWaitInterval = timeWaiting; - } + nextMove.coords[drive] += extrusionPending[drive - numAxes]; } - if (simulationMode < 2) // in simulation mode 2 and higher, we don't process incoming moves beyond this point - { -#if 0 // disabled this because it causes jerky movements on the SCARA printer - // Add on the extrusion left over from last time. - const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); - for (size_t drive = numAxes; drive < DRIVES; ++drive) - { - nextMove.coords[drive] += extrusionPending[drive - numAxes]; - } #endif - if (nextMove.moveType == 0) - { - AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true); - } - if (ddaRingAddPointer->Init(nextMove, !IsRawMotorMove(nextMove.moveType))) - { - ddaRingAddPointer = ddaRingAddPointer->GetNext(); - idleCount = 0; - scheduledMoves++; - } -#if 0 // see above - // Save the amount of extrusion not done - for (size_t drive = numAxes; drive < DRIVES; ++drive) + if (nextMove.moveType == 0) + { + AxisAndBedTransform(nextMove.coords, nextMove.xAxes, nextMove.yAxes, true); + } + if (ddaRingAddPointer->Init(nextMove, !IsRawMotorMove(nextMove.moveType))) + { + ddaRingAddPointer = ddaRingAddPointer->GetNext(); + idleCount = 0; + scheduledMoves++; + if (moveState == MoveState::idle || moveState == MoveState::timing) { - extrusionPending[drive - numAxes] = nextMove.coords[drive]; + moveState = MoveState::collecting; + const uint32_t now = millis(); + const uint32_t timeWaiting = now - lastStateChangeTime; + if (timeWaiting > longestGcodeWaitInterval) + { + longestGcodeWaitInterval = timeWaiting; + } + lastStateChangeTime = now; } -#endif } - } - else - { - // We wanted another move, but none was available - if (currentDda != nullptr && !waitingForMove) +#if 0 // see above + // Save the amount of extrusion not done + for (size_t drive = numAxes; drive < DRIVES; ++drive) { - gcodeWaitStartTime = millis(); - waitingForMove = true; + extrusionPending[drive - numAxes] = nextMove.coords[drive]; } +#endif } } } } + // 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 + if (!canAddMove || idleCount > 10) // better to have a few moves in the queue so that we can do lookahead { - DDA *dda = ddaRingGetPointer; + // Prepare one move and execute it. We assume that we will enter the next if-block before it completes, giving us time to prepare more moves. + Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure + DDA * const dda = ddaRingGetPointer; // capture volatile variable if (dda->GetState() == DDA::provisional) { dda->Prepare(simulationMode); - } - if (dda->GetState() == DDA::frozen) - { if (simulationMode != 0) { currentDda = dda; // pretend we are executing this move } 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; + moveState = MoveState::executing; } else if (simulationMode != 0) { - if (iState == IdleState::busy && !reprap.GetGCodes().IsPaused()) + if (moveState == MoveState::executing && !reprap.GetGCodes().IsPaused()) { - lastMoveTime = millis(); // record when we first noticed that the machine was idle - iState = IdleState::timing; + lastStateChangeTime = millis(); // record when we first noticed that the machine was idle + moveState = MoveState::timing; } - else if (iState == IdleState::timing && millis() - lastMoveTime >= idleTimeout) + else if (moveState == MoveState::timing && millis() - lastStateChangeTime >= idleTimeout) { reprap.GetPlatform().SetDriversIdle(); // put all drives in idle hold - iState = IdleState::idle; + moveState = MoveState::idle; } } } @@ -283,7 +290,7 @@ void Move::Spin() DDA *cdda = currentDda; // currentDda is volatile, so copy it if (cdda != nullptr) { - // See whether we need to prepare any moves + // See whether we need to prepare any moves. First count how many prepared or executing moves we have and how long they will take. int32_t preparedTime = 0; uint32_t preparedCount = 0; DDA::DDAState st; @@ -298,13 +305,17 @@ void Move::Spin() } } - // If the number of prepared moves will execute in less than the minimum time, prepare another move + // If the number of prepared moves will execute in less than the minimum time, prepare another move. + // Try to avoid preparing deceleration-only moves 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 + && preparedTime < (int32_t)UsualMinimumPreparedTime // prepare moves one eighth of a second ahead of when they will be needed + && preparedCount < DdaRingLength/2 - 1 // but don't prepare as much as half the ring ) { - cdda->Prepare(simulationMode); + if (cdda->IsGoodToPrepare() || preparedTime < (int32_t)AbsoluteMinimumPreparedTime) + { + cdda->Prepare(simulationMode); + } preparedTime += cdda->GetTimeLeft(); ++preparedCount; cdda = cdda->GetNext(); @@ -317,7 +328,7 @@ void Move::Spin() // Simulate completion of the current move //DEBUG //currentDda->DebugPrint(); - simulationTime += currentDda->CalcTime(); + simulationTime += (float)currentDda->GetClocksNeeded()/DDA::stepClockRate; currentDda->Complete(); CurrentMoveCompleted(); } @@ -361,13 +372,10 @@ bool Move::IsRawMotorMove(uint8_t moveType) const bool Move::IsAccessibleProbePoint(float x, float y) const { const ZProbeParameters& params = reprap.GetPlatform().GetCurrentZProbeParameters(); - return kinematics->IsReachable(x - params.xOffset, y - params.yOffset); + return kinematics->IsReachable(x - params.xOffset, y - params.yOffset, false); } -// Pause the print as soon as we can, returning true if we are able to. -// Returns the file position of the first queue move we are going to skip, or noFilePosition we we are not skipping any moves. -// We update 'positions' to the positions and feed rate expected for the next move, and the amount of extrusion in the moves we skipped. -// If we are not skipping any moves then the feed rate, virtual extruder position and iobits are left alone, therefore the caller should set them up first. +// Pause the print as soon as we can, returning true if we are able to skip any moves and updating 'rp' to the first move we skipped. bool Move::PausePrint(RestorePoint& rp) { // Find a move we can pause after. @@ -393,7 +401,7 @@ bool Move::PausePrint(RestorePoint& rp) // In general, we can pause after a move if it is the last segment and its end speed is slow enough. // We can pause before a move if it is the first segment in that move. - const DDA *savedDdaRingAddPointer = ddaRingAddPointer; + const DDA * const savedDdaRingAddPointer = ddaRingAddPointer; bool pauseOkHere; cpu_irq_disable(); @@ -414,7 +422,6 @@ bool Move::PausePrint(RestorePoint& rp) if (pauseOkHere && dda->CanPauseBefore()) { // We can pause before executing this move - (void)dda->Free(); // free the DDA we are going to pause before it so that it won't get executed after we enable interrupts ddaRingAddPointer = dda; break; } @@ -460,7 +467,79 @@ bool Move::PausePrint(RestorePoint& rp) return true; } -uint32_t maxReps = 0; +// Pause the print immediately, returning true if we were able to skip or abort any moves and setting up to the move we aborted +bool Move::LowPowerPause(RestorePoint& rp) +{ + const DDA * const savedDdaRingAddPointer = ddaRingAddPointer; + bool abortedMove = false; + + cpu_irq_disable(); + DDA *dda = currentDda; + if (dda != nullptr && dda->GetFilePosition() != noFilePosition) + { + // We are executing a move that has a file address, so we can interrupt it + Platform::DisableStepInterrupt(); + dda->MoveAborted(); + CurrentMoveCompleted(); // updates live endpoints, extrusion, ddaRingGetPointer, currentDda etc. + --completedMoves; // this move wasn't really completed + abortedMove = true; + } + else + { + if (dda == nullptr) + { + // No move is being executed + dda = ddaRingGetPointer; + } + while (dda != savedDdaRingAddPointer) + { + if (dda->GetFilePosition() != noFilePosition) + { + break; // we can pause before executing this move + } + dda = dda->GetNext(); + } + } + + cpu_irq_enable(); + + if (dda == savedDdaRingAddPointer) + { + return false; // we can't skip any moves + } + + // We are going to skip some moves, or part of a move. + // Store the parameters of the first move we are going to execute when we resume + rp.feedRate = dda->GetRequestedSpeed(); + rp.virtualExtruderPosition = dda->GetVirtualExtruderPosition(); + rp.filePos = dda->GetFilePosition(); + rp.proportionDone = dda->GetProportionDone(); // store how much of the complete multi-segment move's extrusion has been done + +#if SUPPORT_IOBITS + rp.ioBits = dda->GetIoBits(); +#endif + + ddaRingAddPointer = (abortedMove) ? dda->GetNext() : dda; + + // Get the end coordinates of the last move that was or will be completed, or the coordinates of the current move when we aborted it. + DDA * const prevDda = ddaRingAddPointer->GetPrevious(); + const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes(); + for (size_t axis = 0; axis < numVisibleAxes; ++axis) + { + rp.moveCoords[axis] = prevDda->GetEndCoordinate(axis, false); + } + + InverseAxisAndBedTransform(rp.moveCoords, prevDda->GetXAxes(), prevDda->GetYAxes()); // we assume that xAxes and yAxes have't changed between the moves + + // Free the DDAs for the moves we are going to skip + for (dda = ddaRingAddPointer; dda != savedDdaRingAddPointer; dda = dda->GetNext()) + { + (void)dda->Free(); + scheduledMoves--; + } + + return true; +} #if 0 // For debugging @@ -473,8 +552,8 @@ void Move::Diagnostics(MessageType mtype) Platform& p = reprap.GetPlatform(); p.Message(mtype, "=== Move ===\n"); p.MessageF(mtype, "MaxReps: %" PRIu32 ", StepErrors: %u, FreeDm: %d, MinFreeDm %d, MaxWait: %" PRIu32 "ms, Underruns: %u, %u\n", - maxReps, stepErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns); - maxReps = 0; + DDA::maxReps, stepErrors, DriveMovement::NumFree(), DriveMovement::MinFree(), longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns); + DDA::maxReps = 0; numLookaheadUnderruns = numPrepareUnderruns = 0; longestGcodeWaitInterval = 0; DriveMovement::ResetMinFree(); @@ -584,10 +663,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], bool allowModeChange) const +bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool isCoordinated) const { const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), - reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, allowModeChange); + reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, isCoordinated); if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda)) { if (b) @@ -861,9 +940,9 @@ void Move::AdjustMotorPositions(const float_t adjustment[], size_t numMotors) const int32_t * const endCoordinates = lastQueuedMove->DriveCoordinates(); const float * const driveStepsPerUnit = reprap.GetPlatform().GetDriveStepsPerUnit(); - for (size_t drive = 0; drive < DELTA_AXES; ++drive) + for (size_t drive = 0; drive < numMotors; ++drive) { - const int32_t ep = endCoordinates[drive] + (int32_t)(adjustment[drive] * driveStepsPerUnit[drive]); + const int32_t ep = endCoordinates[drive] + lrintf(adjustment[drive] * driveStepsPerUnit[drive]); lastQueuedMove->SetDriveCoordinate(ep, drive); liveEndPoints[drive] = ep; } diff --git a/src/Movement/Move.h b/src/Movement/Move.h index ac145c41..38e55e5e 100644 --- a/src/Movement/Move.h +++ b/src/Movement/Move.h @@ -21,7 +21,7 @@ // Each DDA needs one DM per drive that it moves. // However, DM's are large, so we provide fewer than DRIVES * DdaRingLength of them. The planner checks that enough DMs are available before filling in a new DDA. -#ifdef DUET_NG +#if SAM4E || SAM4S const unsigned int DdaRingLength = 30; const unsigned int NumDms = DdaRingLength * 8; // suitable for e.g. a delta + 5 input hot end #else @@ -46,10 +46,9 @@ public: // Return the position (after all queued moves have been executed) in transformed coords int32_t GetEndPoint(size_t drive) const { return liveEndPoints[drive]; } // Get the current position of a motor void LiveCoordinates(float m[DRIVES], AxesBitmap xAxes, AxesBitmap yAxes); // Gives the last point at the end of the last complete DDA transformed to user coords - void Interrupt(); // The hardware's (i.e. platform's) interrupt should call this. - void InterruptTime(); // Test function - not used + void Interrupt() __attribute__ ((hot)); // The hardware's (i.e. platform's) interrupt should call this. bool AllMovesAreFinished(); // Is the look-ahead ring empty? Stops more moves being added as well. - void DoLookAhead(); // Run the look-ahead procedure + void DoLookAhead() __attribute__ ((hot)); // Run the look-ahead procedure void SetNewPosition(const float positionNow[DRIVES], bool doBedCompensation); // Set the current position to be this void SetLiveCoordinates(const float coords[DRIVES]); // Force the live coordinates (see above) to be these void ResetExtruderPositions(); // Resets the extrusion amounts of the live coordinates @@ -75,7 +74,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], bool allowModeChange) const; + bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool isCoordinated) 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 @@ -90,8 +89,8 @@ public: bool IsRawMotorMove(uint8_t moveType) const; // Return true if this is a raw motor move - void CurrentMoveCompleted(); // Signal that the current move has just been completed - bool TryStartNextMove(uint32_t startTime); // Try to start another move, returning true if Step() needs to be called immediately + void CurrentMoveCompleted() __attribute__ ((hot)); // Signal that the current move has just been completed + bool TryStartNextMove(uint32_t startTime) __attribute__ ((hot)); // Try to start another move, returning true if Step() needs to be called immediately float IdleTimeout() const; // Returns the idle timeout in seconds void SetIdleTimeout(float timeout); // Set the idle timeout in seconds @@ -100,6 +99,8 @@ public: void PrintCurrentDda() const; // For debugging bool PausePrint(RestorePoint& rp); // Pause the print as soon as we can, returning true if we were able to + bool LowPowerPause(RestorePoint& rp); // Pause the print immediately, returning true if we were able to + bool NoLiveMovement() const; // Is a move running, or are there any queued? bool IsExtruding() const; // Is filament being extruded? @@ -122,9 +123,15 @@ public: static float MotorEndpointToPosition(int32_t endpoint, size_t drive); // Convert number of motor steps to motor position private: - enum class IdleState : uint8_t { idle, busy, timing }; + enum class MoveState : uint8_t + { + idle, // no moves being executed or in queue, motors are at idle hold + collecting, // no moves currently being executed but we are collecting moves ready to execute them + executing, // we are executing moves + timing // no moves being executed or in queue, motors are at full current + }; - bool StartNextMove(uint32_t startTime); // Start the next move, returning true if Step() needs to be called immediately + bool StartNextMove(uint32_t startTime) __attribute__ ((hot)); // Start the next move, returning true if Step() needs to be called immediately void BedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Take a position and apply the bed compensations void InverseBedTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Go from a bed-transformed point back to user coordinates void AxisTransform(float move[MaxAxes], AxesBitmap xAxes, AxesBitmap yAxes) const; // Take a position and apply the axis-angle compensations @@ -142,12 +149,12 @@ private: bool active; // Are we live and running? uint8_t simulationMode; // Are we simulating, or really printing? - bool waitingForMove; // True if we are waiting for a new move + MoveState moveState; // whether the idle timer is active + unsigned int numLookaheadUnderruns; // How many times we have run out of moves to adjust during lookahead unsigned int numPrepareUnderruns; // How many times we wanted a new move but there were only un-prepared moves in the queue unsigned int idleCount; // The number of times Spin was called and had no new moves to process - uint32_t longestGcodeWaitInterval; // the longest we had to wait for a new gcode - uint32_t gcodeWaitStartTime; // When we last asked for a gcode and didn't get one + uint32_t longestGcodeWaitInterval; // the longest we had to wait for a new GCode float simulationTime; // Print time since we started simulating float extrusionPending[MaxExtruders]; // Extrusion not done due to rounding to nearest step @@ -170,9 +177,8 @@ private: float taperHeight; // Height over which we taper uint32_t idleTimeout; // How long we wait with no activity before we reduce motor currents to idle, in milliseconds - uint32_t lastMoveTime; // The approximate time at which the last move was completed + uint32_t lastStateChangeTime; // The approximate time at which the state last changed, except we don't record timing->idle uint32_t longWait; // A long time for things that need to be done occasionally - IdleState iState; // whether the idle timer is active Kinematics *kinematics; // What kinematics we are using |