diff options
Diffstat (limited to 'src')
71 files changed, 3515 insertions, 2179 deletions
diff --git a/src/Accelerometers/LIS3DH.cpp b/src/Accelerometers/LIS3DH.cpp index 65dcc52f..c783de83 100644 --- a/src/Accelerometers/LIS3DH.cpp +++ b/src/Accelerometers/LIS3DH.cpp @@ -224,7 +224,7 @@ unsigned int LIS3DH::CollectData(const uint16_t **collectedData, uint16_t &dataR const uint32_t interval = lastInterruptTime - firstInterruptTime; dataRate = (totalNumRead == 0 || interval == 0) ? 0 - : (totalNumRead * StepTimer::StepClockRate)/interval; + : (totalNumRead * StepClockRate)/interval; totalNumRead += numToRead; } return numToRead; diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp index 35ccc87e..d7d93946 100644 --- a/src/CAN/CanInterface.cpp +++ b/src/CAN/CanInterface.cpp @@ -892,6 +892,13 @@ pre(driver.IsRemote()) { case -1: case 0: + if (gb.SeenAny("RS")) + { + if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb)) + { + return GCodeResult::notFinished; + } + } { CanMessageGenericConstructor cons(M569Params); cons.PopulateFromCommand(gb); @@ -899,12 +906,26 @@ pre(driver.IsRemote()) } case 1: + if (gb.SeenAny("STERID")) + { + if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb)) + { + return GCodeResult::notFinished; + } + } { CanMessageGenericConstructor cons(M569Point1Params); cons.PopulateFromCommand(gb); return cons.SendAndGetResponse(CanMessageType::m569p1, driver.boardAddress, reply); } + case 2: + { + CanMessageGenericConstructor cons(M569Point2Params); + cons.PopulateFromCommand(gb); + return cons.SendAndGetResponse(CanMessageType::m569p2, driver.boardAddress, reply); + } + default: return GCodeResult::errorNotSupported; } diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp index c1c82dc2..da0375a6 100644 --- a/src/CAN/CanMotion.cpp +++ b/src/CAN/CanMotion.cpp @@ -53,75 +53,130 @@ void CanMotion::StartMovement() noexcept } } -// This is called by DDA::Prepare for each active CAN DM in the move -// If steps == 0 then the drivers just need to be enabled -void CanMotion::AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept +CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept { - if (canDriver.localDriver < MaxLinearDriversPerCanSlave) + if (canDriver.localDriver >= MaxLinearDriversPerCanSlave) { - // Search for the correct movement buffer - CanMessageBuffer* buf = movementBufferList; - while (buf != nullptr && buf->id.Dst() != canDriver.boardAddress) - { - buf = buf->next; - } + return nullptr; + } + CanMessageBuffer* buf = movementBufferList; + while (buf != nullptr && buf->id.Dst() != canDriver.boardAddress) + { + buf = buf->next; + } + + if (buf == nullptr) + { + // Allocate a new movement buffer + buf = CanMessageBuffer::Allocate(); if (buf == nullptr) { - // Allocate a new movement buffer - buf = CanMessageBuffer::Allocate(); - if (buf == nullptr) - { - reprap.GetPlatform().Message(ErrorMessage, "Out of CAN buffers\n"); - return; //TODO error handling - } + reprap.GetPlatform().Message(ErrorMessage, "Out of CAN buffers\n"); + return nullptr; //TODO error handling + } - buf->next = movementBufferList; - movementBufferList = buf; + buf->next = movementBufferList; + movementBufferList = buf; - auto move = buf->SetupRequestMessage<CanMessageMovementLinear>(0, CanId::MasterAddress, canDriver.boardAddress); +#if USE_REMOTE_INPUT_SHAPING + auto move = buf->SetupRequestMessage<CanMessageMovementLinearShaped>(0, CanId::MasterAddress, canDriver.boardAddress); +#else + auto move = buf->SetupRequestMessage<CanMessageMovementLinear>(0, CanId::MasterAddress, canDriver.boardAddress); +#endif - // Common parameters - if (buf->next == nullptr) - { - // This is the first CAN-connected board for this movement - move->accelerationClocks = lrintf(params.accelTime * StepTimer::StepClockRate); - move->steadyClocks = lrintf(params.steadyTime * StepTimer::StepClockRate); - move->decelClocks = lrintf(params.decelTime * StepTimer::StepClockRate); - currentMoveClocks = move->accelerationClocks + move->steadyClocks + move->decelClocks; - } - else - { - // Save some maths by using the values from the previous buffer - move->accelerationClocks = buf->next->msg.moveLinear.accelerationClocks; - move->steadyClocks = buf->next->msg.moveLinear.steadyClocks; - move->decelClocks = buf->next->msg.moveLinear.decelClocks; - } - move->initialSpeedFraction = params.initialSpeedFraction; - move->finalSpeedFraction = params.finalSpeedFraction; - move->pressureAdvanceDrives = 0; - move->numDrivers = canDriver.localDriver + 1; - move->zero = 0; - - // Clear out the per-drive fields. Can't use a range-based FOR loop on a packed struct. - for (size_t drive = 0; drive < ARRAY_SIZE(move->perDrive); ++drive) - { - move->perDrive[drive].Init(); - } + // Common parameters + if (buf->next == nullptr) + { + // This is the first CAN-connected board for this movement + move->accelerationClocks = (uint32_t)params.unshaped.accelClocks; + move->steadyClocks = (uint32_t)params.unshaped.steadyClocks; + move->decelClocks = (uint32_t)params.unshaped.decelClocks; + currentMoveClocks = move->accelerationClocks + move->steadyClocks + move->decelClocks; } - else if (canDriver.localDriver >= buf->msg.moveLinear.numDrivers) + else { - buf->msg.moveLinear.numDrivers = canDriver.localDriver + 1; + // Save some maths by using the values from the previous buffer +#if USE_REMOTE_INPUT_SHAPING + move->accelerationClocks = buf->next->msg.moveLinearShaped.accelerationClocks; + move->steadyClocks = buf->next->msg.moveLinearShaped.steadyClocks; + move->decelClocks = buf->next->msg.moveLinearShaped.decelClocks; +#else + move->accelerationClocks = buf->next->msg.moveLinear.accelerationClocks; + move->steadyClocks = buf->next->msg.moveLinear.steadyClocks; + move->decelClocks = buf->next->msg.moveLinear.decelClocks; +#endif } + move->initialSpeedFraction = params.initialSpeedFraction; + move->finalSpeedFraction = params.finalSpeedFraction; +#if USE_REMOTE_INPUT_SHAPING + move->numDriversMinusOne = canDriver.localDriver; + move->moveTypes = 0; + move->shaperAccelPhasesMinusOne = params.shapingPlan.accelSegments - 1; + move->shaperDecelPhasesMinusOne = params.shapingPlan.decelSegments - 1; +#else + move->pressureAdvanceDrives = 0; + move->numDrivers = canDriver.localDriver + 1; + move->zero = 0; +#endif + // Clear out the per-drive fields. Can't use a range-based FOR loop on a packed struct. + for (size_t drive = 0; drive < ARRAY_SIZE(move->perDrive); ++drive) + { + move->perDrive[drive].Init(); + } + } +#if USE_REMOTE_INPUT_SHAPING + else if (canDriver.localDriver > buf->msg.moveLinearShaped.numDriversMinusOne) + { + buf->msg.moveLinearShaped.numDriversMinusOne = canDriver.localDriver; + } +#else + else if (canDriver.localDriver >= buf->msg.moveLinear.numDrivers) + { + buf->msg.moveLinear.numDrivers = canDriver.localDriver + 1; + } +#endif + return buf; +} + +// This is called by DDA::Prepare for each active CAN DM in the move +#if USE_REMOTE_INPUT_SHAPING +void CanMotion::AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps) noexcept +#else +void CanMotion::AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept +#endif +{ + CanMessageBuffer * const buf = GetBuffer(params, canDriver); + if (buf != nullptr) + { +#if USE_REMOTE_INPUT_SHAPING + buf->msg.moveLinearShaped.perDrive[canDriver.localDriver].iSteps = steps; +#else buf->msg.moveLinear.perDrive[canDriver.localDriver].steps = steps; if (usePressureAdvance) { buf->msg.moveLinear.pressureAdvanceDrives |= 1u << canDriver.localDriver; } +#endif + } +} + +#if USE_REMOTE_INPUT_SHAPING + +void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept +{ + CanMessageBuffer * const buf = GetBuffer(params, canDriver); + if (buf != nullptr) + { + buf->msg.moveLinearShaped.perDrive[canDriver.localDriver].fDist = extrusion; + const auto mt = (usePressureAdvance) ? CanMessageMovementLinearShaped::MoveType::extruderWithPa : CanMessageMovementLinearShaped::MoveType::extruderNoPa; + buf->msg.moveLinearShaped.ChangeMoveTypeFromDefault(canDriver.localDriver, mt); } } +#endif + // This is called by DDA::Prepare when all DMs for CAN drives have been processed. Return the calculated move time in steps, or 0 if there are no CAN moves uint32_t CanMotion::FinishMovement(uint32_t moveStartTime) noexcept { @@ -138,11 +193,19 @@ uint32_t CanMotion::FinishMovement(uint32_t moveStartTime) noexcept do { boardsActiveInLastMove.SetBit(buf->id.Dst()); //TODO should we set this if there were no steps for drives on the board, just drives to be enabled? +#if USE_REMOTE_INPUT_SHAPING + buf->msg.moveLinearShaped.whenToExecute = moveStartTime; + uint8_t& seq = nextSeq[buf->id.Dst()]; + buf->msg.moveLinearShaped.seq = seq; + seq = (seq + 1) & 0x7F; + buf->dataLength = buf->msg.moveLinearShaped.GetActualDataLength(); +#else buf->msg.moveLinear.whenToExecute = moveStartTime; uint8_t& seq = nextSeq[buf->id.Dst()]; buf->msg.moveLinear.seq = seq; seq = (seq + 1) & 0x7F; buf->dataLength = buf->msg.moveLinear.GetActualDataLength(); +#endif CanMessageBuffer * const nextBuffer = buf->next; // must get this before sending the buffer, because sending the buffer releases it CanInterface::SendMotion(buf); // queues the buffer for sending and frees it when done #if 0 @@ -232,7 +295,11 @@ void CanMotion::StopDriver(bool isBeingPrepared, DriverId driver) noexcept if (buf != nullptr) { +#if USE_REMOTE_INPUT_SHAPING + buf->msg.moveLinearShaped.perDrive[driver.localDriver].steps = 0; +#else buf->msg.moveLinear.perDrive[driver.localDriver].steps = 0; +#endif } } else @@ -277,11 +344,19 @@ void CanMotion::StopAll(bool isBeingPrepared) noexcept // We still send the messages so that the drives get enabled, but we set the steps to zero for (CanMessageBuffer *buf = movementBufferList; buf != nullptr; buf = buf->next) { +#if USE_REMOTE_INPUT_SHAPING + buf->msg.moveLinearShaped.accelerationClocks = buf->msg.moveLinearShaped.decelClocks = buf->msg.moveLinearShaped.steadyClocks = 0; + for (size_t drive = 0; drive < ARRAY_SIZE(buf->msg.moveLinearShaped.perDrive); ++drive) + { + buf->msg.moveLinearShaped.perDrive[drive].steps = 0; + } +#else buf->msg.moveLinear.accelerationClocks = buf->msg.moveLinear.decelClocks = buf->msg.moveLinear.steadyClocks = 0; for (size_t drive = 0; drive < ARRAY_SIZE(buf->msg.moveLinear.perDrive); ++drive) { buf->msg.moveLinear.perDrive[drive].steps = 0; } +#endif } } else diff --git a/src/CAN/CanMotion.h b/src/CAN/CanMotion.h index 40485f41..e14aadcf 100644 --- a/src/CAN/CanMotion.h +++ b/src/CAN/CanMotion.h @@ -18,7 +18,12 @@ namespace CanMotion { void Init() noexcept; void StartMovement() noexcept; - void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept; +#if USE_REMOTE_INPUT_SHAPING + void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps) noexcept; + void AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept; +#else + void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance = false) noexcept; +#endif uint32_t FinishMovement(uint32_t moveStartTime) noexcept; bool CanPrepareMove() noexcept; CanMessageBuffer *GetUrgentMessage() noexcept; diff --git a/src/CAN/CommandProcessor.cpp b/src/CAN/CommandProcessor.cpp index 460a1ca0..05307526 100644 --- a/src/CAN/CommandProcessor.cpp +++ b/src/CAN/CommandProcessor.cpp @@ -407,6 +407,12 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept reprap.GetMove().AddMoveFromRemote(buf->msg.moveLinear); return; // no reply needed +#if USE_REMOTE_INPUT_SHAPING + case CanMessageType::movementLinearShaped: + reprap.GetMove().AddShapedMoveFromRemote(buf->msg.moveLinearShaped); + return; // no reply needed +#endif + case CanMessageType::returnInfo: requestId = buf->msg.getInfo.requestId; rslt = EutGetInfo(buf->msg.getInfo, replyRef, extra); @@ -444,7 +450,7 @@ void CommandProcessor::ProcessReceivedMessage(CanMessageBuffer *buf) noexcept case CanMessageType::setPressureAdvance: requestId = buf->msg.multipleDrivesRequestFloat.requestId; - rslt = reprap.GetPlatform().EutSetRemotePressureAdvance(buf->msg.multipleDrivesRequestFloat, buf->dataLength, replyRef); + rslt = reprap.GetMove().EutSetRemotePressureAdvance(buf->msg.multipleDrivesRequestFloat, buf->dataLength, replyRef); break; case CanMessageType::m569: diff --git a/src/Configuration.h b/src/Configuration.h index efdd6141..89765445 100644 --- a/src/Configuration.h +++ b/src/Configuration.h @@ -48,7 +48,12 @@ constexpr float DefaultXYInstantDv = 15.0; // mm/sec constexpr float DefaultZInstantDv = 0.2; constexpr float DefaultEInstantDv = 2.0; -constexpr float DefaultMinFeedrate = 0.5; // The minimum movement speed (extruding moves will go slower than this if the extrusion rate demands it) +constexpr float DefaultMinFeedrate = 0.5; // the default minimum movement speed in mm/sec (extruding moves will go slower than this if the extrusion rate demands it) +constexpr float AbsoluteMinFeedrate = 0.01; // the absolute minimum movement speed in mm/sec +constexpr float MinimumJerk = 0.1; // the minimum jerk in mm/sec +constexpr float MinimumAcceleration = 0.1; // the minimum acceleration in mm/sec^2 +constexpr float DefaultPrintingAcceleration = 20000.0; // higher than the likely max acceleration defined by M201 +constexpr float DefaultTravelAcceleration = 20000.0; // higher than the likely max acceleration defined by M201 constexpr float DefaultAxisMinimum = 0.0; constexpr float DefaultAxisMaximum = 200.0; @@ -288,7 +293,8 @@ constexpr size_t ObjectNamesStringSpace = 500; // How much space we reserve fo // Move system constexpr float DefaultFeedRate = 3000.0; // The initial requested feed rate after resetting the printer, in mm/min constexpr float DefaultG0FeedRate = 18000; // The initial feed rate for G0 commands after resetting the printer, in mm/min -constexpr float DefaultRetractSpeed = 1000.0; // The default firmware retraction and un-retraction speed, in mm +constexpr float MinRetractSpeed = 60.0; // The minimum firmware retraction/un-retraction speed in mm/min +constexpr float DefaultRetractSpeed = 1000.0; // The default firmware retraction and un-retraction speed, in mm/min constexpr float DefaultRetractLength = 2.0; constexpr float MaxArcDeviation = 0.005; // maximum deviation from ideal arc due to segmentation diff --git a/src/Endstops/ZProbe.cpp b/src/Endstops/ZProbe.cpp index 95853119..2147b664 100644 --- a/src/Endstops/ZProbe.cpp +++ b/src/Endstops/ZProbe.cpp @@ -56,31 +56,31 @@ constexpr ObjectModelArrayDescriptor ZProbe::speedsArrayDescriptor = nullptr, [] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return ARRAY_SIZE(ZProbe::probeSpeeds); }, [] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue - { return ExpressionValue(((const ZProbe*)self)->probeSpeeds[context.GetLastIndex()], 1); } + { return ExpressionValue(InverseConvertSpeedToMmPerMin(((const ZProbe*)self)->probeSpeeds[context.GetLastIndex()]), 1); } }; constexpr ObjectModelTableEntry ZProbe::objectModelTable[] = { // Within each group, these entries must be in alphabetical order // 0. Probe members - { "calibrationTemperature", OBJECT_MODEL_FUNC(self->calibTemperature, 1), ObjectModelEntryFlags::none }, - { "deployedByUser", OBJECT_MODEL_FUNC(self->isDeployedByUser), ObjectModelEntryFlags::none }, - { "disablesHeaters", OBJECT_MODEL_FUNC((bool)self->misc.parts.turnHeatersOff), ObjectModelEntryFlags::none }, - { "diveHeight", OBJECT_MODEL_FUNC(self->diveHeight, 1), ObjectModelEntryFlags::none }, - { "lastStopHeight", OBJECT_MODEL_FUNC(self->lastStopHeight, 3), ObjectModelEntryFlags::none }, - { "maxProbeCount", OBJECT_MODEL_FUNC((int32_t)self->misc.parts.maxTaps), ObjectModelEntryFlags::none }, - { "offsets", OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), ObjectModelEntryFlags::none }, - { "recoveryTime", OBJECT_MODEL_FUNC(self->recoveryTime, 1), ObjectModelEntryFlags::none }, - { "speed", OBJECT_MODEL_FUNC(self->probeSpeeds[1], 1), ObjectModelEntryFlags::obsolete }, - { "speeds", OBJECT_MODEL_FUNC_NOSELF(&speedsArrayDescriptor), ObjectModelEntryFlags::none }, - { "temperatureCoefficient", OBJECT_MODEL_FUNC(self->temperatureCoefficients[0], 5), ObjectModelEntryFlags::obsolete }, - { "temperatureCoefficients", OBJECT_MODEL_FUNC_NOSELF(&temperatureCoefficientsArrayDescriptor), ObjectModelEntryFlags::none }, - { "threshold", OBJECT_MODEL_FUNC((int32_t)self->adcValue), ObjectModelEntryFlags::none }, - { "tolerance", OBJECT_MODEL_FUNC(self->tolerance, 3), ObjectModelEntryFlags::none }, - { "travelSpeed", OBJECT_MODEL_FUNC(self->travelSpeed, 1), ObjectModelEntryFlags::none }, - { "triggerHeight", OBJECT_MODEL_FUNC(-self->offsets[Z_AXIS], 3), ObjectModelEntryFlags::none }, - { "type", OBJECT_MODEL_FUNC((int32_t)self->type), ObjectModelEntryFlags::none }, - { "value", OBJECT_MODEL_FUNC_NOSELF(&valueArrayDescriptor), ObjectModelEntryFlags::live }, + { "calibrationTemperature", OBJECT_MODEL_FUNC(self->calibTemperature, 1), ObjectModelEntryFlags::none }, + { "deployedByUser", OBJECT_MODEL_FUNC(self->isDeployedByUser), ObjectModelEntryFlags::none }, + { "disablesHeaters", OBJECT_MODEL_FUNC((bool)self->misc.parts.turnHeatersOff), ObjectModelEntryFlags::none }, + { "diveHeight", OBJECT_MODEL_FUNC(self->diveHeight, 1), ObjectModelEntryFlags::none }, + { "lastStopHeight", OBJECT_MODEL_FUNC(self->lastStopHeight, 3), ObjectModelEntryFlags::none }, + { "maxProbeCount", OBJECT_MODEL_FUNC((int32_t)self->misc.parts.maxTaps), ObjectModelEntryFlags::none }, + { "offsets", OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), ObjectModelEntryFlags::none }, + { "recoveryTime", OBJECT_MODEL_FUNC(self->recoveryTime, 1), ObjectModelEntryFlags::none }, + { "speed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerMin(self->probeSpeeds[1]), 1), ObjectModelEntryFlags::obsolete }, + { "speeds", OBJECT_MODEL_FUNC_NOSELF(&speedsArrayDescriptor), ObjectModelEntryFlags::none }, + { "temperatureCoefficient", OBJECT_MODEL_FUNC(self->temperatureCoefficients[0], 5), ObjectModelEntryFlags::obsolete }, + { "temperatureCoefficients", OBJECT_MODEL_FUNC_NOSELF(&temperatureCoefficientsArrayDescriptor), ObjectModelEntryFlags::none }, + { "threshold", OBJECT_MODEL_FUNC((int32_t)self->adcValue), ObjectModelEntryFlags::none }, + { "tolerance", OBJECT_MODEL_FUNC(self->tolerance, 3), ObjectModelEntryFlags::none }, + { "travelSpeed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerMin(self->travelSpeed), 1), ObjectModelEntryFlags::none }, + { "triggerHeight", OBJECT_MODEL_FUNC(-self->offsets[Z_AXIS], 3), ObjectModelEntryFlags::none }, + { "type", OBJECT_MODEL_FUNC((int32_t)self->type), ObjectModelEntryFlags::none }, + { "value", OBJECT_MODEL_FUNC_NOSELF(&valueArrayDescriptor), ObjectModelEntryFlags::live }, }; constexpr uint8_t ZProbe::objectModelTableDescriptor[] = { 1, 18 }; @@ -386,14 +386,14 @@ GCodeResult ZProbe::Configure(GCodeBuffer& gb, const StringRef &reply, bool& see float userProbeSpeeds[2]; size_t numSpeeds = 2; gb.GetFloatArray(userProbeSpeeds, numSpeeds, true); - probeSpeeds[0] = userProbeSpeeds[0] * SecondsToMinutes; - probeSpeeds[1] = userProbeSpeeds[1] * SecondsToMinutes; + probeSpeeds[0] = ConvertSpeedFromMmPerMin(userProbeSpeeds[0]); + probeSpeeds[1] = ConvertSpeedFromMmPerMin(userProbeSpeeds[1]); seen = true; } if (gb.Seen('T')) // travel speed to probe point { - travelSpeed = gb.GetFValue() * SecondsToMinutes; + travelSpeed = gb.GetSpeedFromMm(false); seen = true; } @@ -422,7 +422,9 @@ GCodeResult ZProbe::Configure(GCodeBuffer& gb, const StringRef &reply, bool& see const GCodeResult rslt = AppendPinNames(reply); reply.catf(", dive height %.1fmm, probe speeds %d,%dmm/min, travel speed %dmm/min, recovery time %.2f sec, heaters %s, max taps %u, max diff %.2f", (double)diveHeight, - (int)(probeSpeeds[0] * MinutesToSeconds), (int)(probeSpeeds[1] * MinutesToSeconds), (int)(travelSpeed * MinutesToSeconds), + (int)InverseConvertSpeedToMmPerMin(probeSpeeds[0]), + (int)InverseConvertSpeedToMmPerMin(probeSpeeds[1]), + (int)InverseConvertSpeedToMmPerMin(travelSpeed), (double)recoveryTime, (misc.parts.turnHeatersOff) ? "suspended" : "normal", misc.parts.maxTaps, (double)tolerance); diff --git a/src/Endstops/ZProbe.h b/src/Endstops/ZProbe.h index 8cda06c9..ca5c3e3f 100644 --- a/src/Endstops/ZProbe.h +++ b/src/Endstops/ZProbe.h @@ -92,8 +92,8 @@ protected: float calibTemperature; // the temperature at which we did the calibration float temperatureCoefficients[2]; // the variation of height with bed temperature and with the square of temperature float diveHeight; // the dive height we use when probing - float probeSpeeds[2]; // the initial speed of probing - float travelSpeed; // the speed at which we travel to the probe point + float probeSpeeds[2]; // the initial speed of probing in mm per step clock + float travelSpeed; // the speed at which we travel to the probe point ni mm per step clock float recoveryTime; // Z probe recovery time float tolerance; // maximum difference between probe heights when doing >1 taps float lastStopHeight; // the height at which the last G30 probe move stopped diff --git a/src/Fans/LedStripDriver.cpp b/src/Fans/LedStripDriver.cpp index 1fe44f62..647302d3 100644 --- a/src/Fans/LedStripDriver.cpp +++ b/src/Fans/LedStripDriver.cpp @@ -53,7 +53,7 @@ namespace LedStripDriver { constexpr uint32_t DefaultDotStarSpiClockFrequency = 1000000; // 1MHz default constexpr uint32_t DefaultNeoPixelSpiClockFrequency = 2500000; // must be between about 2MHz and about 4MHz - constexpr uint32_t MinNeoPixelResetTicks = (250 * StepTimer::StepClockRate)/1000000; // 250us minimum Neopixel reset time on later chips + constexpr uint32_t MinNeoPixelResetTicks = (250 * StepClockRate)/1000000; // 250us minimum Neopixel reset time on later chips // Define the size of the buffer used to accumulate a sequence of colours to send to the string #if defined(DUET3_V06) diff --git a/src/Fans/LocalFan.cpp b/src/Fans/LocalFan.cpp index eb461670..bef4ccad 100644 --- a/src/Fans/LocalFan.cpp +++ b/src/Fans/LocalFan.cpp @@ -215,8 +215,8 @@ int32_t LocalFan::GetRPM() const noexcept // When the fan stops, we get no interrupts and fanInterval stops getting updated. We must recognise this and return zero. return (!tachoPort.IsValid()) ? -1 // we return -1 if there is no tacho configured - : (fanInterval != 0 && StepTimer::GetTimerTicks() - fanLastResetTime < 3 * StepTimer::StepClockRate) // if we have a reading and it is less than 3 seconds old - ? (StepTimer::StepClockRate * fanMaxInterruptCount * (60/2))/fanInterval // then calculate RPM assuming 2 interrupts per rev + : (fanInterval != 0 && StepTimer::GetTimerTicks() - fanLastResetTime < 3 * StepClockRate) // if we have a reading and it is less than 3 seconds old + ? (StepClockRate * fanMaxInterruptCount * (60/2))/fanInterval // then calculate RPM assuming 2 interrupts per rev : 0; // else assume fan is off or tacho not connected } diff --git a/src/FilamentMonitors/Duet3DFilamentMonitor.cpp b/src/FilamentMonitors/Duet3DFilamentMonitor.cpp index 41c957c9..cc91f0d9 100644 --- a/src/FilamentMonitors/Duet3DFilamentMonitor.cpp +++ b/src/FilamentMonitors/Duet3DFilamentMonitor.cpp @@ -85,7 +85,7 @@ Duet3DFilamentMonitor::PollResult Duet3DFilamentMonitor::PollReceiveBuffer(uint1 { // For the Duet3D sensors we need to decode the received data from the transition times recorded in the edgeCaptures array static constexpr uint32_t BitsPerSecond = 1000; // the nominal bit rate that the data is transmitted at - static constexpr uint32_t NominalBitLength = StepTimer::StepClockRate/BitsPerSecond; // the nominal bit length in step clocks + static constexpr uint32_t NominalBitLength = StepClockRate/BitsPerSecond; // the nominal bit length in step clocks static constexpr uint32_t MinBitLength = (NominalBitLength * 10)/13; // allow 30% clock speed tolerance static constexpr uint32_t MaxBitLength = (NominalBitLength * 13)/10; // allow 30% clock speed tolerance static constexpr uint32_t ErrorRecoveryDelayBits = 8; // before a start bit we want the line to be low for this long diff --git a/src/GCodes/GCodeBuffer/BinaryParser.cpp b/src/GCodes/GCodeBuffer/BinaryParser.cpp index de4b7b02..1b6d3398 100644 --- a/src/GCodes/GCodeBuffer/BinaryParser.cpp +++ b/src/GCodes/GCodeBuffer/BinaryParser.cpp @@ -88,6 +88,25 @@ bool BinaryParser::Seen(char c) noexcept return false; } +// Return true if any of the parameter letters in the bitmap were seen +bool BinaryParser::SeenAny(Bitmap<uint32_t> bm) const noexcept +{ + if (bufferLength != 0 && header->numParameters != 0) + { + const char *parameterStart = reinterpret_cast<const char*>(gb.buffer) + sizeof(CodeHeader); + for (size_t i = 0; i < header->numParameters; i++) + { + const CodeParameter *param = reinterpret_cast<const CodeParameter*>(parameterStart + i * sizeof(CodeParameter)); + const char paramLetter = param->letter; + if (paramLetter >= 'A' && paramLetter <= 'Z' && bm.IsBitSet(paramLetter - 'A')) + { + return true; + } + } + } + return false; +} + char BinaryParser::GetCommandLetter() const noexcept { return (bufferLength != 0) ? header->letter : 'Q'; @@ -455,19 +474,58 @@ void BinaryParser::GetPossiblyQuotedString(const StringRef& str, bool allowEmpty } } -void BinaryParser::GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) +void BinaryParser::GetFloatArray(float arr[], size_t& length) THROWS(GCodeException) { - GetArray(arr, length, doPad, DataType::Float); + if (seenParameter == nullptr) + { + THROW_INTERNAL_ERROR; + } + + if (seenParameter->type == DataType::Expression) + { + ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1); + parser.ParseFloatArray(arr, length); + } + else + { + GetArray(arr, length); + } } -void BinaryParser::GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException) +void BinaryParser::GetIntArray(int32_t arr[], size_t& length) THROWS(GCodeException) { - GetArray(arr, length, doPad, DataType::Int); + if (seenParameter == nullptr) + { + THROW_INTERNAL_ERROR; + } + + if (seenParameter->type == DataType::Expression) + { + ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1); + parser.ParseIntArray(arr, length); + } + else + { + GetArray(arr, length); + } } -void BinaryParser::GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException) +void BinaryParser::GetUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException) { - GetArray(arr, length, doPad, DataType::UInt); + if (seenParameter == nullptr) + { + THROW_INTERNAL_ERROR; + } + + if (seenParameter->type == DataType::Expression) + { + ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1); + parser.ParseUnsignedArray(arr, length); + } + else + { + GetArray(arr, length); + } } // Get a :-separated list of drivers after a key letter @@ -499,15 +557,12 @@ void BinaryParser::GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCode break; case DataType::Expression: - { - float temp[seenParameter->intValue]; - GetArray(temp, length, false, DataType::Float); - for (int i = 0; i < seenParameter->intValue; i++) { - SetDriverIdFromBinary(arr[i], temp[i]); + ExpressionParser parser(gb, seenParameterValue, seenParameterValue + seenParameter->intValue, -1); + parser.ParseDriverIdArray(arr, length); + parser.CheckForExtraCharacters(); } break; - } default: length = 0; @@ -573,13 +628,8 @@ void BinaryParser::AppendFullCommand(const StringRef &s) const noexcept } } -template<typename T> void BinaryParser::GetArray(T arr[], size_t& length, bool doPad, DataType type) THROWS(GCodeException) +template<typename T> void BinaryParser::GetArray(T arr[], size_t& length) THROWS(GCodeException) { - if (seenParameter == nullptr) - { - THROW_INTERNAL_ERROR; - } - int lastIndex = -1; switch (seenParameter->type) { @@ -627,122 +677,12 @@ template<typename T> void BinaryParser::GetArray(T arr[], size_t& length, bool d lastIndex = seenParameter->intValue - 1; break; - case DataType::Expression: - { - const char *pos = seenParameterValue, *endPos = seenParameterValue + seenParameter->intValue; - - // Check if the whole expression is encapsulated in curly braces and remove them if necessary - if (*pos == '{' && pos != endPos) - { - bool isEncapsulated = true, inQuotes = false; - size_t numBraces = 1; - for (const char *str = pos + 1; str < endPos; str++) - { - if (inQuotes) - { - inQuotes = (*str != '"'); - } - else if (*str == '"') - { - inQuotes = true; - } - else if (*str == '{') - { - numBraces++; - } - else if (*str == '}') - { - numBraces--; - if (numBraces == 0) - { - const char *curPos = str; - while (str != endPos && strchr("\t ", *++str) != nullptr) { } - if (str == endPos) - { - endPos = curPos + 1; - } - else - { - isEncapsulated = false; - } - break; - } - } - } - - if (isEncapsulated) - { - pos++; - endPos--; - } - } - - // Read array expression - for (;;) - { - if (lastIndex >= (int)length) - { - throw ConstructParseException("array too long, max length = %u", (uint32_t)length); - } - - // Read the next expression value - ExpressionParser parser(gb, pos, endPos, -1); - switch (type) - { - case DataType::Int: - arr[++lastIndex] = (T)parser.ParseInteger(); - break; - - case DataType::UInt: - arr[++lastIndex] = (T)parser.ParseUnsigned(); - break; - - case DataType::Float: - arr[++lastIndex] = (T)parser.ParseFloat(); - break; - - default: - throw ConstructParseException("Unsupported array data type"); - } - parser.CheckForExtraCharacters(true); - pos = parser.GetEndptr(); - - if (pos++ >= endPos) - { - break; - } - } - - if (doPad && lastIndex == 0) - { - for (size_t i = 1; i < length; i++) - { - arr[i] = arr[0]; - } - } - else - { - length = lastIndex + 1; - } - break; - } - default: length = 0; return; } - if (doPad && lastIndex == 0) - { - for (size_t i = 1; i < length; i++) - { - arr[i] = arr[0]; - } - } - else - { - length = lastIndex + 1; - } + length = lastIndex + 1; } void BinaryParser::CheckArrayLength(size_t maxLength) THROWS(GCodeException) diff --git a/src/GCodes/GCodeBuffer/BinaryParser.h b/src/GCodes/GCodeBuffer/BinaryParser.h index 3495d9c8..adb2286b 100644 --- a/src/GCodes/GCodeBuffer/BinaryParser.h +++ b/src/GCodes/GCodeBuffer/BinaryParser.h @@ -24,10 +24,11 @@ class BinaryParser { public: BinaryParser(GCodeBuffer& gcodeBuffer) noexcept; - void Init() noexcept; // Set it up to parse another G-code - void Put(const uint32_t *data, size_t len) noexcept; // Add an entire binary code, overwriting any existing content - void DecodeCommand() noexcept; // Print the buffer content in debug mode and prepare for execution - bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? + void Init() noexcept; // Set it up to parse another G-code + void Put(const uint32_t *data, size_t len) noexcept; // Add an entire binary code, overwriting any existing content + void DecodeCommand() noexcept; // Print the buffer content in debug mode and prepare for execution + bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? + bool SeenAny(Bitmap<uint32_t> bm) const noexcept; // Return true if any of the parameter letters in the bitmap were seen char GetCommandLetter() const noexcept; bool HasCommandNumber() const noexcept; @@ -45,9 +46,9 @@ public: void GetCompleteParameters(const StringRef& str) THROWS(GCodeException); // Get the complete parameter string void GetQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException); // Get and copy a quoted string void GetPossiblyQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException); // Get and copy a string which may or may not be quoted - void GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter - void GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException); // Get a :-separated list of ints after a key letter - void GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException); // Get a :-separated list of unsigned ints after a key letter + void GetFloatArray(float arr[], size_t& length) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter + void GetIntArray(int32_t arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of ints after a key letter + void GetUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of unsigned ints after a key letter void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of drivers after a key letter void SetFinished() noexcept; // Set the G Code finished @@ -72,7 +73,7 @@ private: GCodeException ConstructParseException(const char *str, uint32_t param) const noexcept; size_t AddPadding(size_t bytesRead) const noexcept { return (bytesRead + 3u) & (~3u); } - template<typename T> void GetArray(T arr[], size_t& length, bool doPad, DataType type) THROWS(GCodeException) SPEED_CRITICAL; + template<typename T> void GetArray(T arr[], size_t& length) THROWS(GCodeException) SPEED_CRITICAL; void WriteParameters(const StringRef& s, bool quoteStrings) const noexcept; size_t bufferLength; diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.cpp b/src/GCodes/GCodeBuffer/ExpressionParser.cpp index 0e3ac432..0c65a39c 100644 --- a/src/GCodes/GCodeBuffer/ExpressionParser.cpp +++ b/src/GCodes/GCodeBuffer/ExpressionParser.cpp @@ -560,6 +560,60 @@ uint32_t ExpressionParser::ParseUnsigned() THROWS(GCodeException) } } +DriverId ExpressionParser::ParseDriverId() THROWS(GCodeException) +{ + ExpressionValue val = Parse(); + ConvertToDriverId(val, true); + return val.GetDriverIdValue(); +} + +void ExpressionParser::ParseArray(size_t& length, function_ref<void(size_t index)> processElement) THROWS(GCodeException) +{ + size_t numElements = 0; + AdvancePointer(); // skip the '{' + while (numElements < length) + { + processElement(numElements); + ++numElements; + if (CurrentCharacter() != ',') + { + break; + } + if (numElements == length) + { + ThrowParseException("Array too long"); + } + AdvancePointer(); // skip the '{' + } + if (CurrentCharacter() != '}') + { + ThrowParseException("Expected '}'"); + } + AdvancePointer(); // skip the '{' + length = numElements; +} + +// This is called when we expect a non-empty float array parameter and we have encountered (but not skipped) '{' +void ExpressionParser::ParseFloatArray(float arr[], size_t& length) THROWS(GCodeException) +{ + ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseFloat(); }); +} + +void ExpressionParser::ParseIntArray(int32_t arr[], size_t& length) THROWS(GCodeException) +{ + ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseInteger(); }); +} + +void ExpressionParser::ParseUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException) +{ + ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseUnsigned(); }); +} + +void ExpressionParser::ParseDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException) +{ + ParseArray(length, [this, &arr](size_t index) { arr[index] = ParseDriverId(); }); +} + void ExpressionParser::BalanceNumericTypes(ExpressionValue& val1, ExpressionValue& val2, bool evaluate) const THROWS(GCodeException) { // First convert any Uint64 or Uint32 operands to float @@ -704,6 +758,51 @@ void ExpressionParser::ConvertToString(ExpressionValue& val, bool evaluate) noex } } +void ExpressionParser::ConvertToDriverId(ExpressionValue& val, bool evaluate) const THROWS(GCodeException) +{ + switch (val.GetType()) + { + case TypeCode::DriverId: + break; + + case TypeCode::Int32: +#if SUPPORT_CAN_EXPANSION + val.Set(DriverId(0, val.uVal)); +#else + val.Set(DriverId(val.uVal)); +#endif + break; + + case TypeCode::Float: + { + const float f10val = 10.0 * val.fVal; + const int32_t ival = lrintf(f10val); +#if SUPPORT_CAN_EXPANSION + if (ival >= 0 && fabsf(f10val - (float)ival) <= 0.002) + { + val.Set(ival/10, ival % 10); + } +#else + if (ival >= 0 && ival < 10 && fabsf(f10val - (float)ival) <= 0.002) + { + val.Set(0, ival % 10); + } +#endif + else + { + ThrowParseException("invalid driver ID"); + } + } + break; + + default: + if (evaluate) + { + ThrowParseException("expected driver ID"); + } + } +} + void ExpressionParser::SkipWhiteSpace() noexcept { char c; diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.h b/src/GCodes/GCodeBuffer/ExpressionParser.h index bfd4de4e..adfff7c7 100644 --- a/src/GCodes/GCodeBuffer/ExpressionParser.h +++ b/src/GCodes/GCodeBuffer/ExpressionParser.h @@ -24,6 +24,13 @@ public: float ParseFloat() THROWS(GCodeException); int32_t ParseInteger() THROWS(GCodeException); uint32_t ParseUnsigned() THROWS(GCodeException); + DriverId ParseDriverId() THROWS(GCodeException); + + // These 4 functions may be removed when we support array-valued expressions more generally + void ParseFloatArray(float arr[], size_t& length) THROWS(GCodeException); + void ParseIntArray(int32_t arr[], size_t& length) THROWS(GCodeException); + void ParseUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException); + void ParseDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException); void SkipWhiteSpace() noexcept; void CheckForExtraCharacters(bool isArrayExpression = false) THROWS(GCodeException); @@ -41,11 +48,15 @@ private: void __attribute__((noinline)) ParseIdentifierExpression(ExpressionValue& rslt, bool evaluate, bool applyLengthOperator, bool applyExists) THROWS(GCodeException) pre(readPointer >= 0; isalpha(gb.buffer[readPointer])); void __attribute__((noinline)) ParseQuotedString(ExpressionValue& rslt) THROWS(GCodeException); + + void ParseArray(size_t& length, function_ref<void(size_t index)> processElement) THROWS(GCodeException); + void GetVariableValue(ExpressionValue& rslt, const VariableSet *vars, const char *name, bool parameter, bool wantExists) THROWS(GCodeException); void ConvertToFloat(ExpressionValue& val, bool evaluate) const THROWS(GCodeException); void ConvertToBool(ExpressionValue& val, bool evaluate) const THROWS(GCodeException); void ConvertToString(ExpressionValue& val, bool evaluate) noexcept; + void ConvertToDriverId(ExpressionValue& val, bool evaluate) const THROWS(GCodeException); void CheckStack(uint32_t calledFunctionStackUsage) const THROWS(GCodeException); diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp index 37e616c2..bc607448 100644 --- a/src/GCodes/GCodeBuffer/GCodeBuffer.cpp +++ b/src/GCodes/GCodeBuffer/GCodeBuffer.cpp @@ -19,6 +19,7 @@ #include <GCodes/GCodeException.h> #include <Platform/RepRap.h> #include <Platform/Platform.h> +#include <Movement/StepTimer.h> // Macros to reduce the amount of explicit conditional compilation in this file #if HAS_LINUX_INTERFACE @@ -49,17 +50,17 @@ constexpr ObjectModelTableEntry GCodeBuffer::objectModelTable[] = { // Within each group, these entries must be in alphabetical order // 0. inputs[] root - { "axesRelative", OBJECT_MODEL_FUNC((bool)self->machineState->axesRelative), ObjectModelEntryFlags::none }, - { "compatibility", OBJECT_MODEL_FUNC(self->machineState->compatibility.ToString()), ObjectModelEntryFlags::none }, - { "distanceUnit", OBJECT_MODEL_FUNC((self->machineState->usingInches) ? "inch" : "mm"), ObjectModelEntryFlags::none }, - { "drivesRelative", OBJECT_MODEL_FUNC((bool)self->machineState->drivesRelative), ObjectModelEntryFlags::none }, - { "feedRate", OBJECT_MODEL_FUNC(self->machineState->feedRate, 1), ObjectModelEntryFlags::live }, - { "inMacro", OBJECT_MODEL_FUNC((bool)self->machineState->doingFileMacro), ObjectModelEntryFlags::none }, - { "lineNumber", OBJECT_MODEL_FUNC((int32_t)self->GetLineNumber()), ObjectModelEntryFlags::live }, - { "name", OBJECT_MODEL_FUNC(self->codeChannel.ToString()), ObjectModelEntryFlags::none }, - { "stackDepth", OBJECT_MODEL_FUNC((int32_t)self->GetStackDepth()), ObjectModelEntryFlags::none }, - { "state", OBJECT_MODEL_FUNC(self->GetStateText()), ObjectModelEntryFlags::live }, - { "volumetric", OBJECT_MODEL_FUNC((bool)self->machineState->volumetricExtrusion), ObjectModelEntryFlags::none }, + { "axesRelative", OBJECT_MODEL_FUNC((bool)self->machineState->axesRelative), ObjectModelEntryFlags::none }, + { "compatibility", OBJECT_MODEL_FUNC(self->machineState->compatibility.ToString()), ObjectModelEntryFlags::none }, + { "distanceUnit", OBJECT_MODEL_FUNC(self->GetDistanceUnits()), ObjectModelEntryFlags::none }, + { "drivesRelative", OBJECT_MODEL_FUNC((bool)self->machineState->drivesRelative), ObjectModelEntryFlags::none }, + { "feedRate", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->machineState->feedRate), 1), ObjectModelEntryFlags::live }, + { "inMacro", OBJECT_MODEL_FUNC((bool)self->machineState->doingFileMacro), ObjectModelEntryFlags::none }, + { "lineNumber", OBJECT_MODEL_FUNC((int32_t)self->GetLineNumber()), ObjectModelEntryFlags::live }, + { "name", OBJECT_MODEL_FUNC(self->codeChannel.ToString()), ObjectModelEntryFlags::none }, + { "stackDepth", OBJECT_MODEL_FUNC((int32_t)self->GetStackDepth()), ObjectModelEntryFlags::none }, + { "state", OBJECT_MODEL_FUNC(self->GetStateText()), ObjectModelEntryFlags::live }, + { "volumetric", OBJECT_MODEL_FUNC((bool)self->machineState->volumetricExtrusion), ObjectModelEntryFlags::none }, }; constexpr uint8_t GCodeBuffer::objectModelTableDescriptor[] = { 1, 11 }; @@ -344,6 +345,12 @@ bool GCodeBuffer::Seen(char c) noexcept return PARSER_OPERATION(Seen(c)); } +// Return true if any of the parameter letters in the bitmap were seen +bool GCodeBuffer::SeenAny(Bitmap<uint32_t> bm) const noexcept +{ + return PARSER_OPERATION(SeenAny(bm)); +} + // Test for character present, throw error if not void GCodeBuffer::MustSee(char c) THROWS(GCodeException) { @@ -388,6 +395,24 @@ float GCodeBuffer::GetDistance() THROWS(GCodeException) return ConvertDistance(GetFValue()); } +// Get a speed in mm/min or inches/min and convert it to mm/step_clock +float GCodeBuffer::GetSpeed() THROWS(GCodeException) +{ + return ConvertSpeed(GetFValue()); +} + +// Get a speed in mm/min mm/sec and convert it to mm/step_clock +float GCodeBuffer::GetSpeedFromMm(bool useSeconds) THROWS(GCodeException) +{ + return ConvertSpeedFromMm(GetFValue(), useSeconds); +} + +// Get an acceleration in mm/sec^2 and convert it to mm/step_clock^2 +float GCodeBuffer::GetAcceleration() THROWS(GCodeException) +{ + return ConvertAcceleration(GetFValue()); +} + // Get an integer after a key letter int32_t GCodeBuffer::GetIValue() THROWS(GCodeException) { @@ -482,19 +507,46 @@ void GCodeBuffer::GetReducedString(const StringRef& str) THROWS(GCodeException) // Get a colon-separated list of floats after a key letter void GCodeBuffer::GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) { - PARSER_OPERATION(GetFloatArray(arr, length, doPad)); + const size_t maxLength = length; + PARSER_OPERATION(GetFloatArray(arr, length)); + // If there is one entry and doPad is true, fill the rest of the array with the first entry. + if (doPad && length == 1) + { + while (length < maxLength) + { + arr[length++] = arr[0]; + } + } } // Get a :-separated list of ints after a key letter void GCodeBuffer::GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException) { - PARSER_OPERATION(GetIntArray(arr, length, doPad)); + const size_t maxLength = length; + PARSER_OPERATION(GetIntArray(arr, length)); + // If there is one entry and doPad is true, fill the rest of the array with the first entry. + if (doPad && length == 1) + { + while (length < maxLength) + { + arr[length++] = arr[0]; + } + } } // Get a :-separated list of unsigned ints after a key letter void GCodeBuffer::GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException) { - PARSER_OPERATION(GetUnsignedArray(arr, length, doPad)); + const size_t maxLength = length; + PARSER_OPERATION(GetUnsignedArray(arr, length)); + // If there is one entry and doPad is true, fill the rest of the array with the first entry. + if (doPad && length == 1) + { + while (length < maxLength) + { + arr[length++] = arr[0]; + } + } } // Get a :-separated list of drivers after a key letter @@ -727,13 +779,30 @@ GCodeMachineState& GCodeBuffer::CurrentFileMachineState() const noexcept // Convert from inches to mm if necessary float GCodeBuffer::ConvertDistance(float distance) const noexcept { - return (machineState->usingInches) ? distance * InchToMm : distance; + return (UsingInches()) ? distance * InchToMm : distance; } // Convert from mm to inches if necessary float GCodeBuffer::InverseConvertDistance(float distance) const noexcept { - return (machineState->usingInches) ? distance/InchToMm : distance; + return (UsingInches()) ? distance/InchToMm : distance; +} + +// Convert speed from mm/min or inches/min to mm per step clock +float GCodeBuffer::ConvertSpeed(float speed) const noexcept +{ + return speed * ((UsingInches()) ? InchToMm/(StepClockRate * iMinutesToSeconds) : 1.0/(StepClockRate * iMinutesToSeconds)); +} + +// Convert speed to mm/min or inches/min +float GCodeBuffer::InverseConvertSpeed(float speed) const noexcept +{ + return speed * ((UsingInches()) ? (StepClockRate * iMinutesToSeconds)/InchToMm : (float)(StepClockRate * iMinutesToSeconds)); +} + +const char *GCodeBuffer::GetDistanceUnits() const noexcept +{ + return (UsingInches()) ? "in" : "mm"; } // Return the current stack depth diff --git a/src/GCodes/GCodeBuffer/GCodeBuffer.h b/src/GCodes/GCodeBuffer/GCodeBuffer.h index fb1f7a3f..b6cec720 100644 --- a/src/GCodes/GCodeBuffer/GCodeBuffer.h +++ b/src/GCodes/GCodeBuffer/GCodeBuffer.h @@ -72,9 +72,13 @@ public: bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? void MustSee(char c) THROWS(GCodeException); // Test for character present, throw error if not char MustSee(char c1, char c2) THROWS(GCodeException); // Test for one of two characters present, throw error if not + inline bool SeenAny(const char *s) const noexcept { return SeenAny(Bitmap<uint32_t>(ParametersToBitmap(s))); } float GetFValue() THROWS(GCodeException) SPEED_CRITICAL; // Get a float after a key letter float GetDistance() THROWS(GCodeException); // Get a distance or coordinate and convert it from inches to mm if necessary + float GetSpeed() THROWS(GCodeException); // Get a speed in mm/min or inches/min and convert it to mm/step_clock + float GetSpeedFromMm(bool useSeconds) THROWS(GCodeException); // Get a speed in mm/min or optionally /sec and convert it to mm/step_clock + float GetAcceleration() THROWS(GCodeException); // Get an acceleration in mm/sec^2 or inches/sec^2 and convert it to mm/step_clock^2 int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL; // Get an integer after a key letter int32_t GetLimitedIValue(char c, int32_t minValue, int32_t maxValue) THROWS(GCodeException) pre(minvalue <= maxValue) @@ -127,8 +131,13 @@ public: GCodeMachineState::BlockState& GetBlockState() const noexcept { return CurrentFileMachineState().CurrentBlockState(); } uint16_t GetBlockIndent() const noexcept { return GetBlockState().GetIndent(); } + void UseInches(bool inchesNotMm) noexcept { machineState->usingInches = inchesNotMm; } + bool UsingInches() const noexcept { return machineState->usingInches; } float ConvertDistance(float distance) const noexcept; float InverseConvertDistance(float distance) const noexcept; + float ConvertSpeed(float speed) const noexcept; + float InverseConvertSpeed(float speed) const noexcept; + const char *GetDistanceUnits() const noexcept; unsigned int GetStackDepth() const noexcept; bool PushState(bool withinSameFile) noexcept; // Push state returning true if successful (i.e. stack not overflowed) bool PopState(bool withinSameFile) noexcept; // Pop state returning true if successful (i.e. no stack underrun) @@ -235,6 +244,15 @@ protected: DECLARE_OBJECT_MODEL private: + bool SeenAny(Bitmap<uint32_t> bm) const noexcept; // Return true if any of the parameter letters in the bitmap were seen + + // Convert a string of uppercase parameter letters to a bit map + static inline constexpr uint32_t ParametersToBitmap(const char *s) noexcept + { + return (*s == 0) ? 0 + : (*s >= 'A' && *s <= 'Z') ? ((uint32_t)1 << (*s - 'A')) | ParametersToBitmap(s + 1) + : ParametersToBitmap(s + 1); + } #if SUPPORT_OBJECT_MODEL const char *GetStateText() const noexcept; diff --git a/src/GCodes/GCodeBuffer/StringParser.cpp b/src/GCodes/GCodeBuffer/StringParser.cpp index 9247c577..4876f3d6 100644 --- a/src/GCodes/GCodeBuffer/StringParser.cpp +++ b/src/GCodes/GCodeBuffer/StringParser.cpp @@ -1059,6 +1059,12 @@ bool StringParser::Seen(char c) noexcept return false; } +// Return true if any of the parameter letters in the bitmap were seen +bool StringParser::SeenAny(Bitmap<uint32_t> bm) const noexcept +{ + return parametersPresent.Intersects(bm); +} + // Get a float after a G Code letter found by a call to Seen() float StringParser::GetFValue() THROWS(GCodeException) { @@ -1074,35 +1080,32 @@ float StringParser::GetFValue() THROWS(GCodeException) // Get a colon-separated list of floats after a key letter // If doPad is true then we allow just one element to be given, in which case we fill all elements with that value -void StringParser::GetFloatArray(float arr[], size_t& returnedLength, bool doPad) THROWS(GCodeException) +void StringParser::GetFloatArray(float arr[], size_t& returnedLength) THROWS(GCodeException) { if (readPointer <= 0) { THROW_INTERNAL_ERROR; } - size_t length = 0; - for (;;) + if (gb.buffer[readPointer] == '{') { - CheckArrayLength(length, returnedLength); - arr[length++] = ReadFloatValue(); - if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR) - { - break; - } - ++readPointer; + ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer); + parser.ParseFloatArray(arr, returnedLength); } - - // Special case if there is one entry and returnedLength requests several. Fill the array with the first entry. - if (doPad && length == 1 && returnedLength > 1) + else { - for (size_t i = 1; i < returnedLength; i++) + size_t length = 0; + for (;;) { - arr[i] = arr[0]; + CheckArrayLength(length, returnedLength); + arr[length++] = ReadFloatValue(); + if (gb.buffer[readPointer] != LIST_SEPARATOR) + { + break; + } + ++readPointer; } - } - else - { + returnedLength = length; } @@ -1110,72 +1113,67 @@ void StringParser::GetFloatArray(float arr[], size_t& returnedLength, bool doPad } // Get a :-separated list of ints after a key letter -void StringParser::GetIntArray(int32_t arr[], size_t& returnedLength, bool doPad) THROWS(GCodeException) +void StringParser::GetIntArray(int32_t arr[], size_t& returnedLength) THROWS(GCodeException) { if (readPointer <= 0) { THROW_INTERNAL_ERROR; } - size_t length = 0; - for (;;) + if (gb.buffer[readPointer] == '{') { - CheckArrayLength(length, returnedLength); - arr[length] = ReadIValue(); - length++; - if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR) - { - break; - } - ++readPointer; + ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer); + parser.ParseIntArray(arr, returnedLength); } - - // Special case if there is one entry and returnedLength requests several. Fill the array with the first entry. - if (doPad && length == 1 && returnedLength > 1) + else { - for (size_t i = 1; i < returnedLength; i++) + size_t length = 0; + for (;;) { - arr[i] = arr[0]; + CheckArrayLength(length, returnedLength); + arr[length] = ReadIValue(); + length++; + if (gb.buffer[readPointer] != LIST_SEPARATOR) + { + break; + } + ++readPointer; } - } - else - { + returnedLength = length; } + readPointer = -1; } // Get a :-separated list of unsigned ints after a key letter -void StringParser::GetUnsignedArray(uint32_t arr[], size_t& returnedLength, bool doPad) THROWS(GCodeException) +void StringParser::GetUnsignedArray(uint32_t arr[], size_t& returnedLength) THROWS(GCodeException) { if (readPointer <= 0) { THROW_INTERNAL_ERROR; } - size_t length = 0; - for (;;) + if (gb.buffer[readPointer] == '{') { - CheckArrayLength(length, returnedLength); - arr[length] = ReadUIValue(); - length++; - if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR) - { - break; - } - ++readPointer; + ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer); + parser.ParseUnsignedArray(arr, returnedLength); } - - // Special case if there is one entry and returnedLength requests several. Fill the array with the first entry. - if (doPad && length == 1 && returnedLength > 1) + else { - for (size_t i = 1; i < returnedLength; i++) + size_t length = 0; + for (;;) { - arr[i] = arr[0]; + CheckArrayLength(length, returnedLength); + arr[length] = ReadUIValue(); + length++; + if (gb.buffer[readPointer] != LIST_SEPARATOR) + { + break; + } + ++readPointer; } - } - else - { + returnedLength = length; } @@ -1190,20 +1188,29 @@ void StringParser::GetDriverIdArray(DriverId arr[], size_t& returnedLength) THRO THROW_INTERNAL_ERROR; } - size_t length = 0; - for (;;) + if (gb.buffer[readPointer] == '{') + { + ExpressionParser parser(gb, gb.buffer + readPointer, gb.buffer + ARRAY_SIZE(gb.buffer), commandIndent + readPointer); + parser.ParseDriverIdArray(arr, returnedLength); + } + else { - CheckArrayLength(length, returnedLength); - arr[length] = ReadDriverIdValue(); - length++; - if (gb.buffer[readPointer] != EXPRESSION_LIST_SEPARATOR && gb.buffer[readPointer] != LIST_SEPARATOR) + size_t length = 0; + for (;;) { - break; + CheckArrayLength(length, returnedLength); + arr[length] = ReadDriverIdValue(); + length++; + if (gb.buffer[readPointer] != LIST_SEPARATOR) + { + break; + } + ++readPointer; } - ++readPointer; + + returnedLength = length; } - returnedLength = length; readPointer = -1; } diff --git a/src/GCodes/GCodeBuffer/StringParser.h b/src/GCodes/GCodeBuffer/StringParser.h index 3e7c515b..b56aa9f9 100644 --- a/src/GCodes/GCodeBuffer/StringParser.h +++ b/src/GCodes/GCodeBuffer/StringParser.h @@ -43,22 +43,23 @@ public: bool IsLastCommand() const noexcept; bool ContainsExpression() const noexcept { return seenExpression; } - bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? - float GetFValue() THROWS(GCodeException) SPEED_CRITICAL; // Get a float after a key letter - float GetDistance() THROWS(GCodeException) SPEED_CRITICAL; // Get a distance or coordinate and convert it from inches to mm if necessary - int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL; // Get an integer after a key letter + bool Seen(char c) noexcept SPEED_CRITICAL; // Is a character present? + bool SeenAny(Bitmap<uint32_t> bm) const noexcept; // Return true if any of the parameter letters in the bitmap were seen + float GetFValue() THROWS(GCodeException) SPEED_CRITICAL; // Get a float after a key letter + float GetDistance() THROWS(GCodeException) SPEED_CRITICAL; // Get a distance or coordinate and convert it from inches to mm if necessary + int32_t GetIValue() THROWS(GCodeException) SPEED_CRITICAL; // Get an integer after a key letter uint32_t GetUIValue() THROWS(GCodeException); // Get an unsigned integer value DriverId GetDriverId() THROWS(GCodeException); // Get a driver ID void GetIPAddress(IPAddress& returnedIp) THROWS(GCodeException); // Get an IP address quad after a key letter void GetMacAddress(MacAddress& mac) THROWS(GCodeException); // Get a MAC address sextet after a key letter void GetUnprecedentedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException); // Get a string with no preceding key letter - void GetCompleteParameters(const StringRef& str) const noexcept; // Get the complete parameter string - void GetQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException); // Get and copy a quoted string + void GetCompleteParameters(const StringRef& str) const noexcept; // Get the complete parameter string + void GetQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException); // Get and copy a quoted string void GetPossiblyQuotedString(const StringRef& str, bool allowEmpty) THROWS(GCodeException); // Get and copy a string which may or may not be quoted - void GetFloatArray(float arr[], size_t& length, bool doPad) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter - void GetIntArray(int32_t arr[], size_t& length, bool doPad) THROWS(GCodeException); // Get a :-separated list of ints after a key letter - void GetUnsignedArray(uint32_t arr[], size_t& length, bool doPad) THROWS(GCodeException); // Get a :-separated list of unsigned ints after a key letter - void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of drivers after a key letter + void GetFloatArray(float arr[], size_t& length) THROWS(GCodeException) SPEED_CRITICAL; // Get a colon-separated list of floats after a key letter + void GetIntArray(int32_t arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of ints after a key letter + void GetUnsignedArray(uint32_t arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of unsigned ints after a key letter + void GetDriverIdArray(DriverId arr[], size_t& length) THROWS(GCodeException); // Get a :-separated list of drivers after a key letter void SetFinished() noexcept; // Set the G Code finished void SetCommsProperties(uint32_t arg) noexcept { checksumRequired = (arg & 1); } diff --git a/src/GCodes/GCodeMachineState.cpp b/src/GCodes/GCodeMachineState.cpp index 1af502d1..1e6e6dda 100644 --- a/src/GCodes/GCodeMachineState.cpp +++ b/src/GCodes/GCodeMachineState.cpp @@ -12,7 +12,7 @@ // Create a default initialised GCodeMachineState GCodeMachineState::GCodeMachineState() noexcept - : feedRate(DefaultFeedRate * SecondsToMinutes), + : feedRate(ConvertSpeedFromMmPerMin(DefaultFeedRate)), #if HAS_LINUX_INTERFACE fileId(NoFileId), #endif diff --git a/src/GCodes/GCodeQueue.cpp b/src/GCodes/GCodeQueue.cpp index 9412e735..a87a1f9b 100644 --- a/src/GCodes/GCodeQueue.cpp +++ b/src/GCodes/GCodeQueue.cpp @@ -220,14 +220,15 @@ void GCodeQueue::Clear() noexcept void GCodeQueue::Diagnostics(MessageType mtype) noexcept { - reprap.GetPlatform().MessageF(mtype, "Code queue is %s\n", (queuedItems == nullptr) ? "empty." : "not empty:"); - if (queuedItems != nullptr) + if (queuedItems == nullptr) + { + reprap.GetPlatform().Message(mtype, "Code queue is empty\n"); + } + else { const QueuedCode *item = queuedItems; - size_t queueLength = 0; do { - queueLength++; #if HAS_LINUX_INTERFACE // The following may output binary gibberish if this code is stored in binary. // We could restore this message by using GCodeBuffer::AppendFullCommand but there is probably no need to @@ -237,7 +238,6 @@ void GCodeQueue::Diagnostics(MessageType mtype) noexcept reprap.GetPlatform().MessageF(mtype, "Queued '%.*s' for move %" PRIu32 "\n", item->dataLength, item->data, item->executeAtMove); } } while ((item = item->Next()) != nullptr); - reprap.GetPlatform().MessageF(mtype, "%d of %d codes have been queued.\n", queueLength, maxQueuedCodes); } } diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp index 9c716f3b..85e88c33 100644 --- a/src/GCodes/GCodes.cpp +++ b/src/GCodes/GCodes.cpp @@ -451,6 +451,7 @@ void GCodes::Spin() noexcept ) // if autoPause is active { (void)SpinGCodeBuffer(*autoPauseGCode); + (void)SpinGCodeBuffer(*queuedGCode); // autopause sometimes to wait for queued GCodes to complete, so spin queuedGCodes too to avoid lockup } else { @@ -646,9 +647,7 @@ bool GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept // We never get here if the file ends in M0 because CancelPrint gets called directly in that case. // Don't close the file until all moves have been completed, in case the print gets paused. // Also, this keeps the state as 'Printing' until the print really has finished. - if ( LockMovementAndWaitForStandstill(gb) // wait until movement has finished - && IsCodeQueueIdle() // must also wait until deferred command queue has caught up - ) + if (LockMovementAndWaitForStandstill(gb)) // wait until movement has finished and deferred command queue has caught up { StopPrint(StopPrintReason::normalCompletion); } @@ -784,9 +783,7 @@ bool GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept // We never get here if the file ends in M0 because CancelPrint gets called directly in that case. // Don't close the file until all moves have been completed, in case the print gets paused. // Also, this keeps the state as 'Printing' until the print really has finished. - if ( LockMovementAndWaitForStandstill(gb) // wait until movement has finished - && IsCodeQueueIdle() // must also wait until deferred command queue has caught up - ) + if (LockMovementAndWaitForStandstill(gb)) // wait until movement has finished and deferred command queue has caught up { StopPrint(StopPrintReason::normalCompletion); } @@ -1495,7 +1492,7 @@ void GCodes::SaveResumeInfo(bool wasPowerFailure) noexcept buf.catf("\nG0 F6000 Z%.3f\n", (double)pauseRestorePoint.moveCoords[Z_AXIS]); // Set the feed rate - buf.catf("G1 F%.1f", (double)(pauseRestorePoint.feedRate * MinutesToSeconds)); + buf.catf("G1 F%.1f", (double)InverseConvertSpeedToMmPerMin(pauseRestorePoint.feedRate)); #if SUPPORT_LASER if (machineType == MachineType::laser) { @@ -1577,7 +1574,12 @@ bool GCodes::LockMovementAndWaitForStandstill(GCodeBuffer& gb) noexcept return false; } - gb.MotionStopped(); // must do this after we have finished waiting, so that we don't stop waiting when executing G4 + if (&gb != queuedGCode && !IsCodeQueueIdle()) // wait for deferred command queue to catch up + { + return false; + } + + gb.MotionStopped(); // must do this after we have finished waiting, so that we don't stop waiting when executing G4 if (RTOSIface::GetCurrentTask() == Tasks::GetMainTask()) { @@ -1628,7 +1630,7 @@ const char * GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, bool isP moveBuffer.applyM220M221 = (moveBuffer.moveType == 0 && isPrintingMove && !gb.IsDoingFileMacro()); if (gb.Seen(feedrateLetter)) { - gb.LatestMachineState().feedRate = gb.GetDistance() * SecondsToMinutes; // update requested speed, not allowing for speed factor + gb.LatestMachineState().feedRate = gb.GetSpeed(); // update requested speed, not allowing for speed factor } moveBuffer.feedRate = (moveBuffer.applyM220M221) ? speedFactor * gb.LatestMachineState().feedRate @@ -1638,7 +1640,7 @@ const char * GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, bool isP else { moveBuffer.applyM220M221 = false; - moveBuffer.feedRate = DefaultG0FeedRate; // use maximum feed rate, the M203 parameters will limit it + moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultG0FeedRate); // use maximum feed rate, the M203 parameters will limit it moveBuffer.usingStandardFeedrate = false; } @@ -1648,11 +1650,11 @@ const char * GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, bool isP moveBuffer.coords[drive] = 0.0; } moveBuffer.hasPositiveExtrusion = false; - moveBuffer.virtualExtruderPosition = virtualExtruderPosition; // save this before we update it + moveBuffer.virtualExtruderPosition = virtualExtruderPosition; // save this before we update it ExtrudersBitmap extrudersMoving; // Check if we are extruding - if (gb.Seen(extrudeLetter)) // DC 2018-08-07: at E3D's request, extrusion is now recognised even on uncoordinated moves + if (gb.Seen(extrudeLetter)) // DC 2018-08-07: at E3D's request, extrusion is now recognised even on uncoordinated moves { // Check that we have a tool to extrude with Tool* const tool = reprap.GetCurrentTool(); @@ -2116,7 +2118,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e moveLengthSquared += fsquare(currentUserPosition[Z_AXIS] - initialUserPosition[Z_AXIS]); } const float moveLength = fastSqrtf(moveLengthSquared); - const float moveTime = moveLength/moveBuffer.feedRate; // this is a best-case time, often the move will take longer + const float moveTime = moveLength/(moveBuffer.feedRate * StepClockRate); // this is a best-case time, often the move will take longer moveBuffer.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(moveLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond()))); } else @@ -2466,7 +2468,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err) // We leave out the square term because it is very small // In CNC applications even very small deviations can be visible, so we use a smaller segment length at low speeds const float arcSegmentLength = constrain<float> - ( min<float>(fastSqrtf(8 * moveBuffer.arcRadius * MaxArcDeviation), moveBuffer.feedRate * (1.0/MinArcSegmentsPerSec)), + ( min<float>(fastSqrtf(8 * moveBuffer.arcRadius * MaxArcDeviation), moveBuffer.feedRate * StepClockRate * (1.0/MinArcSegmentsPerSec)), MinArcSegmentLength, MaxArcSegmentLength ); @@ -2814,10 +2816,32 @@ bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissi #if HAS_LINUX_INTERFACE || HAS_MASS_STORAGE gb.LatestMachineState().doingFileMacro = true; - gb.LatestMachineState().runningM501 = (codeRunning == 501); - gb.LatestMachineState().runningM502 = (codeRunning == 502); - gb.LatestMachineState().runningSystemMacro = (codeRunning == SystemHelperMacroCode || codeRunning == AsyncSystemMacroCode || codeRunning == 29 || codeRunning == 32); - // running a system macro e.g. homing, so don't use workplace coordinates + + // The following three flags need to be inherited in the case that a system macro calls another macro, e.g.homeall.g calls homez.g. The Push call copied them over already. + switch (codeRunning) + { + case 501: + gb.LatestMachineState().runningM501 = true; + gb.LatestMachineState().runningSystemMacro = true; // running a system macro e.g. homing, so don't use workplace coordinates + break; + + case 502: + gb.LatestMachineState().runningM502 = true; + gb.LatestMachineState().runningSystemMacro = true; // running a system macro e.g. homing, so don't use workplace coordinates + break; + + case SystemHelperMacroCode: + case AsyncSystemMacroCode: + case ToolChangeMacroCode: + case 29: + case 32: + gb.LatestMachineState().runningSystemMacro = true; // running a system macro e.g. homing, so don't use workplace coordinates + break; + + default: + break; + } + gb.SetState(GCodeState::normal); gb.Init(); diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp index d5dc4af4..ee50c201 100644 --- a/src/GCodes/GCodes2.cpp +++ b/src/GCodes/GCodes2.cpp @@ -267,12 +267,12 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx break; case 20: // Inches (which century are we living in, here?) - gb.LatestMachineState().usingInches = true; + gb.UseInches(true); reprap.InputsUpdated(); break; case 21: // mm - gb.LatestMachineState().usingInches = false; + gb.UseInches(false); reprap.InputsUpdated(); break; @@ -515,9 +515,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx reply.copy("Pause the print before attempting to cancel it"); result = GCodeResult::error; } - else if ( !LockMovementAndWaitForStandstill(gb) // wait until everything has stopped - || !IsCodeQueueIdle() // must also wait until deferred command queue has caught up - ) + else if (!LockMovementAndWaitForStandstill(gb)) // wait until everything has stopped and deferred command queue has caught up { return false; } @@ -1464,9 +1462,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx case 109: // Deprecated in RRF, but widely generated by slicers { const bool movementWasLocked = gb.LatestMachineState().lockedResources.IsBitSet(MoveResource); - if ( !LockMovementAndWaitForStandstill(gb) // wait until movement has finished - || !IsCodeQueueIdle() // also wait until deferred command queue has caught up to avoid out-of-order execution - ) + if (!LockMovementAndWaitForStandstill(gb)) // wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution { return false; } @@ -1677,9 +1673,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx break; case 116: // Wait for set temperatures - if ( !LockMovementAndWaitForStandstill(gb) // wait until movement has finished - || !IsCodeQueueIdle() // also wait until deferred command queue has caught up to avoid out-of-order execution - ) + if (!LockMovementAndWaitForStandstill(gb)) // wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution { return false; } @@ -2021,9 +2015,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx case 190: // Set bed temperature and wait case 191: // Set chamber temperature and wait - if ( !LockMovementAndWaitForStandstill(gb) // wait until movement has finished - || !IsCodeQueueIdle() // also wait until deferred command queue has caught up to avoid out-of-order execution - ) + if (!LockMovementAndWaitForStandstill(gb)) // wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution { return false; } @@ -2109,7 +2101,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx { if (gb.Seen(axisLetters[axis])) { - platform.SetAcceleration(axis, gb.GetDistance()); + platform.SetAcceleration(axis, gb.GetAcceleration()); seen = true; } } @@ -2122,7 +2114,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx gb.GetFloatArray(eVals, eCount, true); for (size_t e = 0; e < eCount; e++) { - platform.SetAcceleration(ExtruderToLogicalDrive(e), gb.ConvertDistance(eVals[e])); + platform.SetAcceleration(ExtruderToLogicalDrive(e), ConvertAcceleration(eVals[e])); } } @@ -2132,16 +2124,16 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx } else { - reply.printf("Accelerations (mm/sec^2): "); + reply.copy("Accelerations (mm/sec^2): "); for (size_t axis = 0; axis < numTotalAxes; ++axis) { - reply.catf("%c: %.1f, ", axisLetters[axis], (double)platform.Acceleration(axis)); + reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertAcceleration(platform.Acceleration(axis))); } reply.cat("E:"); char sep = ' '; for (size_t extruder = 0; extruder < numExtruders; extruder++) { - reply.catf("%c%.1f", sep, (double)platform.Acceleration(ExtruderToLogicalDrive(extruder))); + reply.catf("%c%.1f", sep, (double)InverseConvertAcceleration(platform.Acceleration(ExtruderToLogicalDrive(extruder)))); sep = ':'; } } @@ -2152,14 +2144,13 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx { // Units are mm/sec if S1 is given, else mm/min const bool usingMmPerSec = (gb.Seen('S') && gb.GetIValue() == 1); - const float settingMultiplier = (usingMmPerSec) ? 1.0 : SecondsToMinutes; bool seen = false; // Do the minimum first, because we constrain the maximum rates to be no lower than it if (gb.Seen('I')) { seen = true; - platform.SetMinMovementSpeed(gb.GetDistance() * settingMultiplier); + platform.SetMinMovementSpeed(gb.GetSpeedFromMm(usingMmPerSec)); } for (size_t axis = 0; axis < numTotalAxes; ++axis) @@ -2167,7 +2158,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx if (gb.Seen(axisLetters[axis])) { seen = true; - platform.SetMaxFeedrate(axis, gb.GetDistance() * settingMultiplier); + platform.SetMaxFeedrate(axis, gb.GetSpeedFromMm(usingMmPerSec)); } } @@ -2179,7 +2170,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx gb.GetFloatArray(eVals, eCount, true); for (size_t e = 0; e < eCount; e++) { - platform.SetMaxFeedrate(ExtruderToLogicalDrive(e), gb.ConvertDistance(eVals[e]) * settingMultiplier); + platform.SetMaxFeedrate(ExtruderToLogicalDrive(e), ConvertSpeedFromMm(eVals[e], usingMmPerSec)); } } @@ -2189,20 +2180,19 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx } else { - const float reportingMultiplier = (usingMmPerSec) ? 1.0 : MinutesToSeconds; - reply.printf("Max speeds (mm/%s): ", (usingMmPerSec) ? "sec" : "min"); + reply.printf("Max speeds (%s)): ", (usingMmPerSec) ? "mm/sec" : "mm/min"); for (size_t axis = 0; axis < numTotalAxes; ++axis) { - reply.catf("%c: %.1f, ", axisLetters[axis], (double)(platform.MaxFeedrate(axis) * reportingMultiplier)); + reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertSpeedToMm(platform.MaxFeedrate(axis), usingMmPerSec)); } reply.cat("E:"); char sep = ' '; for (size_t extruder = 0; extruder < numExtruders; extruder++) { - reply.catf("%c%.1f", sep, (double)(platform.MaxFeedrate(ExtruderToLogicalDrive(extruder)) * reportingMultiplier)); + reply.catf("%c%.1f", sep, (double)InverseConvertSpeedToMm(platform.MaxFeedrate(ExtruderToLogicalDrive(extruder)), usingMmPerSec)); sep = ':'; } - reply.catf(", min. speed %.2f", (double)(platform.MinMovementSpeed() * reportingMultiplier)); + reply.catf(", min. speed %.2f", (double)InverseConvertSpeedToMm(platform.MinMovementSpeed(), usingMmPerSec)); } } break; @@ -2287,7 +2277,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx if (!seen) { - reply.copy("Axis limit"); + reply.copy("Axis limits (mm)"); char sep = 's'; for (size_t axis = 0; axis < numTotalAxes; axis++) { @@ -2456,7 +2446,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx { // The pipeline is empty, so execute the babystepping move immediately SetMoveBufferDefaults(); - moveBuffer.feedRate = DefaultFeedRate; + moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate); moveBuffer.tool = reprap.GetCurrentTool(); NewMoveAvailable(1); } @@ -3320,13 +3310,13 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx case 205: // Set/print maximum jerk speeds in mm/sec case 566: // Set/print maximum jerk speeds in mm/min { - const float multiplier1 = (code == 566) ? SecondsToMinutes : 1.0; + const bool useMmPerSec = (code == 205); bool seenAxis = false, seenExtruder = false; for (size_t axis = 0; axis < numTotalAxes; axis++) { if (gb.Seen(axisLetters[axis])) { - platform.SetInstantDv(axis, gb.GetDistance() * multiplier1); + platform.SetInstantDv(axis, gb.GetSpeedFromMm(useMmPerSec)); seenAxis = true; } } @@ -3339,7 +3329,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx gb.GetFloatArray(eVals, eCount, true); for (size_t e = 0; e < eCount; e++) { - platform.SetInstantDv(ExtruderToLogicalDrive(e), eVals[e] * multiplier1); + platform.SetInstantDv(ExtruderToLogicalDrive(e), ConvertSpeedFromMm(eVals[e], useMmPerSec)); } } @@ -3355,17 +3345,16 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx } else if (!seenExtruder) { - const float multiplier2 = (code == 566) ? MinutesToSeconds : 1.0; - reply.printf("Maximum jerk rates (mm/%s): ", (code == 566) ? "min" : "sec"); + reply.printf("Maximum jerk rates (%s): ", (useMmPerSec) ? "mm/sec" : "mm/min"); for (size_t axis = 0; axis < numTotalAxes; ++axis) { - reply.catf("%c: %.1f, ", axisLetters[axis], (double)(platform.GetInstantDv(axis) * multiplier2)); + reply.catf("%c: %.1f, ", axisLetters[axis], (double)InverseConvertSpeedToMm(platform.GetInstantDv(axis), useMmPerSec)); } reply.cat("E:"); char sep = ' '; for (size_t extruder = 0; extruder < numExtruders; extruder++) { - reply.catf("%c%.1f", sep, (double)(platform.GetInstantDv(ExtruderToLogicalDrive(extruder)) * multiplier2)); + reply.catf("%c%.1f", sep, (double)InverseConvertSpeedToMm(platform.GetInstantDv(ExtruderToLogicalDrive(extruder)), useMmPerSec)); sep = ':'; } if (code == 566) @@ -3459,25 +3448,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx break; case 572: // Set/report pressure advance - if (gb.Seen('S')) - { - const float advance = gb.GetFValue(); - if (!LockMovementAndWaitForStandstill(gb)) - { - return false; - } - result = platform.SetPressureAdvance(advance, gb, reply); - } - else - { - reply.copy("Extruder pressure advance"); - char c = ':'; - for (size_t i = 0; i < numExtruders; ++i) - { - reply.catf("%c %.3f", c, (double)platform.GetPressureAdvance(i)); - c = ','; - } - } + result = reprap.GetMove().ConfigurePressureAdvance(gb, reply); break; case 573: // Report heater average PWM @@ -3735,11 +3706,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx #endif case 593: // Configure dynamic ringing cancellation - if (!LockMovementAndWaitForStandstill(gb)) - { - return false; - } - result = reprap.GetMove().GetShaper().Configure(gb, reply); + result = reprap.GetMove().GetAxisShaper().Configure(gb, reply); break; #if SUPPORT_ASYNC_MOVES @@ -4022,8 +3989,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx // Get the feedrate (if any) and kick off a new move if (gb.Seen(feedrateLetter)) { - const float rate = gb.ConvertDistance(gb.GetFValue()); - gb.LatestMachineState().feedRate = rate * SecondsToMinutes; // don't apply the speed factor + gb.LatestMachineState().feedRate = gb.GetSpeed(); // don't apply the speed factor } moveBuffer.feedRate = gb.LatestMachineState().feedRate; moveBuffer.usingStandardFeedrate = true; @@ -4660,7 +4626,7 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply) } else if (gb.Seen('T')) { - // We handle "T{expression}" as if it's "T "{expression}, also DSF may pass a T{expression} command in this way + // We handle "T T{expression}" as if it's "T "{expression}, also DSF may pass a T{expression} command in this way seen = true; toolNum = gb.GetIValue(); } @@ -4682,7 +4648,7 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply) if (seen) { - if (!LockMovementAndWaitForStandstill(gb) || !IsCodeQueueIdle()) + if (!LockMovementAndWaitForStandstill(gb)) { return false; } diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp index 5bed126e..42388844 100644 --- a/src/GCodes/GCodes3.cpp +++ b/src/GCodes/GCodes3.cpp @@ -909,7 +909,7 @@ GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply) THROWS(GC // Get the feed rate and axis gb.MustSee(feedrateLetter); - m585Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetDistance() * SecondsToMinutes; // don't apply the speed factor to homing and other special moves + m585Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetSpeed(); // don't apply the speed factor to homing and other special moves m585Settings.axisNumber = FindAxisLetter(gb); m585Settings.offset = gb.GetDistance(); @@ -1000,7 +1000,7 @@ GCodeResult GCodes::FindCenterOfCavity(GCodeBuffer& gb, const StringRef& reply) // Get the feed rate, backoff distance, and axis gb.MustSee(feedrateLetter); - m675Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetDistance() * SecondsToMinutes; // don't apply the speed factor to homing and other special moves + m675Settings.feedRate = gb.LatestMachineState().feedRate = gb.GetSpeed(); // don't apply the speed factor to homing and other special moves m675Settings.backoffDistance = gb.Seen('R') ? gb.GetDistance() : 5.0; m675Settings.axisNumber = FindAxisLetter(gb); @@ -1378,22 +1378,22 @@ GCodeResult GCodes::ConfigureLocalDriver(GCodeBuffer& gb, const StringRef& reply { if (drive < platform.GetNumActualDirectDrivers()) { - bool seen = false; - if (gb.Seen('S')) + if (gb.SeenAny("RS")) { if (!LockMovementAndWaitForStandstill(gb)) { return GCodeResult::notFinished; } + } + + bool seen = false; + if (gb.Seen('S')) + { seen = true; platform.SetDirectionValue(drive, gb.GetIValue() != 0); } if (gb.Seen('R')) { - if (!LockMovementAndWaitForStandstill(gb)) - { - return GCodeResult::notFinished; - } seen = true; platform.SetEnableValue(drive, (int8_t)gb.GetIValue()); } diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp index 34a28473..ac0961cc 100644 --- a/src/GCodes/GCodes4.cpp +++ b/src/GCodes/GCodes4.cpp @@ -384,7 +384,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept if (currentTool != nullptr) { const float newZPos = (moveBuffer.coords[Z_AXIS] - currentTool->GetOffset(Z_AXIS)); - if(newZPos > platform.AxisMaximum(Z_AXIS) || newZPos < platform.AxisMinimum(Z_AXIS)) + if (newZPos > platform.AxisMaximum(Z_AXIS) || newZPos < platform.AxisMinimum(Z_AXIS)) { gb.LatestMachineState().feedRate = toolChangeRestorePoint.feedRate; doingToolChange = false; @@ -394,13 +394,13 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept break; } } + // Restore the original Z axis user position, so that different tool Z offsets work even if the first move after the tool change doesn't have a Z coordinate - // Only do this if we are running as an FDM printer, because it's not appropriate for CNC machines. SetMoveBufferDefaults(); currentUserPosition[Z_AXIS] = toolChangeRestorePoint.moveCoords[Z_AXIS]; ToolOffsetTransform(currentUserPosition, moveBuffer.coords); - moveBuffer.feedRate = DefaultFeedRate * SecondsToMinutes; // ask for a good feed rate, we may have paused during a slow move - moveBuffer.tool = reprap.GetCurrentTool(); // needed so that bed compensation is applied correctly + moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate); // ask for a good feed rate, we may have paused during a slow move + moveBuffer.tool = reprap.GetCurrentTool(); // needed so that bed compensation is applied correctly NewMoveAvailable(1); gb.AdvanceState(); } @@ -408,7 +408,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept case GCodeState::toolChangeComplete: case GCodeState::m109ToolChangeComplete: - if (LockMovementAndWaitForStandstill(gb)) // wait for tpost.g to finish executing or the move to height to finish + if (LockMovementAndWaitForStandstill(gb)) // wait for the move to height to finish { gb.LatestMachineState().feedRate = toolChangeRestorePoint.feedRate; // We don't restore the default fan speed in case the user wants to use a different one for the new tool @@ -511,8 +511,8 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept } SetMoveBufferDefaults(); ToolOffsetTransform(currentUserPosition, moveBuffer.coords); - moveBuffer.feedRate = DefaultFeedRate * SecondsToMinutes; // ask for a good feed rate, we may have paused during a slow move - moveBuffer.tool = reprap.GetCurrentTool(); // needed so that bed compensation is applied correctly + moveBuffer.feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate); // ask for a good feed rate, we may have paused during a slow move + moveBuffer.tool = reprap.GetCurrentTool(); // needed so that bed compensation is applied correctly if (gb.GetState() == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS]) { // First move the head to the correct XY point, then move it down in a separate move @@ -640,14 +640,14 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept axes.SetBit(axis0Num); axes.SetBit(axis1Num); float axesCoords[MaxAxes]; - axesCoords[axis0Num] = axis0Coord; - axesCoords[axis1Num] = axis1Coord; + const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber); + axesCoords[axis0Num] = axis0Coord - zp->GetOffset(axis0Num); + axesCoords[axis1Num] = axis1Coord - zp->GetOffset(axis1Num); if (move.IsAccessibleProbePoint(axesCoords, axes)) { SetMoveBufferDefaults(); - const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber); - moveBuffer.coords[axis0Num] = axis0Coord - zp->GetOffset(axis0Num); - moveBuffer.coords[axis1Num] = axis1Coord - zp->GetOffset(axis1Num); + moveBuffer.coords[axis0Num] = axesCoords[axis0Num]; + moveBuffer.coords[axis1Num] = axesCoords[axis1Num]; moveBuffer.coords[Z_AXIS] = zp->GetStartingHeight(); moveBuffer.feedRate = zp->GetTravelSpeed(); NewMoveAvailable(1); diff --git a/src/GCodes/RestorePoint.cpp b/src/GCodes/RestorePoint.cpp index edbfbf71..f197b25d 100644 --- a/src/GCodes/RestorePoint.cpp +++ b/src/GCodes/RestorePoint.cpp @@ -34,7 +34,7 @@ constexpr ObjectModelTableEntry RestorePoint::objectModelTable[] = { "coords", OBJECT_MODEL_FUNC_NOSELF(&coordinatesArrayDescriptor), ObjectModelEntryFlags::none }, { "extruderPos", OBJECT_MODEL_FUNC(self->virtualExtruderPosition, 1), ObjectModelEntryFlags::none }, { "fanPwm", OBJECT_MODEL_FUNC(self->fanSpeed, 2), ObjectModelEntryFlags::none }, - { "feedRate", OBJECT_MODEL_FUNC(self->feedRate, 1), ObjectModelEntryFlags::none }, + { "feedRate", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->feedRate), 1), ObjectModelEntryFlags::none }, #if SUPPORT_IOBITS { "ioBits", OBJECT_MODEL_FUNC_IF(reprap.GetGCodes().GetMachineType() != MachineType::laser, (int32_t)self->laserPwmOrIoBits.ioBits), ObjectModelEntryFlags::none }, @@ -64,7 +64,7 @@ void RestorePoint::Init() noexcept moveCoords[i] = 0.0; } - feedRate = DefaultFeedRate * SecondsToMinutes; + feedRate = ConvertSpeedFromMmPerMin(DefaultFeedRate); virtualExtruderPosition = 0.0; filePos = noFilePosition; proportionDone = 0.0; diff --git a/src/Heating/Sensors/DhtSensor.cpp b/src/Heating/Sensors/DhtSensor.cpp index 82ddae7c..521e25bb 100644 --- a/src/Heating/Sensors/DhtSensor.cpp +++ b/src/Heating/Sensors/DhtSensor.cpp @@ -17,7 +17,7 @@ constexpr uint16_t MinimumReadInterval = 2000; // ms constexpr uint8_t MaximumReadTime = 20; // ms constexpr uint8_t MinimumOneBitLength = 50; // microseconds -constexpr uint32_t MinimumOneBitStepClocks = (StepTimer::StepClockRate * MinimumOneBitLength)/1000000; +constexpr uint32_t MinimumOneBitStepClocks = (StepClockRate * MinimumOneBitLength)/1000000; // Pulse ISR void DhtDataTransition(CallbackParameter cp) noexcept diff --git a/src/Libraries/Fatfs/diskio.cpp b/src/Libraries/Fatfs/diskio.cpp index 41b8e7fb..22687235 100644 --- a/src/Libraries/Fatfs/diskio.cpp +++ b/src/Libraries/Fatfs/diskio.cpp @@ -67,14 +67,14 @@ unsigned int DiskioGetAndClearMaxRetryCount() noexcept float DiskioGetAndClearLongestReadTime() noexcept { - const float ret = (float)longestReadTime * StepTimer::StepClocksToMillis; + const float ret = (float)longestReadTime * StepClocksToMillis; longestReadTime = 0; return ret; } float DiskioGetAndClearLongestWriteTime() noexcept { - const float ret = (float)longestWriteTime * StepTimer::StepClocksToMillis; + const float ret = (float)longestWriteTime * StepClocksToMillis; longestWriteTime = 0; return ret; } diff --git a/src/Movement/AxisShaper.cpp b/src/Movement/AxisShaper.cpp new file mode 100644 index 00000000..64738915 --- /dev/null +++ b/src/Movement/AxisShaper.cpp @@ -0,0 +1,967 @@ +/* + * InputShaper.cpp + * + * Created on: 20 Feb 2021 + * Author: David + */ + +#include "AxisShaper.h" + +#include <GCodes/GCodeBuffer/GCodeBuffer.h> +#include <Platform/RepRap.h> +#include "StepTimer.h" +#include "DDA.h" +#include "MoveSegment.h" + +// Object model table and functions +// Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17. +// Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM. + +// Macro to build a standard lambda function that includes the necessary type conversions +#define OBJECT_MODEL_FUNC(...) OBJECT_MODEL_FUNC_BODY(AxisShaper, __VA_ARGS__) +#define OBJECT_MODEL_FUNC_IF(...) OBJECT_MODEL_FUNC_IF_BODY(AxisShaper, __VA_ARGS__) + +constexpr ObjectModelTableEntry AxisShaper::objectModelTable[] = +{ + // Within each group, these entries must be in alphabetical order + // 0. InputShaper members + { "damping", OBJECT_MODEL_FUNC(self->zeta, 2), ObjectModelEntryFlags::none }, + { "frequency", OBJECT_MODEL_FUNC(self->frequency, 2), ObjectModelEntryFlags::none }, + { "minAcceleration", OBJECT_MODEL_FUNC(self->minimumAcceleration, 1), ObjectModelEntryFlags::none }, + { "type", OBJECT_MODEL_FUNC(self->type.ToString()), ObjectModelEntryFlags::none }, +}; + +constexpr uint8_t AxisShaper::objectModelTableDescriptor[] = { 1, 4 }; + +DEFINE_GET_OBJECT_MODEL_TABLE(AxisShaper) + +AxisShaper::AxisShaper() noexcept + : numExtraImpulses(0), + frequency(DefaultFrequency), + zeta(DefaultDamping), + minimumAcceleration(ConvertAcceleration(DefaultMinimumAcceleration)), + type(InputShaperType::none) +{ +} + +// Process M593 +GCodeResult AxisShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +{ + constexpr float MinimumInputShapingFrequency = (float)StepClockRate/(2 * 65535); // we use a 16-bit number of step clocks to represent half the input shaping period + constexpr float MaximumInputShapingFrequency = 1000.0; + bool seen = false; + + // If we are changing the type, frequency, damping or custom parameters, we will change multiple stored values used by the motion planner, so wait until movement has stopped. + // Changing just the minimum acceleration is OK because no other variables depend on it. + if (gb.SeenAny("FSPHT")) + { + if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb)) + { + return GCodeResult::notFinished; + } + } + + if (gb.Seen('F')) + { + seen = true; + frequency = gb.GetLimitedFValue('F', MinimumInputShapingFrequency, MaximumInputShapingFrequency); + } + if (gb.Seen('L')) + { + seen = true; + minimumAcceleration = ConvertAcceleration(max<float>(gb.GetFValue(), 1.0)); // very low accelerations cause problems with the maths + } + if (gb.Seen('S')) + { + seen = true; + zeta = gb.GetLimitedFValue('S', 0.0, 0.99); + } + + if (gb.Seen('P')) + { + String<StringLength20> shaperName; + gb.GetReducedString(shaperName.GetRef()); + const InputShaperType newType(shaperName.c_str()); + if (!newType.IsValid()) + { + reply.printf("Unsupported input shaper type '%s'", shaperName.c_str()); + return GCodeResult::error; + } + seen = true; + type = newType; + } + else if (seen && type == InputShaperType::none) + { +#if SUPPORT_DAA + // For backwards compatibility, if we have set input shaping parameters but not defined shaping type, default to DAA for now. Change this when we support better types of input shaping. + type = InputShaperType::daa; +#else + type = InputShaperType::zvd; +#endif + } + + if (seen) + { + const float sqrtOneMinusZetaSquared = fastSqrtf(1.0 - fsquare(zeta)); + const float dampedFrequency = frequency * sqrtOneMinusZetaSquared; + const float dampedPeriod = StepClockRate/dampedFrequency; + const float k = expf(-zeta * Pi/sqrtOneMinusZetaSquared); + switch (type.RawValue()) + { + case InputShaperType::none: + numExtraImpulses = 0; + break; + + case InputShaperType::custom: + { + // Get the coefficients + size_t numAmplitudes = MaxExtraImpulses; + gb.MustSee('H'); + gb.GetFloatArray(coefficients, numAmplitudes, false); + + // Get the impulse durations, if provided + if (gb.Seen('T')) + { + size_t numDurations = numAmplitudes; + gb.GetFloatArray(durations, numDurations, true); + + // Check we have the same number of both + if (numDurations != numAmplitudes) + { + reply.copy("Too few durations given"); + type = InputShaperType::none; + return GCodeResult::error; + } + for (unsigned int i = 0; i < numAmplitudes; ++i) + { + durations[i] *= StepClockRate; // convert from seconds to step clocks + } + } + else + { + for (unsigned int i = 0; i < numAmplitudes; ++i) + { + durations[i] = 0.5 * dampedPeriod; + } + } + numExtraImpulses = numAmplitudes; + } + break; + +#if SUPPORT_DAA + case InputShaperType::daa: + durations[0] = dampedPeriod; + numExtraImpulses = 0; + break; +#endif + + case InputShaperType::zvd: // see https://www.researchgate.net/publication/316556412_INPUT_SHAPING_CONTROL_TO_REDUCE_RESIDUAL_VIBRATION_OF_A_FLEXIBLE_BEAM + { + const float j = 1.0 + 2.0 * k + fsquare(k); + coefficients[0] = 1.0/j; + coefficients[1] = coefficients[0] + 2.0 * k/j; + } + durations[0] = durations[1] = 0.5 * dampedPeriod; + numExtraImpulses = 2; + break; + + case InputShaperType::zvdd: // see https://www.researchgate.net/publication/316556412_INPUT_SHAPING_CONTROL_TO_REDUCE_RESIDUAL_VIBRATION_OF_A_FLEXIBLE_BEAM + { + const float j = 1.0 + 3.0 * (k + fsquare(k)) + k * fsquare(k); + coefficients[0] = 1.0/j; + coefficients[1] = coefficients[0] + 3.0 * k/j; + coefficients[2] = coefficients[1] + 3.0 * fsquare(k)/j; + } + durations[0] = durations[1] = durations[2] = 0.5 * dampedPeriod; + numExtraImpulses = 3; + break; + + case InputShaperType::ei2: // see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.465.1337&rep=rep1&type=pdf. United States patent #4,916,635. + { + const float zetaSquared = fsquare(zeta); + const float zetaCubed = zetaSquared * zeta; + coefficients[0] = (0.16054) + (0.76699) * zeta + (2.26560) * zetaSquared + (-1.22750) * zetaCubed; + coefficients[1] = (0.16054 + 0.33911) + (0.76699 + 0.45081) * zeta + (2.26560 - 2.58080) * zetaSquared + (-1.22750 + 1.73650) * zetaCubed; + coefficients[2] = (0.16054 + 0.33911 + 0.34089) + (0.76699 + 0.45081 - 0.61533) * zeta + (2.26560 - 2.58080 - 0.68765) * zetaSquared + (-1.22750 + 1.73650 + 0.42261) * zetaCubed; + + durations[0] = ((0.49890) + ( 0.16270 ) * zeta + ( -0.54262) * zetaSquared + ( 6.16180) * zetaCubed) * dampedPeriod; + durations[1] = ((0.99748 - 0.49890) + ( 0.18382 - 0.16270) * zeta + (-1.58270 + 0.54262) * zetaSquared + (8.17120 - 6.16180) * zetaCubed) * dampedPeriod; + durations[2] = ((1.49920 - 0.99748) + (-0.09297 - 0.18382) * zeta + (-0.28338 + 1.58270) * zetaSquared + (1.85710 - 8.17120) * zetaCubed) * dampedPeriod; + } + numExtraImpulses = 3; + break; + + case InputShaperType::ei3: // see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.465.1337&rep=rep1&type=pdf. United States patent #4,916,635 + { + const float zetaSquared = fsquare(zeta); + const float zetaCubed = zetaSquared * zeta; + coefficients[0] = (0.11275) + 0.76632 * zeta + (3.29160) * zetaSquared + (-1.44380) * zetaCubed; + coefficients[1] = (0.11275 + 0.23698) + (0.76632 + 0.61164) * zeta + (3.29160 - 2.57850) * zetaSquared + (-1.44380 + 4.85220) * zetaCubed; + coefficients[2] = (0.11275 + 0.23698 + 0.30008) + (0.76632 + 0.61164 - 0.19062) * zeta + (3.29160 - 2.57850 - 2.14560) * zetaSquared + (-1.44380 + 4.85220 + 0.13744) * zetaCubed; + coefficients[3] = (0.11275 + 0.23698 + 0.30008 + 0.23775) + (0.76632 + 0.61164 - 0.19062 - 0.73297) * zeta + (3.29160 - 2.57850 - 2.14560 + 0.46885) * zetaSquared + (-1.44380 + 4.85220 + 0.13744 - 2.08650) * zetaCubed; + + durations[0] = ((0.49974) + (0.23834) * zeta + (0.44559) * zetaSquared + (12.4720) * zetaCubed) * dampedPeriod; + durations[1] = ((0.99849 - 0.49974) + (0.29808 - 0.23834) * zeta + (-2.36460 - 0.44559) * zetaSquared + (23.3990 - 12.4720) * zetaCubed) * dampedPeriod; + durations[2] = ((1.49870 - 0.99849) + (0.10306 - 0.29808) * zeta + (-2.01390 + 2.36460) * zetaSquared + (17.0320 - 23.3990) * zetaCubed) * dampedPeriod; + durations[3] = ((1.99960 - 1.49870) + (-0.28231 - 0.10306) * zeta + (0.61536 + 2.01390) * zetaSquared + (5.40450 - 17.0320) * zetaCubed) * dampedPeriod; + } + numExtraImpulses = 4; + break; + } + + // Calculate the total extra duration of input shaping + totalShapingClocks = 0.0; + extraClocksAtStart = 0.0; + extraClocksAtEnd = 0.0; + extraDistanceAtStart = 0.0; + extraDistanceAtEnd = 0.0; + + { + float u = 0.0; + for (unsigned int i = 0; i < numExtraImpulses; ++i) + { + const float segTime = durations[i]; + totalShapingClocks += segTime; + extraClocksAtStart += (1.0 - coefficients[i]) * segTime; + extraClocksAtEnd += coefficients[i] * segTime; + const float speedChange = coefficients[i] * segTime; + extraDistanceAtStart += (1.0 - coefficients[i]) * (u + 0.5 * speedChange) * segTime; + u += speedChange; + } + } + + minimumShapingStartOriginalClocks = totalShapingClocks - extraClocksAtStart + (MinimumMiddleSegmentTime * StepClockRate); + minimumShapingEndOriginalClocks = totalShapingClocks - extraClocksAtEnd + (MinimumMiddleSegmentTime * StepClockRate); + minimumNonOverlappedOriginalClocks = (totalShapingClocks * 2) - extraClocksAtStart - extraClocksAtEnd + (MinimumMiddleSegmentTime * StepClockRate); + + { + float v = 0.0; + for (int i = numExtraImpulses - 1; i >= 0; --i) + { + const float segTime = durations[i]; + const float speedChange = (1.0 - coefficients[i]) * segTime; + extraDistanceAtEnd += coefficients[i] * (v - 0.5 * speedChange) * segTime; + v -= speedChange; + } + } + + if (numExtraImpulses != 0) + { + overlappedShapingClocks = 2 * totalShapingClocks; + // Calculate the clocks and coefficients needed when we shape the start of acceleration/deceleration and then immediately shape the end + float maxVal = 0.0; + for (unsigned int i = 0; i < numExtraImpulses; ++i) + { + overlappedDurations[i] = overlappedDurations[i + numExtraImpulses] = durations[i]; + float val = coefficients[i]; + overlappedCoefficients[i] = val; + if (val > maxVal) + { + maxVal = val; + } + val = 1.0 - val; + overlappedCoefficients[i + numExtraImpulses] = val; + if (val > maxVal) + { + maxVal = val; + } + } + + // Now scale the values by maxVal so that the highest coefficient is 1.0, and calculate the total distance per unit acceleration + overlappedDistancePerA = 0.0; + float u = 0.0; + for (unsigned int i = 0; i < 2 * numExtraImpulses; ++i) + { + overlappedCoefficients[i] /= maxVal; + const float speedChange = overlappedCoefficients[i] * overlappedDurations[i]; + overlappedDistancePerA += (u + 0.5 * speedChange) * overlappedDurations[i]; + u += speedChange; + } + overlappedDeltaVPerA = u; + } + + reprap.MoveUpdated(); + } + else if (type == InputShaperType::none) + { + reply.copy("Input shaping is disabled"); + } + else + { + reply.printf("Input shaping '%s' at %.1fHz damping factor %.2f, min. acceleration %.1f", + type.ToString(), (double)frequency, (double)zeta, (double)InverseConvertAcceleration(minimumAcceleration)); + if (numExtraImpulses != 0) + { + reply.cat(", impulses"); + for (unsigned int i = 0; i < numExtraImpulses; ++i) + { + reply.catf(" %.3f", (double)coefficients[i]); + } + reply.cat(" with durations (ms)"); + for (unsigned int i = 0; i < numExtraImpulses; ++i) + { + reply.catf(" %.2f", (double)(durations[i] * StepClocksToMillis)); + } + if (reprap.Debug(moduleMove)) + { + reply.catf(" odpa=%.4e odvpa=%.4e ovc=", (double)overlappedDistancePerA, (double)overlappedDeltaVPerA); + for (unsigned int i = 0; i < 2 * numExtraImpulses; ++i) + { + reply.catf(" %.3f", (double)overlappedCoefficients[i]); + } + } + } + } + return GCodeResult::ok; +} + +// Plan input shaping, generate the MoveSegment, and set up the basic move parameters. +// On entry, params.shapingPlan is set to 'no shaping'. +// Currently we use a single input shaper for all axes, so the move segments are attached to the DDA not the DM +void AxisShaper::PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled) const noexcept +{ + switch ((shapingEnabled) ? type.RawValue() : InputShaperType::none) + { +#if SUPPORT_DAA + case InputShaperType::daa: + do + { + // Try to reduce the acceleration/deceleration of the move to cancel ringing + const float idealPeriod = 1.0/frequency; // for DAA this the full period, 1.0 + + float proposedAcceleration = dda.acceleration, proposedAccelDistance = dda.beforePrepare.accelDistance; + bool adjustAcceleration = false; + if (dda.topSpeed > dda.startSpeed && ((dda.GetPrevious()->state != DDA::DDAState::frozen && dda.GetPrevious()->state != DDA::DDAState::executing) || !dda.GetPrevious()->flags.wasAccelOnlyMove)) + { + const float accelTime = (dda.topSpeed - dda.startSpeed)/dda.acceleration; + if (accelTime < idealPeriod) + { + proposedAcceleration = (dda.topSpeed - dda.startSpeed) * frequency; + adjustAcceleration = true; + } + else if (accelTime < idealPeriod * 2) + { + proposedAcceleration = (dda.topSpeed - dda.startSpeed) * frequency * 0.5; + adjustAcceleration = true; + } + if (adjustAcceleration) + { + proposedAccelDistance = (fsquare(dda.topSpeed) - fsquare(dda.startSpeed))/(2 * proposedAcceleration); + } + } + + float proposedDeceleration = dda.deceleration, proposedDecelDistance = dda.beforePrepare.decelDistance; + bool adjustDeceleration = false; + if (dda.GetNext()->state != DDA::DDAState::provisional || !dda.GetNext()->IsDecelerationMove()) + { + const float decelTime = (dda.topSpeed - dda.endSpeed)/dda.deceleration; + if (decelTime < idealPeriod) + { + proposedDeceleration = (dda.topSpeed - dda.endSpeed) * frequency; + adjustDeceleration = true; + } + else if (decelTime < idealPeriod * 2) + { + proposedDeceleration = (dda.topSpeed - dda.endSpeed) * frequency * 0.5; + adjustDeceleration = true; + } + if (adjustDeceleration) + { + proposedDecelDistance = (fsquare(dda.topSpeed) - fsquare(dda.endSpeed))/(2 * proposedDeceleration); + } + } + + if (adjustAcceleration || adjustDeceleration) + { + if (proposedAccelDistance + proposedDecelDistance <= dda.totalDistance) + { + if (proposedAcceleration < minimumAcceleration || proposedDeceleration < minimumAcceleration) + { + break; + } + dda.acceleration = proposedAcceleration; + dda.deceleration = proposedDeceleration; + dda.beforePrepare.accelDistance = proposedAccelDistance; + dda.beforePrepare.decelDistance = proposedDecelDistance; + } + else + { + // We can't keep this as a trapezoidal move with the original top speed. + // Try an accelerate-decelerate move with acceleration and deceleration times equal to the ideal period. + const float twiceTotalDistance = 2 * dda.totalDistance; + float proposedTopSpeed = dda.totalDistance * frequency - (dda.startSpeed + dda.endSpeed)/2; + if (proposedTopSpeed > dda.startSpeed && proposedTopSpeed > dda.endSpeed) + { + proposedAcceleration = (twiceTotalDistance - ((3 * dda.startSpeed + dda.endSpeed) * idealPeriod)) * fsquare(frequency) * 0.5; + proposedDeceleration = (twiceTotalDistance - ((dda.startSpeed + 3 * dda.endSpeed) * idealPeriod)) * fsquare(frequency) * 0.5; + if ( proposedAcceleration < minimumAcceleration || proposedDeceleration < minimumAcceleration + || proposedAcceleration > dda.acceleration || proposedDeceleration > dda.deceleration + ) + { + break; + } + dda.topSpeed = proposedTopSpeed; + dda.acceleration = proposedAcceleration; + dda.deceleration = proposedDeceleration; + dda.beforePrepare.accelDistance = dda.startSpeed * idealPeriod + (dda.acceleration * fsquare(idealPeriod)) * 0.5; + dda.beforePrepare.decelDistance = dda.endSpeed * idealPeriod + (dda.deceleration * fsquare(idealPeriod)) * 0.5; + } + else if (dda.startSpeed < dda.endSpeed) + { + // Change it into an accelerate-only move, accelerating as slowly as we can + proposedAcceleration = (fsquare(dda.endSpeed) - fsquare(dda.startSpeed))/twiceTotalDistance; + if (proposedAcceleration < minimumAcceleration) + { + break; // avoid very small accelerations because they can be problematic + } + dda.acceleration = proposedAcceleration; + dda.topSpeed = dda.endSpeed; + dda.beforePrepare.accelDistance = dda.totalDistance; + dda.beforePrepare.decelDistance = 0.0; + } + else if (dda.startSpeed > dda.endSpeed) + { + // Change it into a decelerate-only move, decelerating as slowly as we can + proposedDeceleration = (fsquare(dda.startSpeed) - fsquare(dda.endSpeed))/twiceTotalDistance; + if (proposedDeceleration < minimumAcceleration) + { + break; // avoid very small accelerations because they can be problematic + } + dda.deceleration = proposedDeceleration; + dda.topSpeed = dda.startSpeed; + dda.beforePrepare.decelDistance = dda.totalDistance; + } + else + { + // Start and end speeds are exactly the same, possibly zero, so give up trying to adjust this move + break; + } + } + + if (reprap.Debug(moduleMove)) + { + debugPrintf("DAA: new a=%.1f d=%.1f\n", (double)dda.acceleration, (double)dda.deceleration); + } + } + } while (false); // this loop is solely for the purpose of catching 'break' statements + params.SetFromDDA(dda); + break; +#endif + + case InputShaperType::none: + default: + params.SetFromDDA(dda); + break; + + // The other input shapers all have multiple impulses with varying coefficients + case InputShaperType::zvd: + case InputShaperType::zvdd: + case InputShaperType::ei2: + case InputShaperType::ei3: + params.SetFromDDA(dda); // set up the provisional parameters + + if (params.unshaped.accelDistance < params.unshaped.decelStartDistance) // we can't do any shaping unless there is a steady speed segment that can be shortened + { + params.shaped = params.unshaped; + //TODO if we want to shape both acceleration and deceleration but the steady distance is zero or too short, we could reduce the top speed + if (params.unshaped.accelDistance > 0.0) + { + if ((dda.GetPrevious()->state != DDA::DDAState::frozen && dda.GetPrevious()->state != DDA::DDAState::executing) || !dda.GetPrevious()->flags.wasAccelOnlyMove) + { + TryShapeAccelBoth(dda, params); + } + else if (params.unshaped.accelClocks >= minimumShapingEndOriginalClocks) + { + TryShapeAccelEnd(dda, params); + } + } + if (params.unshaped.decelStartDistance < dda.totalDistance) + { + if (dda.GetNext()->GetState() != DDA::DDAState::provisional || !dda.GetNext()->IsDecelerationMove()) + { + TryShapeDecelBoth(dda, params); + } + else if (params.unshaped.decelClocks >= minimumShapingStartOriginalClocks) + { + TryShapeDecelStart(dda, params); + } + } + } + break; + } + + // If we are doing any input shaping then set up dda.shapedSegments, else leave it as null + if (params.shapingPlan.IsShaped()) + { + MoveSegment * const accelSegs = GetAccelerationSegments(dda, params); + MoveSegment * const decelSegs = GetDecelerationSegments(dda, params); + params.shaped.Finalise(dda.topSpeed); // this sets up params.shaped.steadyClocks, which is needed by FinishShapedSegments + dda.clocksNeeded = params.shaped.TotalClocks(); + dda.shapedSegments = FinishShapedSegments(dda, params, accelSegs, decelSegs); + params.unshaped.steadyClocks = max<float>(dda.clocksNeeded - params.unshaped.accelClocks - params.unshaped.decelClocks, 0.0); + } + else + { + params.unshaped.Finalise(dda.topSpeed); // this sets up params.steadyClocks + dda.clocksNeeded = params.unshaped.TotalClocks(); + } + +// debugPrintf(" final plan %03x\n", (unsigned int)params.shapingPlan.all); +} + +// Try to shape the end of the acceleration. We already know that there is sufficient acceleration time to do this, but we still need to check that there is enough distance. +void AxisShaper::TryShapeAccelEnd(const DDA& dda, PrepParams& params) const noexcept +{ + const float extraAccelDistance = GetExtraAccelEndDistance(dda.topSpeed, params.unshaped.acceleration); + if (ImplementAccelShaping(dda, params, params.unshaped.accelDistance + extraAccelDistance, params.unshaped.accelClocks + extraClocksAtEnd)) + { + params.shapingPlan.shapeAccelEnd = true; + } + else + { + // Not enough constant speed time to the acceleration shaping + if (reprap.Debug(Module::moduleDda)) + { + debugPrintf("Can't shape accel end\n"); + } + } +} + +void AxisShaper::TryShapeAccelBoth(DDA& dda, PrepParams& params) const noexcept +{ + if (dda.topSpeed - dda.startSpeed <= overlappedDeltaVPerA * params.unshaped.acceleration) + { + // We can use overlapped shaping + const float newAcceleration = (dda.topSpeed - dda.startSpeed)/overlappedDeltaVPerA; + if (newAcceleration >= minimumAcceleration) + { + const float newAccelDistance = (dda.startSpeed * overlappedShapingClocks) + (newAcceleration * overlappedDistancePerA); + if (ImplementAccelShaping(dda, params, newAccelDistance, overlappedShapingClocks)) + { + params.shapingPlan.shapeAccelOverlapped = true; + params.shaped.acceleration = newAcceleration; + } + } + } + else if (params.unshaped.accelClocks < minimumNonOverlappedOriginalClocks) + { + // The speed change is too high to allow overlapping, but non-overlapped shaping will give a very short steady acceleration segment. + // If we have enough spare distance, reduce the acceleration slightly to lengthen that segment. + const float newAcceleration = (dda.topSpeed - dda.startSpeed)/minimumNonOverlappedOriginalClocks; + const float newUnshapedAccelDistance = (dda.startSpeed + 0.5 * newAcceleration * minimumNonOverlappedOriginalClocks) * minimumNonOverlappedOriginalClocks; + const float extraAccelDistance = GetExtraAccelStartDistance(dda.startSpeed, newAcceleration) + GetExtraAccelEndDistance(dda.topSpeed, newAcceleration); + if (ImplementAccelShaping(dda, params, newUnshapedAccelDistance + extraAccelDistance, minimumNonOverlappedOriginalClocks + extraClocksAtStart + extraClocksAtEnd)) + { + params.shapingPlan.shapeAccelStart = params.shapingPlan.shapeAccelEnd = true; + params.shaped.acceleration = newAcceleration; + //params.shapingPlan.debugPrint = true; + } + } + else + { + // We only attempt shaping if we can shape both the start and end of acceleration + const float extraAccelDistance = GetExtraAccelStartDistance(dda.startSpeed, params.unshaped.acceleration) + GetExtraAccelEndDistance(dda.topSpeed, params.unshaped.acceleration); + if (ImplementAccelShaping(dda, params, params.unshaped.accelDistance + extraAccelDistance, params.unshaped.accelClocks + extraClocksAtStart + extraClocksAtEnd)) + { + params.shapingPlan.shapeAccelStart = params.shapingPlan.shapeAccelEnd = true; + } + } +} + +// Check whether we can implement acceleration shaping using the proposed parameters; if so then implement it and return true; else return false with nothing changed +bool AxisShaper::ImplementAccelShaping(const DDA& dda, PrepParams& params, float newAccelDistance, float newAccelClocks) const noexcept +{ + if (newAccelDistance <= params.shaped.decelStartDistance) + { + const float speedIncrease = dda.topSpeed - dda.startSpeed; + const float unshapedAccelClocks = 2 * (dda.topSpeed * newAccelClocks - newAccelDistance)/speedIncrease; + const float unshapedAccelDistance = (dda.startSpeed + dda.topSpeed) * unshapedAccelClocks * 0.5; + if (unshapedAccelDistance <= params.unshaped.decelStartDistance) + { + params.shaped.accelDistance = newAccelDistance; + params.shaped.accelClocks = newAccelClocks; + params.unshaped.accelClocks = unshapedAccelClocks; + params.unshaped.accelDistance = unshapedAccelDistance; + params.unshaped.acceleration = speedIncrease/unshapedAccelClocks; + return true; + } + } + + return false; +} + +// Try to shape the start of the deceleration. We already know that there is sufficient deceleration time to do this, but we still need to check that there is enough distance. +void AxisShaper::TryShapeDecelStart(const DDA& dda, PrepParams& params) const noexcept +{ + const float extraDecelDistance = GetExtraDecelStartDistance(dda.topSpeed, params.unshaped.deceleration); + if (ImplementDecelShaping(dda, params, params.unshaped.decelStartDistance - extraDecelDistance, params.unshaped.decelClocks + extraClocksAtStart)) + { + params.shapingPlan.shapeDecelStart = true; + } + else + { + // Not enough constant speed time to do deceleration shaping + if (reprap.Debug(Module::moduleDda)) + { + debugPrintf("Can't shape decel start\n"); + } + } +} + +void AxisShaper::TryShapeDecelBoth(DDA& dda, PrepParams& params) const noexcept +{ + if (dda.topSpeed - dda.endSpeed <= overlappedDeltaVPerA * params.unshaped.deceleration) + { + // We can use overlapped shaping + const float newDeceleration = (dda.topSpeed - dda.endSpeed)/overlappedDeltaVPerA; + if (newDeceleration >= minimumAcceleration) + { + const float newDecelDistance = (dda.topSpeed * overlappedShapingClocks) - (newDeceleration * overlappedDistancePerA); + if (ImplementDecelShaping(dda, params, dda.totalDistance - newDecelDistance, overlappedShapingClocks)) + { + params.shapingPlan.shapeDecelOverlapped = true; + params.shaped.deceleration = newDeceleration; + } + } + } + else if (params.unshaped.decelClocks < minimumNonOverlappedOriginalClocks) + { + // The speed change is too high to allow overlapping, but non-overlapped shaping will give a very short steady acceleration segment. + // If we have enough spare distance, reduce the acceleration slightly to lengthen that segment. + const float newDeceleration = (dda.topSpeed - dda.startSpeed)/minimumNonOverlappedOriginalClocks; + const float newUnshapedDecelDistance = (dda.endSpeed + (0.5 * newDeceleration * minimumNonOverlappedOriginalClocks)) * minimumNonOverlappedOriginalClocks; + const float extraDecelDistance = GetExtraDecelStartDistance(dda.startSpeed, newDeceleration) + GetExtraDecelEndDistance(dda.topSpeed, newDeceleration); + if (ImplementDecelShaping(dda, params, dda.totalDistance - (newUnshapedDecelDistance + extraDecelDistance), minimumNonOverlappedOriginalClocks + extraClocksAtStart + extraClocksAtEnd)) + { + params.shapingPlan.shapeDecelStart = params.shapingPlan.shapeDecelEnd = true; + params.shaped.deceleration = newDeceleration; + //params.shapingPlan.debugPrint = true; + } + } + else + { + // Only perform shaping if we can shape both the start and end of deceleration, otherwise we may not be able to generate a corresponding unshaped move because it might require negative steady distance + const float extraDecelDistance = GetExtraDecelStartDistance(dda.topSpeed, params.unshaped.deceleration) + GetExtraDecelEndDistance(dda.endSpeed, params.unshaped.deceleration); + if (ImplementDecelShaping(dda, params, params.unshaped.decelStartDistance - extraDecelDistance, params.unshaped.decelClocks + extraClocksAtStart + extraClocksAtEnd)) + { + params.shapingPlan.shapeDecelStart = params.shapingPlan.shapeDecelEnd = true; + } + } +} + +// Check whether we can implement acceleration shaping using the proposed parameters; if so then implement it and return true; else return false with nothing changed +bool AxisShaper::ImplementDecelShaping(const DDA& dda, PrepParams& params, float newDecelStartDistance, float newDecelClocks) const noexcept +{ + if (params.shaped.accelDistance <= newDecelStartDistance) + { + const float speedDecrease = dda.topSpeed - dda.endSpeed; + const float unshapedDecelClocks = 2 * (dda.topSpeed * newDecelClocks - (dda.totalDistance - newDecelStartDistance))/speedDecrease; + const float unshapedDecelDistance = (dda.topSpeed + dda.endSpeed) * unshapedDecelClocks * 0.5; + if (params.unshaped.accelDistance + unshapedDecelDistance <= dda.totalDistance) + { + params.shaped.decelStartDistance = newDecelStartDistance; + params.shaped.decelClocks = newDecelClocks; + params.unshaped.decelClocks = unshapedDecelClocks; + params.unshaped.decelStartDistance = dda.totalDistance - unshapedDecelDistance; + params.unshaped.deceleration = speedDecrease/unshapedDecelClocks; + return true; + } + } + + return false; +} + +// If there is an acceleration phase, generate the acceleration segments according to the plan, and set the number of acceleration segments in the plan +MoveSegment *AxisShaper::GetAccelerationSegments(const DDA& dda, PrepParams& params) const noexcept +{ + if (params.shaped.accelDistance > 0.0) + { + if (params.shapingPlan.shapeAccelOverlapped) + { + MoveSegment *accelSegs = nullptr; + float segStartSpeed = dda.topSpeed; + for (unsigned int i = 2 * numExtraImpulses; i != 0; ) + { + --i; + accelSegs = MoveSegment::Allocate(accelSegs); + const float acceleration = params.shaped.acceleration * overlappedCoefficients[i]; + const float segTime = overlappedDurations[i]; + segStartSpeed -= acceleration * segTime; + const float b = segStartSpeed/(-acceleration); + const float c = 2.0/acceleration; + const float segLen = (segStartSpeed + (0.5 * acceleration * segTime)) * segTime; + accelSegs->SetNonLinear(segLen, segTime, b, c); + } + return accelSegs; + } + + float accumulatedSegTime = 0.0; + float endDistance = params.shaped.accelDistance; + MoveSegment *endAccelSegs = nullptr; + if (params.shapingPlan.shapeAccelEnd) + { + // Shape the end of the acceleration + float segStartSpeed = dda.topSpeed; + for (unsigned int i = numExtraImpulses; i != 0; ) + { + --i; + endAccelSegs = MoveSegment::Allocate(endAccelSegs); + const float acceleration = params.shaped.acceleration * (1.0 - coefficients[i]); + const float segTime = durations[i]; + segStartSpeed -= acceleration * segTime; + const float b = segStartSpeed/(-acceleration); + const float c = 2.0/acceleration; + const float segLen = (segStartSpeed + (0.5 * acceleration * segTime)) * segTime; + endDistance -= segLen; + endAccelSegs->SetNonLinear(segLen, segTime, b, c); + } + accumulatedSegTime += totalShapingClocks; + } + + float startDistance = 0.0; + float startSpeed = dda.startSpeed; + MoveSegment *startAccelSegs = nullptr; + if (params.shapingPlan.shapeAccelStart) + { + // Shape the start of the acceleration + for (unsigned int i = 0; i < numExtraImpulses; ++i) + { + MoveSegment *seg = MoveSegment::Allocate(nullptr); + const float acceleration = params.shaped.acceleration * coefficients[i]; + const float segTime = durations[i]; + const float b = startSpeed/(-acceleration); + const float c = 2.0/acceleration; + const float segLen = (startSpeed + (0.5 * acceleration * segTime)) * segTime; + startDistance += segLen; + seg->SetNonLinear(segLen, segTime, b, c); + if (i == 0) + { + startAccelSegs = seg; + } + else + { + startAccelSegs->AddToTail(seg); + } + startSpeed += acceleration * segTime; + } + accumulatedSegTime += totalShapingClocks; + } + + // Do the constant acceleration part + if (endDistance > startDistance) + { + endAccelSegs = MoveSegment::Allocate(endAccelSegs); + const float b = startSpeed/(-params.shaped.acceleration); + const float c = 2.0/params.shaped.acceleration; + endAccelSegs->SetNonLinear(endDistance - startDistance, params.shaped.accelClocks - accumulatedSegTime, b, c); + } + else if (reprap.Debug(moduleMove)) + { + debugPrintf("Missing steady accel segment\n"); + params.shapingPlan.debugPrint = true; + } + + if (startAccelSegs == nullptr) + { + return endAccelSegs; + } + + if (endAccelSegs != nullptr) + { + startAccelSegs->AddToTail(endAccelSegs); + } + return startAccelSegs; + } + + return nullptr; +} + +// If there is a deceleration phase, generate the deceleration segments according to the plan, and set the number of deceleration segments in the plan +MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, PrepParams& params) const noexcept +{ + if (params.shaped.decelStartDistance < dda.totalDistance) + { + if (params.shapingPlan.shapeDecelOverlapped) + { + MoveSegment *decelSegs = nullptr; + float segStartSpeed = dda.endSpeed; + for (unsigned int i = 2 * numExtraImpulses; i != 0; ) + { + --i; + decelSegs = MoveSegment::Allocate(decelSegs); + const float deceleration = params.shaped.deceleration * overlappedCoefficients[i]; + const float segTime = overlappedDurations[i]; + segStartSpeed += deceleration * segTime; + const float b = segStartSpeed/deceleration; + const float c = -2.0/deceleration; + const float segLen = (segStartSpeed + (-0.5 * deceleration * segTime)) * segTime; + decelSegs->SetNonLinear(segLen, segTime, b, c); + } + return decelSegs; + } + + float accumulatedSegTime = 0.0; + float endDistance = dda.totalDistance; + MoveSegment *endDecelSegs = nullptr; + if (params.shapingPlan.shapeDecelEnd) + { + // Shape the end of the deceleration + float segStartSpeed = dda.endSpeed; + for (unsigned int i = numExtraImpulses; i != 0; ) + { + --i; + endDecelSegs = MoveSegment::Allocate(endDecelSegs); + const float deceleration = params.shaped.deceleration * (1.0 - coefficients[i]); + const float segTime = durations[i]; + segStartSpeed += deceleration * segTime; + const float b = segStartSpeed/deceleration; + const float c = -2.0/deceleration; + const float segLen = (segStartSpeed + (-0.5 * deceleration * segTime)) * segTime; + endDecelSegs->SetNonLinear(segLen, segTime, b, c); + endDistance -= segLen; + } + accumulatedSegTime += totalShapingClocks; + } + + float startDistance = params.shaped.decelStartDistance; + float startSpeed = dda.topSpeed; + MoveSegment *startDecelSegs = nullptr; + if (params.shapingPlan.shapeDecelStart) + { + // Shape the start of the deceleration + for (unsigned int i = 0; i < numExtraImpulses; ++i) + { + MoveSegment *seg = MoveSegment::Allocate(nullptr); + const float deceleration = params.shaped.deceleration * coefficients[i]; + const float segTime = durations[i]; + const float b = startSpeed/deceleration; + const float c = -2.0/deceleration; + const float segLen = (startSpeed + (-0.5 * deceleration * segTime)) * segTime; + startDistance += segLen; + seg->SetNonLinear(segLen, segTime, b, c); + if (i == 0) + { + startDecelSegs = seg; + } + else + { + startDecelSegs->AddToTail(seg); + } + startSpeed -= deceleration * segTime; + } + accumulatedSegTime += totalShapingClocks; + } + + // Do the constant deceleration part + if (endDistance > startDistance) + { + endDecelSegs = MoveSegment::Allocate(endDecelSegs); + const float b = startSpeed/params.shaped.deceleration; + const float c = -2.0/params.shaped.deceleration; + endDecelSegs->SetNonLinear(endDistance - startDistance, params.shaped.decelClocks - accumulatedSegTime, b, c); + } + else if (reprap.Debug(moduleMove)) + { + debugPrintf("Missing steady decel segment\n"); + params.shapingPlan.debugPrint = true; + } + + if (startDecelSegs == nullptr) + { + return endDecelSegs; + } + + if (endDecelSegs != nullptr) + { + startDecelSegs->AddToTail(endDecelSegs); + } + return startDecelSegs; + } + + return nullptr; +} + +// Generate the steady speed segment (if any), tack the segments together, and attach them to the DDA +// Must set up params.steadyClocks before calling this +MoveSegment *AxisShaper::FinishShapedSegments(const DDA& dda, const PrepParams& params, MoveSegment *accelSegs, MoveSegment *decelSegs) const noexcept +{ + if (params.shaped.steadyClocks > 0.0) + { + // Insert a steady speed segment before the deceleration segments + decelSegs = MoveSegment::Allocate(decelSegs); + const float c = 1.0/dda.topSpeed; + decelSegs->SetLinear(params.shaped.decelStartDistance - params.shaped.accelDistance, params.shaped.steadyClocks, c); + } + + if (accelSegs != nullptr) + { + if (decelSegs != nullptr) + { + accelSegs->AddToTail(decelSegs); + } + return accelSegs; + } + + return decelSegs; +} + +// Calculate the additional acceleration distance needed if we shape the start of acceleration +inline float AxisShaper::GetExtraAccelStartDistance(float startSpeed, float acceleration) const noexcept +{ + return (extraClocksAtStart * startSpeed) + (extraDistanceAtStart * acceleration); +} + +// Calculate the additional acceleration distance needed if we shape the end of acceleration +inline float AxisShaper::GetExtraAccelEndDistance(float topSpeed, float acceleration) const noexcept +{ + return (extraClocksAtEnd * topSpeed) + (extraDistanceAtEnd * acceleration); +} + +inline float AxisShaper::GetExtraDecelStartDistance(float topSpeed, float deceleration) const noexcept +{ + return (extraClocksAtStart * topSpeed) - (extraDistanceAtStart * deceleration); +} + +// Calculate the additional deceleration distance needed if we shape the end of deceleration +inline float AxisShaper::GetExtraDecelEndDistance(float endSpeed, float deceleration) const noexcept +{ + return (extraClocksAtEnd * endSpeed) - (extraDistanceAtEnd * deceleration); +} + +/*static*/ MoveSegment *AxisShaper::GetUnshapedSegments(DDA& dda, const PrepParams& params) noexcept +{ + // Deceleration phase + MoveSegment * tempSegments; + if (params.unshaped.decelClocks > 0.0) + { + tempSegments = MoveSegment::Allocate(nullptr); + const float b = dda.topSpeed/params.unshaped.deceleration; + const float c = -2.0/params.unshaped.deceleration; + tempSegments->SetNonLinear(dda.totalDistance - params.unshaped.decelStartDistance, params.unshaped.decelClocks, b, c); + } + else + { + tempSegments = nullptr; + } + + // Steady speed phase + if (params.unshaped.steadyClocks > 0.0) + { + tempSegments = MoveSegment::Allocate(tempSegments); + const float c = 1.0/dda.topSpeed; + tempSegments->SetLinear(params.unshaped.decelStartDistance - params.unshaped.accelDistance, params.unshaped.steadyClocks, c); + } + + // Acceleration phase + if (params.unshaped.accelClocks > 0.0) + { + tempSegments = MoveSegment::Allocate(tempSegments); + const float b = dda.startSpeed/(-params.unshaped.acceleration); + const float c = 2.0/params.unshaped.acceleration; + tempSegments->SetNonLinear(params.unshaped.accelDistance, params.unshaped.accelClocks, b, c); + } + + return tempSegments; +} + +// End diff --git a/src/Movement/AxisShaper.h b/src/Movement/AxisShaper.h new file mode 100644 index 00000000..02aa2b0e --- /dev/null +++ b/src/Movement/AxisShaper.h @@ -0,0 +1,95 @@ +/* + * InputShaper.h + * + * Created on: 20 Feb 2021 + * Author: David + */ + +#ifndef SRC_MOVEMENT_AXISSHAPER_H_ +#define SRC_MOVEMENT_AXISSHAPER_H_ + +#define SUPPORT_DAA (1) + +#include <RepRapFirmware.h> +#include <General/NamedEnum.h> +#include <ObjectModel/ObjectModel.h> +#include "InputShaperPlan.h" + +// These names must be in alphabetical order and lowercase +NamedEnum(InputShaperType, uint8_t, + custom, +#if SUPPORT_DAA + daa, +#endif + ei2, + ei3, + none, + zvd, + zvdd, +); + +class DDA; +class PrepParams; +class MoveSegment; + +class AxisShaper INHERIT_OBJECT_MODEL +{ +public: + AxisShaper() noexcept; + + float GetFrequency() const noexcept { return frequency; } + float GetDamping() const noexcept { return zeta; } + InputShaperType GetType() const noexcept { return type; } + void PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled) const noexcept; + + GCodeResult Configure(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // process M593 + + static MoveSegment *GetUnshapedSegments(DDA& dda, const PrepParams& params) noexcept; + +protected: + DECLARE_OBJECT_MODEL + +private: + MoveSegment *GetAccelerationSegments(const DDA& dda, PrepParams& params) const noexcept; + MoveSegment *GetDecelerationSegments(const DDA& dda, PrepParams& params) const noexcept; + MoveSegment *FinishShapedSegments(const DDA& dda, const PrepParams& params, MoveSegment *accelSegs, MoveSegment *decelSegs) const noexcept; + float GetExtraAccelStartDistance(float startSpeed, float acceleration) const noexcept; + float GetExtraAccelEndDistance(float topSpeed, float acceleration) const noexcept; + float GetExtraDecelStartDistance(float topSpeed, float deceleration) const noexcept; + float GetExtraDecelEndDistance(float endSpeed, float deceleration) const noexcept; + void TryShapeAccelEnd(const DDA& dda, PrepParams& params) const noexcept; + void TryShapeAccelBoth(DDA& dda, PrepParams& params) const noexcept; + void TryShapeDecelStart(const DDA& dda, PrepParams& params) const noexcept; + void TryShapeDecelBoth(DDA& dda, PrepParams& params) const noexcept; + bool ImplementAccelShaping(const DDA& dda, PrepParams& params, float newAccelDistance, float newAccelClocks) const noexcept; + bool ImplementDecelShaping(const DDA& dda, PrepParams& params, float newDecelStartDistance, float newDecelClocks) const noexcept; + + static constexpr unsigned int MaxExtraImpulses = 4; + static constexpr float DefaultFrequency = 40.0; + static constexpr float DefaultDamping = 0.1; + static constexpr float DefaultMinimumAcceleration = 10.0; + static constexpr float MinimumMiddleSegmentTime = 8.0/1000.0; // minimum length of the segment between shaped start and shaped end of an acceleration or deceleration + + unsigned int numExtraImpulses; // the number of extra impulses + float frequency; // the undamped frequency in Hz + float zeta; // the damping ratio, see https://en.wikipedia.org/wiki/Damping. 0 = undamped, 1 = critically damped. + float minimumAcceleration; // the minimum value that we reduce average acceleration to in mm/sec^2 + float coefficients[MaxExtraImpulses]; // the coefficients of all the impulses + float durations[MaxExtraImpulses]; // the duration in step clocks of each impulse + float totalShapingClocks; // the total input shaping time in step clocks + float minimumShapingStartOriginalClocks; // the minimum acceleration/deceleration time for which we can shape the start, without changing the acceleration/deceleration + float minimumShapingEndOriginalClocks; // the minimum acceleration/deceleration time for which we can shape the start, without changing the acceleration/deceleration + float minimumNonOverlappedOriginalClocks; // the minimum original acceleration or deceleration time using non-overlapped start and end shaping + float extraClocksAtStart; // the extra time needed to shape the start of acceleration or deceleration + float extraClocksAtEnd; // the extra time needed to shape the end of acceleration or deceleration + float extraDistanceAtStart; // the extra distance per unit acceleration to shape the start of acceleration or deceleration, less the initial velocity contribution + float extraDistanceAtEnd; // the extra distance per unit acceleration to shape the end of acceleration or deceleration, less the final velocity contribution + float overlappedDurations[2 * MaxExtraImpulses]; // the duration in step clocks of each impulse of an overlapped acceleration or deceleration + float overlappedCoefficients[2 * MaxExtraImpulses]; // the coefficients if we use a shaped start immediately followed by a shaped end + float overlappedShapingClocks; // the acceleration or deceleration duration when we use overlapping, in step clocks + float overlappedDeltaVPerA; // the effective acceleration time (velocity change per unit acceleration) when we use overlapping, in step clocks + float overlappedDistancePerA; // the distance needed by an overlapped acceleration or deceleration, less the initial velocity contribution + InputShaperType type; +}; + +#endif /* SRC_MOVEMENT_AXISSHAPER_H_ */ diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index 645b31e3..a4c85cab 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -115,9 +115,35 @@ void DDA::LogProbePosition() noexcept #endif +// Set up the parameters from the DDA, excluding steadyClocks because that may be affected by input shaping +void PrepParams::SetFromDDA(const DDA& dda) noexcept +{ + unshaped.decelStartDistance = dda.totalDistance - dda.beforePrepare.decelDistance; + // Due to rounding error, for an accelerate-decelerate move we may have accelDistance+decelDistance slightly greater than totalDistance. + // We need to make sure that accelDistance <= decelStartDistance for subsequent calculations to work. + unshaped.accelDistance = min<float>(dda.beforePrepare.accelDistance, unshaped.decelStartDistance); + unshaped.acceleration = dda.acceleration; + unshaped.deceleration = dda.deceleration; + unshaped.accelClocks = (dda.topSpeed - dda.startSpeed)/dda.acceleration; + unshaped.decelClocks = (dda.topSpeed - dda.endSpeed)/dda.deceleration; + +#if SUPPORT_CAN_EXPANSION + initialSpeedFraction = dda.startSpeed/dda.topSpeed; + finalSpeedFraction = dda.endSpeed/dda.topSpeed; +#endif +} + +// Calculate the steady clocks and set the total clocks in the DDA +void PrepParams::PrepParamSet::Finalise(float topSpeed) noexcept +{ + const float steadyDistance = decelStartDistance - accelDistance; + steadyClocks = (steadyDistance <= 0.0) ? 0.0 : steadyDistance/topSpeed; +} + DDA::DDA(DDA* n) noexcept : next(n), prev(nullptr), state(empty) { activeDMs = completedDMs = nullptr; + shapedSegments = unshapedSegments = nullptr; tool = nullptr; // needed in case we pause before any moves have been done // Set the endpoints to zero, because Move will ask for them. @@ -127,7 +153,7 @@ DDA::DDA(DDA* n) noexcept : next(n), prev(nullptr), state(empty) ep = 0; } - flags.all = 0; // in particular we need to set endCoordinatesValid to false + flags.all = 0; // in particular we need to set endCoordinatesValid and usePressureAdvance to false virtualExtruderPosition = 0.0; filePos = noFilePosition; @@ -152,6 +178,20 @@ void DDA::ReleaseDMs() noexcept dm = dnext; } activeDMs = completedDMs = nullptr; + + for (MoveSegment* seg = shapedSegments; seg != nullptr; ) + { + MoveSegment* const nextSeg = seg->GetNext(); + MoveSegment::Release(seg); + seg = nextSeg; + } + for (MoveSegment* seg = unshapedSegments; seg != nullptr; ) + { + MoveSegment* const nextSeg = seg->GetNext(); + MoveSegment::Release(seg); + seg = nextSeg; + } + shapedSegments = unshapedSegments = nullptr; } // Return the number of clocks this DDA still needs to execute. @@ -224,13 +264,18 @@ void DDA::DebugPrint(const char *tag) const noexcept DebugPrintVector(" end", endCoordinates, numAxes); } - debugPrintf(" s=%f", (double)totalDistance); - DebugPrintVector(" vec", directionVector, 5); - debugPrintf("\n" - "a=%f d=%f reqv=%f startv=%f topv=%f endv=%f\n" - "cks=%" PRIu32 " sstcda=%" PRIu32 " tstcddpdsc=%" PRIu32 " exac=%" PRIi32 "\n", - (double)acceleration, (double)deceleration, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed, clocksNeeded, - afterPrepare.startSpeedTimesCdivA, afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks, afterPrepare.extraAccelerationClocks); + debugPrintf(" s=%.4e", (double)totalDistance); + DebugPrintVector(" vec", directionVector, MaxAxesPlusExtruders); + debugPrintf("\n" "a=%.4e d=%.4e reqv=%.4e startv=%.4e topv=%.4e endv=%.4e cks=%" PRIu32 " fp=%" PRIu32 " fl=%04x\n", + (double)acceleration, (double)deceleration, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed, clocksNeeded, (uint32_t)filePos, flags.all); + for (const MoveSegment *segs = shapedSegments; segs != nullptr; segs = segs->GetNext()) + { + segs->DebugPrint('S'); + } + for (const MoveSegment *segs = unshapedSegments; segs != nullptr; segs = segs->GetNext()) + { + segs->DebugPrint('U'); + } } // Print the DDA and active DMs @@ -342,11 +387,11 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM } if (flags.xyMoving && nextMove.usePressureAdvance) { - const float compensationTime = reprap.GetPlatform().GetPressureAdvance(LogicalDriveToExtruder(drive)); - if (compensationTime > 0.0) + const float compensationClocks = reprap.GetMove().GetPressureAdvanceClocks(LogicalDriveToExtruder(drive)); + if (compensationClocks > 0.0) { // Compensation causes instant velocity changes equal to acceleration * k, so we may need to limit the acceleration - accelerations[drive] = min<float>(accelerations[drive], reprap.GetPlatform().GetInstantDv(drive)/compensationTime); + accelerations[drive] = min<float>(accelerations[drive], reprap.GetPlatform().GetInstantDv(drive)/compensationClocks); } } } @@ -635,57 +680,149 @@ bool DDA::InitAsyncMove(DDARing& ring, const AsyncMove& nextMove) noexcept #if SUPPORT_REMOTE_COMMANDS // Set up a remote move. Return true if it represents real movement, else false. +// All values have already been converted to step clocks and the total distance has been normalised to 1.0. +//TODO pass the input shaping plan in the message. For now we don't use input shaping. +# if USE_REMOTE_INPUT_SHAPING +bool DDA::InitShapedFromRemote(const CanMessageMovementLinearShaped& msg) noexcept +# else bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept +#endif { afterPrepare.moveStartTime = StepTimer::ConvertToLocalTime(msg.whenToExecute); clocksNeeded = msg.accelerationClocks + msg.steadyClocks + msg.decelClocks; flags.all = 0; flags.isRemote = true; +# if !USE_REMOTE_INPUT_SHAPING flags.isPrintingMove = (msg.pressureAdvanceDrives != 0); +# endif // Normalise the move to unit distance and convert time units from step clocks to seconds totalDistance = 1.0; - topSpeed = (2.0 * StepTimer::StepClockRate)/(2 * msg.steadyClocks + (msg.initialSpeedFraction + 1.0) * msg.accelerationClocks + (msg.finalSpeedFraction + 1.0) * msg.decelClocks); + topSpeed = 2.0/(2 * msg.steadyClocks + (msg.initialSpeedFraction + 1.0) * msg.accelerationClocks + (msg.finalSpeedFraction + 1.0) * msg.decelClocks); startSpeed = topSpeed * msg.initialSpeedFraction; endSpeed = topSpeed * msg.finalSpeedFraction; - acceleration = (msg.accelerationClocks == 0) ? 0.0 : (topSpeed * (1.0 - msg.initialSpeedFraction) * StepTimer::StepClockRate)/msg.accelerationClocks; - deceleration = (msg.decelClocks == 0) ? 0.0 : (topSpeed * (1.0 - msg.finalSpeedFraction) * StepTimer::StepClockRate)/msg.decelClocks; + acceleration = (msg.accelerationClocks == 0) ? 0.0 : (topSpeed * (1.0 - msg.initialSpeedFraction))/msg.accelerationClocks; + deceleration = (msg.decelClocks == 0) ? 0.0 : (topSpeed * (1.0 - msg.finalSpeedFraction))/msg.decelClocks; PrepParams params; - params.accelDistance = topSpeed * (1.0 + msg.initialSpeedFraction) * msg.accelerationClocks/(2 * StepTimer::StepClockRate); - params.decelDistance = topSpeed * (1.0 + msg.finalSpeedFraction) * msg.decelClocks/(2 * StepTimer::StepClockRate); - params.decelStartDistance = 1.0 - params.decelDistance; - - afterPrepare.startSpeedTimesCdivA = (uint32_t)roundU32(startSpeed/acceleration); -#if DM_USE_FPU - params.fTopSpeedTimesCdivD = topSpeed/deceleration; - afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks = (uint32_t)(params.fTopSpeedTimesCdivD) + msg.accelerationClocks + msg.steadyClocks; -#else - params.topSpeedTimesCdivD = (uint32_t)roundU32(topSpeed/deceleration); - afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks = params.topSpeedTimesCdivD + msg.accelerationClocks + msg.steadyClocks; -#endif - afterPrepare.extraAccelerationClocks = msg.accelerationClocks - roundS32(params.accelDistance/topSpeed); - params.accelCompFactor = (topSpeed - startSpeed)/topSpeed; - - activeDMs = nullptr; + params.unshaped.accelDistance = topSpeed * (1.0 + msg.initialSpeedFraction) * msg.accelerationClocks * 0.5; + const float decelDistance = topSpeed * (1.0 + msg.finalSpeedFraction) * msg.decelClocks * 0.5; + params.unshaped.decelStartDistance = 1.0 - decelDistance; + params.unshaped.accelClocks = msg.accelerationClocks; + params.unshaped.steadyClocks = msg.steadyClocks; + params.unshaped.decelClocks = msg.decelClocks; + + shapedSegments = unshapedSegments = nullptr; + activeDMs = completedDMs = nullptr; +# if USE_REMOTE_INPUT_SHAPING + const size_t numDrivers = min<size_t>(msg.numDriversMinusOne + 1, min<size_t>(NumDirectDrivers, MaxLinearDriversPerCanSlave)); +# else const size_t numDrivers = min<size_t>(msg.numDrivers, min<size_t>(NumDirectDrivers, MaxLinearDriversPerCanSlave)); +# endif for (size_t drive = 0; drive < numDrivers; drive++) { - endPoint[drive] = prev->endPoint[drive]; // the steps for this move will be added later - const int32_t delta = msg.perDrive[drive].steps; + endPoint[drive] = prev->endPoint[drive]; // the steps for this move will be added later +# if USE_REMOTE_INPUT_SHAPING + switch (msg.GetMoveType(drive)) + { + case CanMessageMovementLinearShaped::shapedAxis: + // Note, drivers that are not moving are set to shapedAxis, so we need to check that the driver really is moving + { + const int32_t delta = msg.perDrive[drive].iSteps; + if (delta != 0) + { + if (shapedSegments == nullptr) + { + // Calculate the segments needed for axis movement + //TODO the message will include input shaping info + shapedSegments = AxisShaper::GetUnshapedSegments(*this, params); + } + + DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); + pdm->totalSteps = labs(delta); // for now this is the number of net steps, but gets adjusted later if there is a reverse in direction + pdm->direction = (delta >= 0); // for now this is the direction of net movement, but gets adjusted later if it is a delta movement + + reprap.GetPlatform().EnableDrivers(drive); + const bool stepsToDo = pdm->PrepareCartesianAxis(*this, params); + if (stepsToDo) + { + pdm->directionChanged = false; + InsertDM(pdm); + const uint32_t netSteps = (pdm->reverseStartStep < pdm->totalSteps) ? (2 * pdm->reverseStartStep) - pdm->totalSteps : pdm->totalSteps; + if (pdm->direction) + { + endPoint[drive] += netSteps; + } + else + { + endPoint[drive] -= netSteps; + } + + // Check for sensible values, print them if they look dubious + if (reprap.Debug(moduleDda) && pdm->totalSteps > 1000000) + { + DebugPrintAll("rem"); + } + } + else + { + DriveMovement::Release(pdm); + } + } + } + break; + + case CanMessageMovementLinearShaped::shapedDelta: + { + if (shapedSegments == nullptr) + { + // Calculate the segments needed for axis movement + //TODO the message will include input shaping info + shapedSegments = AxisShaper::GetUnshapedSegments(*this, params); + } + + //TODO + } + break; + case CanMessageMovementLinearShaped::extruderWithPa: + flags.usePressureAdvance = true; // either all extruder use pressure advance, or none do + // no break + case CanMessageMovementLinearShaped::MoveType::extruderNoPa: + { + if (unshapedSegments != nullptr) + { + unshapedSegments = AxisShaper::GetUnshapedSegments(*this, params); + } + if (unshapedSegments != nullptr) + { + DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); + pdm->PrepareExtruder(*this, params); + InsertDM(pdm); + } + } + break; + } + } +# else + const int32_t delta = msg.perDrive[drive].steps; if (delta != 0) { - DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::accel0); + if (shapedSegments == nullptr) + { + EnsureUnshapedSegments(params); + } + + DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); pdm->totalSteps = labs(delta); // for now this is the number of net steps, but gets adjusted later if there is a reverse in direction pdm->direction = (delta >= 0); // for now this is the direction of net movement, but gets adjusted later if it is a delta movement reprap.GetPlatform().EnableDrivers(drive); const bool stepsToDo = ((msg.pressureAdvanceDrives & (1u << drive)) != 0) - ? pdm->PrepareRemoteExtruder(*this, params) + ? pdm->PrepareExtruder(*this, params) : pdm->PrepareCartesianAxis(*this, params); if (stepsToDo) { @@ -706,7 +843,6 @@ bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept { DebugPrintAll("rem"); } - } else { @@ -714,6 +850,7 @@ bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept } } } +# endif // 2. Throw it away if there's no real movement. if (activeDMs == nullptr) @@ -734,7 +871,7 @@ bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept // Return true if this move is or might have been intended to be a deceleration-only move // A move planned as a deceleration-only move may have a short acceleration segment at the start because of rounding error -inline bool DDA::IsDecelerationMove() const noexcept +bool DDA::IsDecelerationMove() const noexcept { return beforePrepare.decelDistance == totalDistance // the simple case - is a deceleration-only move || (topSpeed < requestedSpeed // can't have been intended as deceleration-only if it reaches the requested speed @@ -744,7 +881,7 @@ inline bool DDA::IsDecelerationMove() const noexcept // Return true if this move is or might have been intended to be a deceleration-only move // A move planned as a deceleration-only move may have a short acceleration segment at the start because of rounding error -inline bool DDA::IsAccelerationMove() const noexcept +bool DDA::IsAccelerationMove() const noexcept { return beforePrepare.accelDistance == totalDistance // the simple case - is an acceleration-only move || (topSpeed < requestedSpeed // can't have been intended as deceleration-only if it reaches the requested speed @@ -1049,7 +1186,7 @@ void DDA::RecalculateMove(DDARing& ring) noexcept const float totalTime = (topSpeed - startSpeed)/acceleration + (topSpeed - endSpeed)/deceleration + (totalDistance - beforePrepare.accelDistance - beforePrepare.decelDistance)/topSpeed; - clocksNeeded = (uint32_t)(totalTime * StepTimer::StepClockRate); + clocksNeeded = (uint32_t)totalTime; } // Decide what speed we would really like this move to end at. @@ -1130,146 +1267,21 @@ pre(disableDeltaMapping || drive < MaxAxes) } } -// Adjust the acceleration and deceleration to reduce ringing -// This is only called once, so inlined for speed -inline void DDA::AdjustAcceleration() noexcept +// Set up unshapedSegments if we haven't done so already +void DDA::EnsureUnshapedSegments(const PrepParams& params) noexcept { - // Try to reduce the acceleration/deceleration of the move to cancel ringing - const float idealPeriod = reprap.GetMove().GetShaper().GetFullPeriod(); - - float proposedAcceleration = acceleration, proposedAccelDistance = beforePrepare.accelDistance; - bool adjustAcceleration = false; - if (topSpeed > startSpeed && ((prev->state != DDAState::frozen && prev->state != DDAState::executing) || !prev->flags.wasAccelOnlyMove)) + if (unshapedSegments == nullptr) { - const float accelTime = (topSpeed - startSpeed)/acceleration; - if (accelTime < idealPeriod) - { - proposedAcceleration = (topSpeed - startSpeed)/idealPeriod; - adjustAcceleration = true; - } - else if (accelTime < idealPeriod * 2) - { - proposedAcceleration = (topSpeed - startSpeed)/(idealPeriod * 2); - adjustAcceleration = true; - } - if (adjustAcceleration) - { - proposedAccelDistance = (fsquare(topSpeed) - fsquare(startSpeed))/(2 * proposedAcceleration); - } - } - - float proposedDeceleration = deceleration, proposedDecelDistance = beforePrepare.decelDistance; - bool adjustDeceleration = false; - if (topSpeed > endSpeed && (next->state != DDAState::provisional || !next->IsDecelerationMove())) - { - const float decelTime = (topSpeed - endSpeed)/deceleration; - if (decelTime < idealPeriod) - { - proposedDeceleration = (topSpeed - endSpeed)/idealPeriod; - adjustDeceleration = true; - } - else if (decelTime < idealPeriod * 2) - { - proposedDeceleration = (topSpeed - endSpeed)/(idealPeriod * 2); - adjustDeceleration = true; - } - if (adjustDeceleration) - { - proposedDecelDistance = (fsquare(topSpeed) - fsquare(endSpeed))/(2 * proposedDeceleration); - } - } - - if (adjustAcceleration || adjustDeceleration) - { - const float drcMinimumAcceleration = reprap.GetMove().GetShaper().GetMinimumAcceleration(); - if (proposedAccelDistance + proposedDecelDistance <= totalDistance) - { - if (proposedAcceleration < drcMinimumAcceleration || proposedDeceleration < drcMinimumAcceleration) - { - return; - } - acceleration = proposedAcceleration; - deceleration = proposedDeceleration; - beforePrepare.accelDistance = proposedAccelDistance; - beforePrepare.decelDistance = proposedDecelDistance; - } - else - { - // We can't keep this as a trapezoidal move with the original top speed. - // Try an accelerate-decelerate move with acceleration and deceleration times equal to the ideal period. - const float twiceTotalDistance = 2 * totalDistance; - float proposedTopSpeed = totalDistance/idealPeriod - (startSpeed + endSpeed)/2; - if (proposedTopSpeed > startSpeed && proposedTopSpeed > endSpeed) - { - proposedAcceleration = (twiceTotalDistance - ((3 * startSpeed + endSpeed) * idealPeriod))/(2 * fsquare(idealPeriod)); - proposedDeceleration = (twiceTotalDistance - ((startSpeed + 3 * endSpeed) * idealPeriod))/(2 * fsquare(idealPeriod)); - if ( proposedAcceleration < drcMinimumAcceleration || proposedDeceleration < drcMinimumAcceleration - || proposedAcceleration > acceleration || proposedDeceleration > deceleration - ) - { - return; - } - topSpeed = proposedTopSpeed; - acceleration = proposedAcceleration; - deceleration = proposedDeceleration; - beforePrepare.accelDistance = startSpeed * idealPeriod + (acceleration * fsquare(idealPeriod))/2; - beforePrepare.decelDistance = endSpeed * idealPeriod + (deceleration * fsquare(idealPeriod))/2; - } - else if (startSpeed < endSpeed) - { - // Change it into an accelerate-only move, accelerating as slowly as we can - proposedAcceleration = (fsquare(endSpeed) - fsquare(startSpeed))/twiceTotalDistance; - if (proposedAcceleration < drcMinimumAcceleration) - { - return; // avoid very small accelerations because they can be problematic - } - acceleration = proposedAcceleration; - topSpeed = endSpeed; - beforePrepare.accelDistance = totalDistance; - beforePrepare.decelDistance = 0.0; - } - else if (startSpeed > endSpeed) - { - // Change it into a decelerate-only move, decelerating as slowly as we can - proposedDeceleration = (fsquare(startSpeed) - fsquare(endSpeed))/twiceTotalDistance; - if (proposedDeceleration < drcMinimumAcceleration) - { - return; // avoid very small accelerations because they can be problematic - } - deceleration = proposedDeceleration; - topSpeed = startSpeed; - beforePrepare.accelDistance = 0.0; - beforePrepare.decelDistance = totalDistance; - } - else - { - // Start and end speeds are exactly the same, possibly zero, so give up trying to adjust this move - return; - } - } - - const float totalTime = (topSpeed - startSpeed)/acceleration - + (topSpeed - endSpeed)/deceleration - + (totalDistance - beforePrepare.accelDistance - beforePrepare.decelDistance)/topSpeed; - clocksNeeded = (uint32_t)(totalTime * StepTimer::StepClockRate); - if (reprap.Debug(moduleMove)) - { - debugPrintf("New a=%.1f d=%.1f\n", (double)acceleration, (double)deceleration); - } + unshapedSegments = AxisShaper::GetUnshapedSegments(*this, params); } } // Prepare this DDA for execution. // This must not be called with interrupts disabled, because it calls Platform::EnableDrive. -void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept +void DDA::Prepare(uint8_t simMode) noexcept { flags.wasAccelOnlyMove = IsAccelerationMove(); // save this for the next move to look at - if (flags.xyMoving && reprap.GetMove().GetShaper().GetType() == InputShaperType::daa) - { - AdjustAcceleration(); - } - #if SUPPORT_LASER if (topSpeed < requestedSpeed && reprap.GetGCodes().GetMachineType() == MachineType::laser) { @@ -1278,10 +1290,25 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept } #endif - PrepParams params; - params.accelDistance = beforePrepare.accelDistance; - params.decelDistance = beforePrepare.decelDistance; - params.decelStartDistance = totalDistance - beforePrepare.decelDistance; + // Prepare for movement + shapedSegments = unshapedSegments = nullptr; + + PrepParams params; // the default constructor clears params.plan to 'no shaping' + if (flags.xyMoving) + { + reprap.GetMove().GetAxisShaper().PlanShaping(*this, params, flags.xyMoving); // this will set up shapedSegments if we are doing any shaping + } + else + { + params.SetFromDDA(*this); + params.unshaped.Finalise(topSpeed); + clocksNeeded = params.unshaped.TotalClocks(); + } + + // Copy the unshaped acceleration and deceleration back to the DDA because ManageLaserPower uses them + //TODO change ManageLaserPower to work on the shaped segments instead + acceleration = params.unshaped.acceleration; + deceleration = params.unshaped.deceleration; if (simMode == 0) { @@ -1306,30 +1333,6 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept params.dparams = static_cast<const LinearDeltaKinematics*>(&(reprap.GetMove().GetKinematics())); } - // Convert the accelerate/decelerate distances to times - const float accelStopTime = (topSpeed - startSpeed)/acceleration; - const float steadyTime = (params.decelStartDistance - beforePrepare.accelDistance)/topSpeed; -#if SUPPORT_CAN_EXPANSION - params.accelTime = accelStopTime; - params.steadyTime = steadyTime; - params.decelTime = (topSpeed - endSpeed)/deceleration; - params.initialSpeedFraction = startSpeed/topSpeed; - params.finalSpeedFraction = endSpeed/topSpeed; - params.accelCompFactor = 1.0 - params.initialSpeedFraction; -#else - params.accelCompFactor = (topSpeed - startSpeed)/topSpeed; -#endif - const float decelStartTime = accelStopTime + steadyTime; - afterPrepare.startSpeedTimesCdivA = (uint32_t)roundU32((startSpeed * StepTimer::StepClockRate)/acceleration); -#if DM_USE_FPU - params.fTopSpeedTimesCdivD = (topSpeed * StepTimer::StepClockRate)/deceleration; - afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks = roundU32(params.fTopSpeedTimesCdivD + decelStartTime * StepTimer::StepClockRate); -#else - params.topSpeedTimesCdivD = roundU32((topSpeed * StepTimer::StepClockRate)/deceleration); - afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks = params.topSpeedTimesCdivD + (uint32_t)roundU32(decelStartTime * StepTimer::StepClockRate); -#endif - afterPrepare.extraAccelerationClocks = roundS32((accelStopTime - (beforePrepare.accelDistance/topSpeed)) * StepTimer::StepClockRate); - activeDMs = completedDMs = nullptr; #if SUPPORT_CAN_EXPANSION @@ -1360,26 +1363,27 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept { const int32_t delta = lrintf(directionVector[drive] * totalDistance * platform.DriveStepsPerUnit(Z_AXIS)); const DriverId driver = config.driverNumbers[drive]; -#if SUPPORT_CAN_EXPANSION - if (driver.IsRemote()) + if (delta != 0) { - CanMotion::AddMovement(params, driver, delta, false); - } - else +#if SUPPORT_CAN_EXPANSION + if (driver.IsRemote()) + { + CanMotion::AddMovement(params, driver, delta); + } + else #endif - { - if (delta != 0) { - DriveMovement* const pdm = DriveMovement::Allocate(driver.localDriver + MaxAxesPlusExtruders, DMState::accel0); - pdm->totalSteps = labs(delta); + EnsureUnshapedSegments(params); + DriveMovement* const pdm = DriveMovement::Allocate(driver.localDriver + MaxAxesPlusExtruders, DMState::idle); pdm->direction = (delta >= 0); + pdm->totalSteps = labs(delta); if (pdm->PrepareCartesianAxis(*this, params)) { pdm->directionChanged = false; // Check for sensible values, print them if they look dubious if (reprap.Debug(moduleDda) && pdm->totalSteps > 1000000) { - DebugPrintAll("pr"); + DebugPrintAll("pr_err1"); } InsertDM(pdm); } @@ -1395,21 +1399,26 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept } else if (flags.isDeltaMovement && reprap.GetMove().GetKinematics().GetMotionType(drive) == MotionType::segmentFreeDelta) { - platform.EnableDrivers(drive); // On a delta we need to move all towers even if some of them have no net movement + platform.EnableDrivers(drive); + if (shapedSegments == nullptr) + { + EnsureUnshapedSegments(params); + } + const int32_t delta = endPoint[drive] - prev->endPoint[drive]; if (platform.GetDriversBitmap(drive) != 0) // if any of the drives is local { - DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::accel0); - pdm->totalSteps = labs(delta); + DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); pdm->direction = (delta >= 0); + pdm->totalSteps = labs(delta); // this is net steps for now if (pdm->PrepareDeltaAxis(*this, params)) { pdm->directionChanged = false; // Check for sensible values, print them if they look dubious if (reprap.Debug(moduleDda) && pdm->totalSteps > 1000000) { - DebugPrintAll("pt"); + DebugPrintAll("pr_err2"); } InsertDM(pdm); } @@ -1429,7 +1438,7 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept const DriverId driver = config.driverNumbers[i]; if (driver.IsRemote()) { - CanMotion::AddMovement(params, driver, delta, false); + CanMotion::AddMovement(params, driver, delta); } } #endif @@ -1437,11 +1446,15 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept } else if (drive < reprap.GetGCodes().GetTotalAxes()) { - // It's a linear drive + // It's a linear axis int32_t delta = endPoint[drive] - prev->endPoint[drive]; if (delta != 0) { platform.EnableDrivers(drive); + if (shapedSegments == nullptr) + { + EnsureUnshapedSegments(params); + } if (flags.continuousRotationShortcut && reprap.GetMove().GetKinematics().IsContinuousRotationAxis(drive)) { // This is a continuous rotation axis, so we may have adjusted the move to cross the 180 degrees position @@ -1462,16 +1475,16 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept #endif ) { - DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::accel0); - pdm->totalSteps = labs(delta); + DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); pdm->direction = (delta >= 0); + pdm->totalSteps = labs(delta); if (pdm->PrepareCartesianAxis(*this, params)) { pdm->directionChanged = false; // Check for sensible values, print them if they look dubious if (reprap.Debug(moduleDda) && pdm->totalSteps > 1000000) { - DebugPrintAll("pr"); + DebugPrintAll("pr_err3"); } InsertDM(pdm); } @@ -1491,7 +1504,7 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept const DriverId driver = config.driverNumbers[i]; if (driver.IsRemote()) { - CanMotion::AddMovement(params, driver, delta, false); + CanMotion::AddMovement(params, driver, delta); } } #endif @@ -1504,56 +1517,61 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept // It's an extruder drive if (directionVector[drive] != 0.0) { - // If there is any extruder jerk in this move, in theory that means we need to instantly extrude or retract some amount of filament. - // Pass the speed change to PrepareExtruder - float speedChange; - if (flags.usePressureAdvance) - { - const float prevEndSpeed = (prev->flags.usePressureAdvance) ? prev->endSpeed * prev->directionVector[drive] : 0.0; - speedChange = (startSpeed * directionVector[drive]) - prevEndSpeed; - } - else + // Currently, we don't apply input shaping to extruders + platform.EnableDrivers(drive); + const size_t extruder = LogicalDriveToExtruder(drive); +#if SUPPORT_NONLINEAR_EXTRUSION + // Add the nonlinear extrusion correction to totalExtrusion + if (flags.isPrintingMove) { - speedChange = 0.0; + float a, b, limit; + if (platform.GetExtrusionCoefficients(extruder, a, b, limit)) + { + float& dv = directionVector[drive]; + const float averageExtrusionSpeed = (totalDistance * dv)/clocksNeeded; + const float factor = 1.0 + min<float>((averageExtrusionSpeed * a) + (averageExtrusionSpeed * averageExtrusionSpeed * b), limit); + dv *= factor; + } } +#endif - platform.EnableDrivers(drive); - const size_t extruder = LogicalDriveToExtruder(drive); #if SUPPORT_CAN_EXPANSION afterPrepare.drivesMoving.SetBit(drive); const DriverId driver = platform.GetExtruderDriver(extruder); if (driver.IsRemote()) { - const int32_t rawSteps = PrepareRemoteExtruder(drive, extrusionPending[extruder], speedChange); + // This calculation isn't quite right when we use PA and fractional steps accumulate. I will fix it when I change the CAN protocol. + // The MovementLinear message requires the raw step count not adjusted for PA to be passed. The remote board adds the PA. + ExtruderShaper& shaper = reprap.GetMove().GetExtruderShaper(LogicalDriveToExtruder(drive)); + float netMovement = (totalDistance * directionVector[drive]) + shaper.GetExtrusionPending(); + const float stepsPerMm = platform.DriveStepsPerUnit(drive); + const int32_t rawSteps = lrintf(netMovement * stepsPerMm); // we round here instead of truncating to match the old code + if (flags.usePressureAdvance) + { + netMovement += (endSpeed - startSpeed) * directionVector[drive] * shaper.GetKclocks(); + } if (rawSteps != 0) { CanMotion::AddMovement(params, driver, rawSteps, flags.usePressureAdvance); + const int32_t netSteps = (flags.usePressureAdvance) + ? lrintf(netMovement * stepsPerMm) // work out how many steps the remote extruder will take + : rawSteps; + netMovement -= (float)netSteps/stepsPerMm; } + shaper.SetExtrusionPending(netMovement); } else #endif { - DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::accel0); - const bool stepsToDo = pdm->PrepareExtruder(*this, params, extrusionPending[extruder], speedChange, flags.usePressureAdvance); - - if (stepsToDo) + EnsureUnshapedSegments(params); + DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); + pdm->direction = (directionVector[drive] >= 0); + if (pdm->PrepareExtruder(*this, params)) { pdm->directionChanged = false; - // Check for sensible values, print them if they look dubious - if ( reprap.Debug(moduleDda) - && ( pdm->totalSteps > 1000000 - || pdm->reverseStartStep < pdm->mp.cart.decelStartStep - || ( pdm->reverseStartStep <= pdm->totalSteps -#if DM_USE_FPU - && pdm->mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD > pdm->fTwoCsquaredTimesMmPerStepDivD * pdm->reverseStartStep -#else - && pdm->mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD > (int64_t)(pdm->twoCsquaredTimesMmPerStepDivD * pdm->reverseStartStep) -#endif - ) - ) - ) + if (reprap.Debug(moduleDda) && pdm->totalSteps > 1000000) { - DebugPrintAll("pr"); + DebugPrintAll("pr_err4"); } InsertDM(pdm); } @@ -1600,7 +1618,8 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept clocksNeeded = canClocksNeeded; } #endif - if (reprap.Debug(moduleDda) && reprap.Debug(moduleMove)) // temp show the prepared DDA if debug enabled for both modules + + if (reprap.Debug(moduleMove) && (reprap.Debug(moduleDda) || params.shapingPlan.debugPrint)) // show the prepared DDA if debug enabled for both modules { DebugPrintAll("pr"); } @@ -2053,7 +2072,7 @@ void DDA::StepDrivers(Platform& p, uint32_t now) noexcept while (dmToInsert != dm) // note that both of these may be nullptr { DriveMovement * const nextToInsert = dmToInsert->nextDM; - if (dmToInsert->state >= DMState::accel0) + if (dmToInsert->state >= DMState::firstMotionState) { InsertDM(dmToInsert); if (dmToInsert->directionChanged) @@ -2215,64 +2234,15 @@ void DDA::LimitSpeedAndAcceleration(float maxSpeed, float maxAcceleration) noexc } } -#if SUPPORT_CAN_EXPANSION - -// Prepare a remote extruder, returning the number of steps we are going to do before allowing for pressure advance. -// This replicates some of the functionality that DriveMovement::PrepareExtruder does for local extruder drives. -int32_t DDA::PrepareRemoteExtruder(size_t drive, float& extrusionPending, float speedChange) const noexcept -{ - // Calculate the requested extrusion amount and a few other things - float extrusionRequired = totalDistance * directionVector[drive]; - -#if SUPPORT_NONLINEAR_EXTRUSION - // Add the nonlinear extrusion correction to totalExtrusion - if (flags.isPrintingMove) - { - float a, b, limit; - if (reprap.GetPlatform().GetExtrusionCoefficients(LogicalDriveToExtruder(drive), a, b, limit)) - { - const float averageExtrusionSpeed = (extrusionRequired * StepTimer::StepClockRate)/clocksNeeded; - const float factor = 1.0 + min<float>((averageExtrusionSpeed * a) + (averageExtrusionSpeed * averageExtrusionSpeed * b), limit); - extrusionRequired *= factor; - } - } -#endif - - // Add on any fractional extrusion pending from the previous move - extrusionRequired += extrusionPending; - const float rawStepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); - const int32_t originalSteps = lrintf(extrusionRequired * rawStepsPerMm); - int32_t netSteps; - - if (flags.usePressureAdvance && extrusionRequired >= 0.0) - { - // Calculate the pressure advance parameters - const float compensationTime = reprap.GetPlatform().GetPressureAdvance(LogicalDriveToExtruder(drive)); - - // Calculate the net total extrusion to allow for compensation. It may be negative. - const float dv = extrusionRequired/totalDistance; - extrusionRequired += (endSpeed - startSpeed) * compensationTime * dv; - netSteps = lrintf(extrusionRequired * rawStepsPerMm); - } - else - { - netSteps = originalSteps; - } - - extrusionPending = extrusionRequired - (float)netSteps/rawStepsPerMm; - return originalSteps; -} - -#endif - #if SUPPORT_LASER // Manage the laser power. Return the number of ticks until we should be called again, or 0 to be called at the start of the next move. uint32_t DDA::ManageLaserPower() const noexcept { + Platform& platform = reprap.GetPlatform(); if (!flags.controlLaser || laserPwmOrIoBits.laserPwm == 0) { - reprap.GetPlatform().SetLaserPwm(0); + platform.SetLaserPwm(0); return 0; } @@ -2280,39 +2250,38 @@ uint32_t DDA::ManageLaserPower() const noexcept if (clocksMoving >= clocksNeeded) // this also covers the case of now < startTime { // Something has gone wrong with the timing. Set zero laser power, but try again soon. - reprap.GetPlatform().SetLaserPwm(0); + platform.SetLaserPwm(0); return LaserPwmIntervalMillis; } - const float timeMoving = (float)clocksMoving * (1.0/(float)StepTimer::StepClockRate); - const float accelSpeed = startSpeed + acceleration * timeMoving; + const float accelSpeed = startSpeed + acceleration * clocksMoving; if (accelSpeed < topSpeed) { // Acceleration phase const Pwm_t pwm = (Pwm_t)((accelSpeed/topSpeed) * laserPwmOrIoBits.laserPwm); - reprap.GetPlatform().SetLaserPwm(pwm); + platform.SetLaserPwm(pwm); return LaserPwmIntervalMillis; } const uint32_t clocksLeft = clocksNeeded - clocksMoving; - const float decelSpeed = endSpeed + deceleration * (float)clocksLeft * (1.0/(float)StepTimer::StepClockRate); + const float decelSpeed = endSpeed + deceleration * clocksLeft; if (decelSpeed < topSpeed) { // Deceleration phase const Pwm_t pwm = (Pwm_t)((decelSpeed/topSpeed) * laserPwmOrIoBits.laserPwm); - reprap.GetPlatform().SetLaserPwm(pwm); + platform.SetLaserPwm(pwm); return LaserPwmIntervalMillis; } // We must be in the constant speed phase - reprap.GetPlatform().SetLaserPwm(laserPwmOrIoBits.laserPwm); - const uint32_t decelClocks = ((topSpeed - endSpeed)/deceleration) * StepTimer::StepClockRate; + platform.SetLaserPwm(laserPwmOrIoBits.laserPwm); + const uint32_t decelClocks = (topSpeed - endSpeed)/deceleration; if (clocksLeft <= decelClocks) { return LaserPwmIntervalMillis; } const uint32_t clocksToDecel = clocksLeft - decelClocks; - return lrintf((float)clocksToDecel * StepTimer::StepClocksToMillis) + LaserPwmIntervalMillis; + return lrintf((float)clocksToDecel * StepClocksToMillis) + LaserPwmIntervalMillis; } #endif diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h index 3db44ded..0cffb80d 100644 --- a/src/Movement/DDA.h +++ b/src/Movement/DDA.h @@ -11,6 +11,8 @@ #include <RepRapFirmware.h> #include "DriveMovement.h" #include "StepTimer.h" +#include "MoveSegment.h" +#include "InputShaperPlan.h" #include <Platform/Tasks.h> #include <GCodes/GCodes.h> // for class RawMove @@ -22,10 +24,53 @@ class DDARing; +// Struct for passing parameters to the DriveMovement Prepare methods, also accessed by the input shaper +struct PrepParams +{ + struct PrepParamSet + { + float accelDistance; + float decelStartDistance; + float accelClocks, steadyClocks, decelClocks; + float acceleration, deceleration; + + // Calculate the steady clocks and set the total clocks in the DDA + void Finalise(float topSpeed) noexcept; + + // Get the total clocks needed + float TotalClocks() const noexcept { return accelClocks + steadyClocks + decelClocks; } + }; + + // Parameters used for all types of motion + PrepParamSet unshaped; + PrepParamSet shaped; // only valid if the shaping plan is not empty + InputShaperPlan shapingPlan; + +#if SUPPORT_CAN_EXPANSION + // Parameters used by CAN expansion + float initialSpeedFraction, finalSpeedFraction; +#endif + + // Parameters used only for delta moves + float initialX, initialY; +#if SUPPORT_CAN_EXPANSION + float finalX, finalY; + float zMovement; +#endif + const LinearDeltaKinematics *dparams; + float a2plusb2; // sum of the squares of the X and Y movement fractions + + // Set up the parameters from the DDA, excluding steadyClocks because that may be affected by input shaping + void SetFromDDA(const DDA& dda) noexcept; +}; + // This defines a single coordinated movement of one or several motors class DDA { friend class DriveMovement; + friend class AxisShaper; + friend class ExtruderShaper; + friend class PrepParams; public: @@ -59,7 +104,7 @@ public: void SetPrevious(DDA *p) noexcept { prev = p; } void Complete() noexcept { state = completed; } bool Free() noexcept; - void Prepare(uint8_t simMode, float extrusionPending[]) noexcept SPEED_CRITICAL; // Calculate all the values and freeze this DDA + void Prepare(uint8_t simMode) noexcept SPEED_CRITICAL; // Calculate all the values and freeze this DDA bool HasStepError() const noexcept; bool CanPauseAfter() const noexcept; bool IsPrintingMove() const noexcept { return flags.isPrintingMove; } // Return true if this involves both XY movement and extrusion @@ -78,7 +123,11 @@ public: #endif #if SUPPORT_REMOTE_COMMANDS +# if USE_REMOTE_INPUT_SHAPING + bool InitShapedFromRemote(const CanMessageMovementLinearShaped& msg) noexcept; +# else bool InitFromRemote(const CanMessageMovementLinear& msg) noexcept; +# endif #endif const int32_t *DriveCoordinates() const noexcept { return endPoint; } // Get endpoints of a move in machine coordinates @@ -146,28 +195,25 @@ public: // Note: the above measurements were taken some time ago, before some firmware optimisations. #if SAME70 // Use the same defaults as for the SAM4E for now. - static constexpr uint32_t MinCalcIntervalDelta = (40 * StepTimer::StepClockRate)/1000000; // the smallest sensible interval between calculations (40us) in step timer clocks - static constexpr uint32_t MinCalcIntervalCartesian = (40 * StepTimer::StepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t HiccupTime = (30 * StepTimer::StepClockRate)/1000000; // how long we hiccup for in step timer clocks + static constexpr uint32_t MinCalcInterval = (40 * StepClockRate)/1000000; // same as delta for now, but could be lower + static constexpr uint32_t HiccupTime = (30 * StepClockRate)/1000000; // how long we hiccup for in step timer clocks #elif SAM4E || SAM4S || SAME5x - static constexpr uint32_t MinCalcIntervalDelta = (40 * StepTimer::StepClockRate)/1000000; // the smallest sensible interval between calculations (40us) in step timer clocks - static constexpr uint32_t MinCalcIntervalCartesian = (40 * StepTimer::StepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t HiccupTime = (30 * StepTimer::StepClockRate)/1000000; // how long we hiccup for in step timer clocks + static constexpr uint32_t MinCalcIntervalDelta = (40 * StepClockRate)/1000000; // the smallest sensible interval between calculations (40us) in step timer clocks + static constexpr uint32_t MinCalcInterval = (40 * StepClockRate)/1000000; // same as delta for now, but could be lower + static constexpr uint32_t HiccupTime = (30 * StepClockRate)/1000000; // how long we hiccup for in step timer clocks #elif defined(__LPC17xx__) - static constexpr uint32_t MinCalcIntervalDelta = (40 * StepTimer::StepClockRate)/1000000; // the smallest sensible interval between calculations (40us) in step timer clocks - static constexpr uint32_t MinCalcIntervalCartesian = (40 * StepTimer::StepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t HiccupTime = (30 * StepTimer::StepClockRate)/1000000; // how long we hiccup for in step timer clocks + static constexpr uint32_t MinCalcInterval = (40 * StepClockRate)/1000000; // same as delta for now, but could be lower + static constexpr uint32_t HiccupTime = (30 * StepClockRate)/1000000; // how long we hiccup for in step timer clocks #else // SAM3X - static constexpr uint32_t MinCalcIntervalDelta = (60 * StepTimer::StepClockRate)/1000000; // the smallest sensible interval between calculations (60us) in step timer clocks - static constexpr uint32_t MinCalcIntervalCartesian = (60 * StepTimer::StepClockRate)/1000000; // same as delta for now, but could be lower - static constexpr uint32_t HiccupTime = (40 * StepTimer::StepClockRate)/1000000; // how long we hiccup for in step timer clocks + static constexpr uint32_t MinCalcInterval = (60 * StepClockRate)/1000000; // same as delta for now, but could be lower + static constexpr uint32_t HiccupTime = (40 * StepClockRate)/1000000; // how long we hiccup for in step timer clocks #endif - static constexpr uint32_t MaxStepInterruptTime = 10 * StepTimer::MinInterruptInterval; // the maximum time we spend looping in the ISR , in step clocks - static constexpr uint32_t WakeupTime = (100 * StepTimer::StepClockRate)/1000000; // stop resting 100us before the move is due to end - static constexpr uint32_t HiccupIncrement = HiccupTime/2; // how much we increase the hiccup time by on each attempt + static constexpr uint32_t MaxStepInterruptTime = 10 * StepTimer::MinInterruptInterval; // the maximum time we spend looping in the ISR , in step clocks + static constexpr uint32_t WakeupTime = (100 * StepClockRate)/1000000; // stop resting 100us before the move is due to end + static constexpr uint32_t HiccupIncrement = HiccupTime/2; // how much we increase the hiccup time by on each attempt - static constexpr uint32_t UsualMinimumPreparedTime = StepTimer::StepClockRate/10; // 100ms - static constexpr uint32_t AbsoluteMinimumPreparedTime = StepTimer::StepClockRate/20; // 50ms + static constexpr uint32_t UsualMinimumPreparedTime = StepClockRate/10; // 100ms + static constexpr uint32_t AbsoluteMinimumPreparedTime = StepClockRate/20; // 50ms #if DDA_LOG_PROBE_CHANGES static const size_t MaxLoggedProbePositions = 40; @@ -193,8 +239,8 @@ private: void ReleaseDMs() noexcept; bool IsDecelerationMove() const noexcept; // return true if this move is or have been might have been intended to be a deceleration-only move bool IsAccelerationMove() const noexcept; // return true if this move is or have been might have been intended to be an acceleration-only move + void EnsureUnshapedSegments(const PrepParams& params) noexcept; void DebugPrintVector(const char *name, const float *vec, size_t len) const noexcept; - void AdjustAcceleration() noexcept; // Adjust the acceleration and deceleration to reduce ringing #if SUPPORT_CAN_EXPANSION int32_t PrepareRemoteExtruder(size_t drive, float& extrusionPending, float speedChange) const noexcept; @@ -229,7 +275,7 @@ private: xyMoving : 1, // True if movement along an X axis or the Y axis was requested, even it if's too small to do isLeadscrewAdjustmentMove : 1, // True if this is a leadscrews adjustment move usingStandardFeedrate : 1, // True if this move uses the standard feed rate - isNonPrintingExtruderMove : 1, // True if this move is a fast extruder-only move, probably a retract/re-prime + isNonPrintingExtruderMove : 1, // True if this move is an extruder-only move, or involves reverse extrusion (and possibly axis movement too) continuousRotationShortcut : 1, // True if continuous rotation axes take shortcuts checkEndstops : 1, // True if this move monitors endstops or Z probe controlLaser : 1, // True if this move controls the laser or iobits @@ -281,19 +327,15 @@ private: struct { // These are calculated from the above and used in the ISR, so they are set up by Prepare() - uint32_t moveStartTime; // clock count at which the move is due to start (before execution) or was started (during execution) - uint32_t startSpeedTimesCdivA; // the number of clocks it would have taken to reach the start speed from rest - uint32_t topSpeedTimesCdivDPlusDecelStartClocks; - int32_t extraAccelerationClocks; // the additional number of clocks needed because we started the move at less than topSpeed. Negative after ReduceHomingSpeed has been called. + uint32_t moveStartTime; // clock count at which the move is due to start (before execution) or was started (during execution) +#if SUPPORT_CAN_EXPANSION + DriversBitmap drivesMoving; // bitmap of logical drives moving - needed to keep track of whether remote drives are moving + static_assert(MaxAxesPlusExtruders <= DriversBitmap::MaxBits()); +#endif // These are used only in delta calculations #if !DM_USE_FPU - int32_t cKc; // The Z movement fraction multiplied by Kc and converted to integer -#endif - -#if SUPPORT_CAN_EXPANSION - DriversBitmap drivesMoving; // bitmap of logical drives moving - needed to keep track of whether remote drives are moving - static_assert(MaxAxesPlusExtruders <= sizeof(drivesMoving) * CHAR_BIT); + int32_t cKc; // The Z movement fraction multiplied by Kc and converted to integer #endif } afterPrepare; }; @@ -304,8 +346,11 @@ private: void LogProbePosition() noexcept; #endif - DriveMovement* activeDMs; // list of associated DMs that need steps, in step time order - DriveMovement* completedDMs; // list of associated DMs that don't need any more steps + // These three could possibly be moved into afterPrepare + DriveMovement* activeDMs; // list of associated DMs that need steps, in step time order + DriveMovement* completedDMs; // list of associated DMs that don't need any more steps + MoveSegment* shapedSegments; // linked list of move segments used by axis DMs + MoveSegment* unshapedSegments; // linked list of move segments used by extruder DMs }; // Find the DriveMovement record for a given drive even if it is completed, or return nullptr if there isn't one diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp index 11108208..c190c6da 100644 --- a/src/Movement/DDARing.cpp +++ b/src/Movement/DDARing.cpp @@ -15,6 +15,8 @@ # include "CAN/CanMotion.h" #endif +constexpr uint32_t MoveStartPollInterval = 10; // delay in milliseconds between checking whether we should start moves + // Object model table and functions // Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17. // Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM. @@ -85,7 +87,6 @@ void DDARing::Init2() noexcept for (size_t i = 0; i < MaxExtruders; ++i) { extrusionAccumulators[i] = 0; - extrusionPending[i] = 0.0; } extrudersPrinting = false; simulationTime = 0.0; @@ -213,7 +214,7 @@ bool DDARing::CanAddMove() const noexcept prevMoveTime = dda->GetClocksNeeded(); } - return (unPreparedTime < StepTimer::StepClockRate/2 || unPreparedTime + prevMoveTime < 2 * StepTimer::StepClockRate); + return (unPreparedTime < StepClockRate/2 || unPreparedTime + prevMoveTime < 2 * StepClockRate); } return false; } @@ -258,8 +259,9 @@ bool DDARing::AddAsyncMove(const AsyncMove& nextMove) noexcept #endif -// Try to process moves in the ring -void DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept +// Try to process moves in the ring. Called by the Move task. +// Return the maximum time in milliseconds that should elapse before we prepare further unprepared moves that are already in the ring, or TaskBase::TimeoutUnlimited if there are no unprepared moves left. +uint32_t DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept { DDA *cdda = currentDda; // capture volatile variable @@ -267,7 +269,7 @@ void DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept // Do this here rather than at the end, so that when simulating, currentDda is non-null for most of the time and IsExtruding() returns the correct value if (simulationMode != 0 && cdda != nullptr) { - simulationTime += (float)cdda->GetClocksNeeded()/StepTimer::StepClockRate; + simulationTime += (float)cdda->GetClocksNeeded() * (1.0/StepClockRate); cdda->Complete(); CurrentMoveCompleted(); cdda = currentDda; @@ -287,86 +289,100 @@ void DDARing::Spin(uint8_t simulationMode, bool shouldStartMove) noexcept cdda = cdda->GetNext(); if (cdda == addPointer) { - return; // all moves are already prepared + return TaskBase::TimeoutUnlimited; // all the moves we have are already prepared, so nothing to do until new moves arrive } } - PrepareMoves(cdda, preparedTime, preparedCount, simulationMode); + return PrepareMoves(cdda, preparedTime, preparedCount, simulationMode); } - else - { - // No DDA is executing, so start executing a new one if possible - DDA * dda = getPointer; // capture volatile variable - if ( shouldStartMove // if the Move code told us that we should start a move... - || waitingForRingToEmpty // ...or GCodes is waiting for all moves to finish... - || !CanAddMove() // ...or the ring is full... + + // No DDA is executing, so start executing a new one if possible + DDA * dda = getPointer; // capture volatile variable + if ( shouldStartMove // if the Move code told us that we should start a move... + || waitingForRingToEmpty // ...or GCodes is waiting for all moves to finish... + || !CanAddMove() // ...or the ring is full... #if SUPPORT_REMOTE_COMMANDS - || dda->GetState() == DDA::frozen // ...or the move has already been frozen (it's probably a remote move) + || dda->GetState() == DDA::frozen // ...or the move has already been frozen (it's probably a remote move) #endif - ) - { - PrepareMoves(dda, 0, 0, simulationMode); + ) + { + const uint32_t ret = PrepareMoves(dda, 0, 0, simulationMode); - if (dda->GetState() == DDA::completed) + if (dda->GetState() == DDA::completed) + { + // We prepared the move but found there was nothing to do because endstops are already triggered + getPointer = dda = dda->GetNext(); + completedMoves++; + } + else if (dda->GetState() == DDA::frozen) + { + if (simulationMode != 0) { - // We prepared the move but found there was nothing to do because endstops are already triggered - getPointer = dda = dda->GetNext(); - completedMoves++; + currentDda = dda; // pretend we are executing this move } - else if (dda->GetState() == DDA::frozen) + else { - if (simulationMode != 0) + Platform& p = reprap.GetPlatform(); + SetBasePriority(NvicPriorityStep); // shut out step interrupt + const bool wakeLaser = StartNextMove(p, StepTimer::GetTimerTicks()); + if (ScheduleNextStepInterrupt()) { - currentDda = dda; // pretend we are executing this move + Interrupt(p); } - else - { - Platform& p = reprap.GetPlatform(); - SetBasePriority(NvicPriorityStep); // shut out step interrupt - const bool wakeLaser = StartNextMove(p, StepTimer::GetTimerTicks()); - if (ScheduleNextStepInterrupt()) - { - Interrupt(p); - } - SetBasePriority(0); + SetBasePriority(0); #if SUPPORT_LASER || SUPPORT_IOBITS - if (wakeLaser) - { - Move::WakeLaserTask(); - } - else - { - p.SetLaserPwm(0); - } + if (wakeLaser) + { + Move::WakeLaserTask(); + } + else + { + p.SetLaserPwm(0); + } #else - (void)wakeLaser; + (void)wakeLaser; #endif - } } } + return ret; } + + return (dda->GetState() == DDA::provisional) + ? MoveStartPollInterval // there are moves in the queue but it is not time to prepare them yet + : TaskBase::TimeoutUnlimited; // the queue is empty, nothing to do until new moves arrive } // Prepare some moves. moveTimeLeft is the total length remaining of moves that are already executing or prepared. -void DDARing::PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept +// Return the maximum time in milliseconds that should elapse before we prepare further unprepared moves that are already in the ring, or TaskBase::TimeoutUnlimited if there are no unprepared moves left. +uint32_t DDARing::PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept { // If the number of prepared moves will execute in less than the minimum time, prepare another move. // Try to avoid preparing deceleration-only moves too early while ( firstUnpreparedMove->GetState() == DDA::provisional && moveTimeLeft < (int32_t)DDA::UsualMinimumPreparedTime // prepare moves one tenth of a second ahead of when they will be needed - && alreadyPrepared * 2 < numDdasInRing // but don't prepare more than half the ring + && alreadyPrepared * 2 < numDdasInRing // but don't prepare more than half the ring, to handle accelerate/decelerate moves in small segments && (firstUnpreparedMove->IsGoodToPrepare() || moveTimeLeft < (int32_t)DDA::AbsoluteMinimumPreparedTime) #if SUPPORT_CAN_EXPANSION && CanMotion::CanPrepareMove() #endif ) { - firstUnpreparedMove->Prepare(simulationMode, extrusionPending); + firstUnpreparedMove->Prepare(simulationMode); moveTimeLeft += firstUnpreparedMove->GetTimeLeft(); ++alreadyPrepared; firstUnpreparedMove = firstUnpreparedMove->GetNext(); } + + // Decide how soon we want to be called again to prepare further moves + if (firstUnpreparedMove->GetState() == DDA::provisional) + { + // There are more moves waiting to be prepared, so ask to be woken up early + const int32_t clocksTillWakeup = moveTimeLeft - (int32_t)DDA::UsualMinimumPreparedTime; // calculate how long before we run out of prepared moves, less the usual advance prepare time + return (clocksTillWakeup <= 0) ? 2 : min<uint32_t>((uint32_t)clocksTillWakeup/(StepClockRate/1000), 2); // wake up at that time, but delay for at least 2 ticks + } + + return TaskBase::TimeoutUnlimited; } // Return true if this DDA ring is idle @@ -906,6 +922,23 @@ uint32_t DDARing::ManageLaserPower() const noexcept #if SUPPORT_REMOTE_COMMANDS +# if USE_REMOTE_INPUT_SHAPING + +// Add a move from the ATE to the movement queue +void DDARing::AddShapedMoveFromRemote(const CanMessageMovementLinearShaped& msg) noexcept +{ + if (addPointer->GetState() == DDA::empty) + { + if (addPointer->InitShapedFromRemote(msg)) + { + addPointer = addPointer->GetNext(); + scheduledMoves++; + } + } +} + +# else + // Add a move from the ATE to the movement queue void DDARing::AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept { @@ -919,6 +952,7 @@ void DDARing::AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept } } +# endif #endif // End diff --git a/src/Movement/DDARing.h b/src/Movement/DDARing.h index 359baeb6..51355512 100644 --- a/src/Movement/DDARing.h +++ b/src/Movement/DDARing.h @@ -29,7 +29,7 @@ public: bool AddAsyncMove(const AsyncMove& nextMove) noexcept; #endif - void Spin(uint8_t simulationMode, bool shouldStartMove) noexcept SPEED_CRITICAL; // Try to process moves in the ring + uint32_t Spin(uint8_t simulationMode, bool shouldStartMove) noexcept SPEED_CRITICAL; // Try to process moves in the ring bool IsIdle() const noexcept; // Return true if this DDA ring is idle uint32_t GetGracePeriod() const noexcept { return gracePeriod; } // Return the minimum idle time, before we should start a move. Better to have a few moves in the queue so that we can do lookahead @@ -87,7 +87,11 @@ public: GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); #if SUPPORT_REMOTE_COMMANDS +# if USE_REMOTE_INPUT_SHAPING + void AddShapedMoveFromRemote(const CanMessageMovementLinearShaped& msg) noexcept; // add a move from the ATE to the movement queue +# else void AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept; // add a move from the ATE to the movement queue +# endif #endif protected: @@ -95,7 +99,7 @@ protected: private: bool StartNextMove(Platform& p, uint32_t startTime) noexcept SPEED_CRITICAL; // Start the next move, returning true if laser or IObits need to be controlled - void PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept; + uint32_t PrepareMoves(DDA *firstUnpreparedMove, int32_t moveTimeLeft, unsigned int alreadyPrepared, uint8_t simulationMode) noexcept; static void TimerCallback(CallbackParameter p) noexcept; @@ -123,7 +127,6 @@ private: unsigned int stepErrors; // count of step errors, for diagnostics float simulationTime; // Print time since we started simulating - float extrusionPending[MaxExtruders]; // Extrusion not done due to rounding to nearest step volatile int32_t extrusionAccumulators[MaxExtruders]; // Accumulated extruder motor steps volatile uint32_t extrudersPrintingSince; // The milliseconds clock time when extrudersPrinting was set to true diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp index 1110989f..4bf8d6f3 100644 --- a/src/Movement/DriveMovement.cpp +++ b/src/Movement/DriveMovement.cpp @@ -53,67 +53,217 @@ DriveMovement::DriveMovement(DriveMovement *next) noexcept : nextDM(next) // Non static members -// Prepare this DM for a Cartesian axis move, returning true if there are steps to do -bool DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& params) noexcept +void DriveMovement::DebugPrint() const noexcept { - const float stepsPerMm = (float)totalSteps/dda.totalDistance; -#if DM_USE_FPU - fTwoCsquaredTimesMmPerStepDivA = (float)((double)(StepTimer::StepClockRateSquared * 2)/((double)stepsPerMm * (double)dda.acceleration)); - fTwoCsquaredTimesMmPerStepDivD = (float)((double)(StepTimer::StepClockRateSquared * 2)/((double)stepsPerMm * (double)dda.deceleration)); -#else - twoCsquaredTimesMmPerStepDivA = roundU64((double)(StepTimer::StepClockRateSquared * 2)/((double)stepsPerMm * (double)dda.acceleration)); - twoCsquaredTimesMmPerStepDivD = roundU64((double)(StepTimer::StepClockRateSquared * 2)/((double)stepsPerMm * (double)dda.deceleration)); -#endif + const char c = (drive < reprap.GetGCodes().GetTotalAxes()) ? reprap.GetGCodes().GetAxisLetters()[drive] : (char)('0' + LogicalDriveToExtruder(drive)); + if (state != DMState::idle) + { + debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 " psl=%" PRIu32 " A=%.4e B=%.4e C=%.4e", + c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, phaseStepLimit, (double)pA, (double)pB, (double)pC); + if (isDelta) + { + debugPrintf(" hmz0s=%.4e minusAaPlusBbTimesS=%.4e dSquaredMinusAsquaredMinusBsquared=%.4e drev=%.4e\n", + (double)mp.delta.fHmz0s, (double)mp.delta.fMinusAaPlusBbTimesS, (double)mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared, (double)mp.delta.reverseStartDistance); + } + else if (isExtruder) + { + debugPrintf(" pa=%" PRIu32 " eed=%.4e ebf=%.4e\n", (uint32_t)mp.cart.pressureAdvanceK, (double)mp.cart.extraExtrusionDistance, (double)mp.cart.extrusionBroughtForwards); + } + else + { + debugPrintf("\n"); + } + } + else + { + debugPrintf("DM%c: not moving\n", c); + } +} - // Acceleration phase parameters - mp.cart.accelStopStep = (uint32_t)(params.accelDistance * stepsPerMm) + 1; - mp.cart.compensationClocks = mp.cart.accelCompensationClocks = 0; +// This is called when currentSegment has just been changed to a new segment. Return true if there is a new segment to execute. +bool DriveMovement::NewCartesianSegment() noexcept +{ + while (true) + { + if (currentSegment == nullptr) + { + return false; + } - // Constant speed phase parameters -#if DM_USE_FPU - fMmPerStepTimesCdivtopSpeed = (float)StepTimer::StepClockRate/(stepsPerMm * dda.topSpeed); -#else - mmPerStepTimesCKdivtopSpeed = roundU32(((float)StepTimer::StepClockRate * K1)/(stepsPerMm * dda.topSpeed)); -#endif + // Work out the movement limit in steps + pC = currentSegment->CalcC(mp.cart.effectiveMmPerStep); + if (currentSegment->IsLinear()) + { + // Set up pB, pC such that for forward motion, time = pB + pC * stepNumber + pB = currentSegment->CalcLinearB(distanceSoFar, timeSoFar); + state = DMState::cartLinear; + } + else + { + // Set up pA, pB, pC such that for forward motion, time = pB + sqrt(pA + pC * stepNumber) + pA = currentSegment->CalcNonlinearA(distanceSoFar); + pB = currentSegment->CalcNonlinearB(timeSoFar); + state = (currentSegment->IsAccelerating()) ? DMState::cartAccel : DMState::cartDecelNoReverse; + } + + distanceSoFar += currentSegment->GetSegmentLength(); + timeSoFar += currentSegment->GetSegmentTime(); + + phaseStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm) + 1; + if (nextStep < phaseStepLimit) + { + return true; + } + + currentSegment = currentSegment->GetNext(); // skip this segment + } +} - // Deceleration phase parameters - // First check whether there is any deceleration at all, otherwise we may get strange results because of rounding errors - if (params.decelDistance * stepsPerMm < 0.5) +// This is called when currentSegment has just been changed to a new segment. Return true if there is a new segment to execute. +bool DriveMovement::NewDeltaSegment(const DDA& dda) noexcept +{ + while (true) { - mp.cart.decelStartStep = totalSteps + 1; -#if DM_USE_FPU - fTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - twoDistanceToStopTimesCsquaredDivD = 0; -#endif + if (currentSegment == nullptr) + { + return false; + } + + const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); + pC = currentSegment->GetC()/stepsPerMm; //TODO store the reciprocal to avoid the division + if (currentSegment->IsLinear()) + { + // Set up pB, pC such that for forward motion, time = pB + pC * (distanceMoved * steps/mm) + pB = currentSegment->CalcLinearB(distanceSoFar, timeSoFar); + } + else + { + // Set up pA, pB, pC such that for forward motion, time = pB + sqrt(pA + pC * (distanceMoved * steps/mm)) + pA = currentSegment->CalcNonlinearA(distanceSoFar); + pB = currentSegment->CalcNonlinearB(timeSoFar); + } + + const float startDistance = distanceSoFar; + distanceSoFar += currentSegment->GetSegmentLength(); + timeSoFar += currentSegment->GetSegmentTime(); + + // Work out whether we reverse in this segment and the movement limit in steps + const float sDx = distanceSoFar * dda.directionVector[0]; + const float sDy = distanceSoFar * dda.directionVector[1]; + const int32_t netStepsAtEnd = (int32_t)(fastSqrtf(mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared - fsquare(stepsPerMm) * (sDx * (sDx + mp.delta.fTwoA) + sDy * (sDy + mp.delta.fTwoB))) + + (distanceSoFar * dda.directionVector[2] - mp.delta.h0MinusZ0) * stepsPerMm); + + if (mp.delta.reverseStartDistance <= startDistance) + { + // This segment is purely downwards motion and we want the greater of the two quadratic solutions. There may have been upwards motion earlier in the move. + if (direction) + { + direction = false; + directionChanged = true; + } + state = DMState::deltaReverse; + phaseStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 + : (reverseStartStep <= totalSteps) ? (uint32_t)((int32_t)(2 * reverseStartStep) - netStepsAtEnd) + : 1 - netStepsAtEnd; + } + else if (distanceSoFar <= mp.delta.reverseStartDistance) + { + // This segment is purely upwards motion of the tower and we want the lower quadratic solution + state = DMState::deltaForwardsNoReverse; + phaseStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(netStepsAtEnd + 1); + } + else + { + // This segment ends with reverse motion. We want the lower quadratic solution initially. + phaseStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)((int32_t)(2 * reverseStartStep) - netStepsAtEnd); + state = DMState::deltaForwardsReversing; + } + + if (phaseStepLimit > nextStep) + { + return true; + } + + currentSegment = currentSegment->GetNext(); } - else +} + +// This is called when currentSegment has just been changed to a new segment. Return true if there is a new segment to execute. +bool DriveMovement::NewExtruderSegment() noexcept +{ + while (true) { - mp.cart.decelStartStep = (uint32_t)(params.decelStartDistance * stepsPerMm) + 1; -#if DM_USE_FPU - fTwoDistanceToStopTimesCsquaredDivD = fsquare(params.fTopSpeedTimesCdivD) + (params.decelStartDistance * (StepTimer::StepClockRateSquared * 2))/dda.deceleration; -#else - twoDistanceToStopTimesCsquaredDivD = isquare64(params.topSpeedTimesCdivD) + roundU64((params.decelStartDistance * (StepTimer::StepClockRateSquared * 2))/dda.deceleration); -#endif + if (currentSegment == nullptr) + { + return false; + } + + const float startDistance = distanceSoFar; + const float startTime = timeSoFar; + + // Work out the movement limit in steps + distanceSoFar += currentSegment->GetSegmentLength(); + timeSoFar += currentSegment->GetSegmentTime(); + + pC = currentSegment->CalcC(mp.cart.effectiveMmPerStep); + if (currentSegment->IsLinear()) + { + // Set up pB, pC such that for forward motion, time = pB + pC * stepNumber + pB = currentSegment->CalcLinearB(startDistance, startTime); + state = DMState::cartLinear; + } + else + { + // Set up pA, pB, pC such that for forward motion, time = pB + sqrt(pA + pC * stepNumber) + pA = currentSegment->CalcNonlinearA(startDistance, mp.cart.pressureAdvanceK); + pB = currentSegment->CalcNonlinearB(startTime, mp.cart.pressureAdvanceK); + if (currentSegment->IsAccelerating()) + { + // Extruders have a single acceleration segment. We need to add the extra extrusion distance due to pressure advance to the extrusion distance. + distanceSoFar += mp.cart.extraExtrusionDistance; + state = DMState::cartAccel; + } + else + { + // This is the single decelerating segment. If it includes pressure advance then it may include reversal. + state = DMState::cartDecelForwardsReversing; // assume that it may reverse + } + } + + phaseStepLimit = ((currentSegment->GetNext() == nullptr) ? totalSteps : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm)) + 1; + if (nextStep < phaseStepLimit) + { + return true; + } + + currentSegment = currentSegment->GetNext(); // skip this segment } +} - // No reverse phase - reverseStartStep = totalSteps + 1; -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0; -#endif +// Prepare this DM for a Cartesian axis move, returning true if there are steps to do +bool DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& params) noexcept +{ + distanceSoFar = 0.0; + timeSoFar = 0.0; + mp.cart.pressureAdvanceK = 0.0; + // We can't use directionVector here because those values relate to Cartesian space, whereas we may be CoreXY etc. + mp.cart.effectiveStepsPerMm = (float)totalSteps/dda.totalDistance; + mp.cart.effectiveMmPerStep = 1.0/mp.cart.effectiveStepsPerMm; + isDelta = false; + isExtruder = false; + currentSegment = (dda.shapedSegments != nullptr) ? dda.shapedSegments : dda.unshapedSegments; + nextStep = 0; // must do this before calling NewCartesianSegment + + if (!NewCartesianSegment()) + { + return false; + } // Prepare for the first step - nextStep = 0; nextStepTime = 0; stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step stepsTillRecalc = 0; // so that we don't skip the calculation - isDelta = false; - state = (mp.cart.accelStopStep > 1) ? DMState::accel0 - : (mp.cart.decelStartStep > 1) ? DMState::steady - : DMState::decel0; + reverseStartStep = totalSteps + 1; // no reverse phase return CalcNextStepTime(dda); } @@ -125,19 +275,18 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n 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.dparams->GetDiagonalSquared(drive) - fsquare(A) - fsquare(B); - const float h0MinusZ0 = fastSqrtf(dSquaredMinusAsquaredMinusBsquared); + mp.delta.h0MinusZ0 = fastSqrtf(dSquaredMinusAsquaredMinusBsquared); #if DM_USE_FPU - mp.delta.fHmz0s = h0MinusZ0 * stepsPerMm; + mp.delta.fTwoA = 2.0 * A; + mp.delta.fTwoB = 2.0 * B; + mp.delta.fHmz0s = mp.delta.h0MinusZ0 * stepsPerMm; mp.delta.fMinusAaPlusBbTimesS = -(aAplusbB * stepsPerMm); mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared = dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm); - fTwoCsquaredTimesMmPerStepDivA = (float)((double)(2 * StepTimer::StepClockRateSquared)/((double)stepsPerMm * (double)dda.acceleration)); - fTwoCsquaredTimesMmPerStepDivD = (float)((double)(2 * StepTimer::StepClockRateSquared)/((double)stepsPerMm * (double)dda.deceleration)); #else + qq; // incomplete! mp.delta.hmz0sK = roundS32(h0MinusZ0 * stepsPerMm * DriveMovement::K2); mp.delta.minusAaPlusBbTimesKs = -roundS32(aAplusbB * stepsPerMm * DriveMovement::K2); mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = roundS64(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2)); - twoCsquaredTimesMmPerStepDivA = roundU64((double)(2 * StepTimer::StepClockRateSquared)/((double)stepsPerMm * (double)dda.acceleration)); - twoCsquaredTimesMmPerStepDivD = roundU64((double)(2 * StepTimer::StepClockRateSquared)/((double)stepsPerMm * (double)dda.deceleration)); #endif // Calculate the distance at which we need to reverse direction. @@ -145,6 +294,7 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n { // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. direction = (dda.directionVector[Z_AXIS] >= 0.0); + mp.delta.reverseStartDistance = (direction) ? dda.totalDistance + 1.0 : -1.0; // so that we never reverse and NewDeltaSegment knows which way we are going reverseStartStep = totalSteps + 1; } else @@ -153,16 +303,19 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n // the other root corresponds to the carriages being above the bed. const float drev = ((dda.directionVector[Z_AXIS] * fastSqrtf(params.a2plusb2 * params.dparams->GetDiagonalSquared(drive) - 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 + mp.delta.reverseStartDistance = drev; + 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 + fastSqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev)); - const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); + const int32_t numStepsUp = (int32_t)((hrev - mp.delta.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)) + if (numStepsUp < 1) { + mp.delta.reverseStartDistance = -1.0; // so that we know we have reversed already reverseStartStep = totalSteps + 1; + direction = false; } else { @@ -171,7 +324,7 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n // 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 + // Net movement is up, so we will go up first and then down by a lesser amount totalSteps = (2 * numStepsUp) - totalSteps; } else @@ -184,557 +337,286 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n } else { + // No reversal reverseStartStep = totalSteps + 1; + direction = (drev >= 0.0); } } - // Acceleration phase parameters -#if DM_USE_FPU - mp.delta.fAccelStopDs = params.accelDistance * stepsPerMm; -#else - mp.delta.accelStopDsK = roundU32(params.accelDistance * stepsPerMm * K2); -#endif - - // Constant speed phase parameters -#if DM_USE_FPU - fMmPerStepTimesCdivtopSpeed = (float)StepTimer::StepClockRate/(stepsPerMm * dda.topSpeed); -#else - mmPerStepTimesCKdivtopSpeed = roundU32(((float)StepTimer::StepClockRate * K1)/(stepsPerMm * dda.topSpeed)); -#endif + distanceSoFar = 0.0; + timeSoFar = 0.0; + isDelta = true; + currentSegment = (dda.shapedSegments != nullptr) ? dda.shapedSegments : dda.unshapedSegments; - // Deceleration phase parameters - // First check whether there is any deceleration at all, otherwise we may get strange results because of rounding errors - if (params.decelDistance * stepsPerMm < 0.5) + nextStep = 0; // must do this before calling NewDeltaSegment + if (!NewDeltaSegment(dda)) { -#if DM_USE_FPU - mp.delta.fDecelStartDs = std::numeric_limits<float>::max(); - fTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - mp.delta.decelStartDsK = 0xFFFFFFFF; - twoDistanceToStopTimesCsquaredDivD = 0; -#endif - } - else - { -#if DM_USE_FPU - mp.delta.fDecelStartDs = params.decelStartDistance * stepsPerMm; - fTwoDistanceToStopTimesCsquaredDivD = fsquare(params.fTopSpeedTimesCdivD) + (params.decelStartDistance * (StepTimer::StepClockRateSquared * 2))/dda.deceleration; -#else - mp.delta.decelStartDsK = roundU32(params.decelStartDistance * stepsPerMm * K2); - twoDistanceToStopTimesCsquaredDivD = isquare64(params.topSpeedTimesCdivD) + roundU64((params.decelStartDistance * (StepTimer::StepClockRateSquared * 2))/dda.deceleration); -#endif + return false; } // Prepare for the first step - nextStep = 0; nextStepTime = 0; stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step stepsTillRecalc = 0; // so that we don't skip the calculation - //TODO input shaping for delta motion - isDelta = true; return CalcNextStepTime(dda); } // Prepare this DM for an extruder move, returning true if there are steps to do -bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, float& extrusionPending, float speedChange, bool doCompensation) noexcept +// We have already generated the extruder segments and we know that there are some +bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) noexcept { - // Calculate the requested extrusion amount and a few other things - float dv = dda.directionVector[drive]; - float extrusionRequired = dda.totalDistance * dv; - const size_t extruder = LogicalDriveToExtruder(drive); - -#if SUPPORT_NONLINEAR_EXTRUSION - // Add the nonlinear extrusion correction to totalExtrusion - if (dda.flags.isPrintingMove) - { - float a, b, limit; - if (reprap.GetPlatform().GetExtrusionCoefficients(extruder, a, b, limit)) - { - const float averageExtrusionSpeed = (extrusionRequired * StepTimer::StepClockRate)/dda.clocksNeeded; - const float factor = 1.0 + min<float>((averageExtrusionSpeed * a) + (averageExtrusionSpeed * averageExtrusionSpeed * b), limit); - extrusionRequired *= factor; - } - } -#endif - - // Add on any fractional extrusion pending from the previous move - extrusionRequired += extrusionPending; - dv = extrusionRequired/dda.totalDistance; - direction = (extrusionRequired >= 0.0); + const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); + mp.cart.effectiveStepsPerMm = stepsPerMm * fabsf(dda.directionVector[drive]); + mp.cart.effectiveMmPerStep = 1.0/mp.cart.effectiveStepsPerMm; - const float rawStepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); - const float effectiveStepsPerMm = fabsf(dv) * rawStepsPerMm; + ExtruderShaper& shaper = reprap.GetMove().GetExtruderShaper(LogicalDriveToExtruder(drive)); + mp.cart.extrusionBroughtForwards = distanceSoFar = shaper.GetExtrusionPending()/dda.directionVector[drive]; - float compensationTime; - float accelCompensationDistance; + // Calculate the total forward and reverse movement distances + float forwardDistance = distanceSoFar; + float reverseDistance; - if (doCompensation && direction) + if (dda.flags.usePressureAdvance && shaper.GetKclocks() > 0.0) { - // Calculate the pressure advance parameters - compensationTime = reprap.GetPlatform().GetPressureAdvance(extruder); - const float compensationClocks = compensationTime * (float)StepTimer::StepClockRate; - mp.cart.compensationClocks = roundU32(compensationClocks); - mp.cart.accelCompensationClocks = roundU32(compensationClocks * params.accelCompFactor); - -#ifdef COMPENSATE_SPEED_CHANGES - // If there is a speed change at the start of the move, theoretically we should instantly advance or retard the filament by the associated compensation amount. - // We can't do that, so increase or decrease the extrusion factor instead, so that at least the extrusion will be correct by the end of the move. - const float factor = 1.0 + (speedChange * compensationTime)/dda.totalDistance; - stepsPerMm *= factor; -#endif - // Calculate the net total extrusion to allow for compensation. It may be negative. - extrusionRequired += (dda.endSpeed - dda.startSpeed) * compensationTime * dv; + // We are using nonzero pressure advance. Movement must be forwards. + mp.cart.pressureAdvanceK = shaper.GetKclocks(); + mp.cart.extraExtrusionDistance = mp.cart.pressureAdvanceK * params.unshaped.acceleration * params.unshaped.accelClocks; + forwardDistance += mp.cart.extraExtrusionDistance; - // Calculate the acceleration phase parameters - accelCompensationDistance = compensationTime * (dda.topSpeed - dda.startSpeed); - mp.cart.accelStopStep = (uint32_t)((params.accelDistance + accelCompensationDistance) * effectiveStepsPerMm) + 1; + // Check if there is a reversal in the deceleration segment + // There is at most one deceleration segment in the unshaped segments + const MoveSegment *decelSeg = dda.unshapedSegments; + while (decelSeg != nullptr && (decelSeg->IsLinear() || decelSeg->IsAccelerating())) + { + decelSeg = decelSeg->GetNext(); + } + + if (decelSeg == nullptr) + { + forwardDistance += dda.totalDistance; // no deceleration segment + reverseDistance = 0.0; + } + else + { + const float initialDecelSpeed = dda.topSpeed - mp.cart.pressureAdvanceK * params.unshaped.deceleration; + if (initialDecelSpeed <= 0.0) + { + // The entire deceleration segment is in reverse + forwardDistance += params.unshaped.decelStartDistance; + reverseDistance = ((0.5 * params.unshaped.deceleration * params.unshaped.decelClocks) - initialDecelSpeed) * params.unshaped.decelClocks; + } + else + { + const float timeToReverse = initialDecelSpeed * ((-0.5) * decelSeg->GetC()); // 'c' is -2/deceleration, so -0.5*c is 1/deceleration + if (timeToReverse < params.unshaped.decelClocks) + { + // There is a reversal + const float distanceToReverse = 0.5 * params.unshaped.deceleration * fsquare(timeToReverse); + forwardDistance += params.unshaped.decelStartDistance + distanceToReverse; + reverseDistance = 0.5 * params.unshaped.deceleration * fsquare(params.unshaped.decelClocks - timeToReverse); + } + else + { + // No reversal + forwardDistance += dda.totalDistance - (mp.cart.pressureAdvanceK * params.unshaped.deceleration * params.unshaped.decelClocks); + reverseDistance = 0.0; + } + } + } } else { - accelCompensationDistance = compensationTime = 0.0; - mp.cart.compensationClocks = mp.cart.accelCompensationClocks = 0; - - // Calculate the acceleration phase parameters - mp.cart.accelStopStep = (uint32_t)(params.accelDistance * effectiveStepsPerMm) + 1; + // No pressure advance. Movement may be backwards but this still counts as forward distance in the calculations. + mp.cart.pressureAdvanceK = mp.cart.extraExtrusionDistance = 0.0; + forwardDistance += dda.totalDistance; + reverseDistance = 0.0; } - int32_t netSteps = lrintf(extrusionRequired * rawStepsPerMm); - extrusionPending = extrusionRequired - (float)netSteps/rawStepsPerMm; - - if (!direction) + // Check whether there are any steps at all + const float forwardSteps = forwardDistance * mp.cart.effectiveStepsPerMm; + if (reverseDistance > 0.0) { - netSteps = -netSteps; - } - - // Note, netSteps may be negative at this point if we are applying pressure advance -#if DM_USE_FPU - fTwoCsquaredTimesMmPerStepDivA = (double)(StepTimer::StepClockRateSquared * 2)/((double)effectiveStepsPerMm * (double)dda.acceleration); - fTwoCsquaredTimesMmPerStepDivD = (double)(StepTimer::StepClockRateSquared * 2)/((double)effectiveStepsPerMm * (double)dda.deceleration); -#else - twoCsquaredTimesMmPerStepDivA = roundU64((double)(StepTimer::StepClockRateSquared * 2)/((double)effectiveStepsPerMm * (double)dda.acceleration)); - twoCsquaredTimesMmPerStepDivD = roundU64((double)(StepTimer::StepClockRateSquared * 2)/((double)effectiveStepsPerMm * (double)dda.deceleration)); -#endif - - // Constant speed phase parameters -#if DM_USE_FPU - fMmPerStepTimesCdivtopSpeed = (float)StepTimer::StepClockRate/(effectiveStepsPerMm * dda.topSpeed); -#else - mmPerStepTimesCKdivtopSpeed = (uint32_t)(((float)StepTimer::StepClockRate * K1)/(effectiveStepsPerMm * dda.topSpeed)); -#endif + const float netDistance = forwardDistance - reverseDistance; + const int32_t iFwdSteps = (int32_t)forwardSteps; + const int32_t netSteps = (int32_t)(netDistance * mp.cart.effectiveStepsPerMm); + if (netSteps == 0 && iFwdSteps <= 1) + { + // No movement at all, or one step forward and one step back which we will ignore + shaper.SetExtrusionPending(netDistance * dda.directionVector[drive]); + return false; + } - // Calculate the deceleration and reverse phase parameters and update totalSteps - // First check whether there is any deceleration at all, otherwise we may get strange results because of rounding errors - if (params.decelDistance * effectiveStepsPerMm < 0.5) // if less than 1 deceleration step - { - totalSteps = (uint32_t)max<int32_t>(netSteps, 0); - mp.cart.decelStartStep = reverseStartStep = totalSteps + 1; -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0.0; - fTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0; - twoDistanceToStopTimesCsquaredDivD = 0; -#endif + // Note, netSteps may be negative for e.g. a deceleration-only move + if (netSteps > 0 && netSteps + 1 >= iFwdSteps) + { + // There is at least one forward step are zero or one reverse steps. We ignore a single reverse step. + totalSteps = netSteps; + reverseStartStep = totalSteps + 1; // no reverse phase + } + else + { + // We know that netSteps <= iFwdSteps + reverseStartStep = iFwdSteps + 1; + totalSteps = (uint32_t)((int32_t)(2 * iFwdSteps) - netSteps); + } + shaper.SetExtrusionPending((netDistance - (float)netSteps * mp.cart.effectiveMmPerStep) * dda.directionVector[drive]); } else { - mp.cart.decelStartStep = (uint32_t)((params.decelStartDistance + accelCompensationDistance) * effectiveStepsPerMm) + 1; -#if DM_USE_FPU - const float initialDecelSpeedTimesCdivD = params.fTopSpeedTimesCdivD - (float)mp.cart.compensationClocks; - const float initialDecelSpeedTimesCdivDSquared = fsquare(initialDecelSpeedTimesCdivD); - fTwoDistanceToStopTimesCsquaredDivD = - initialDecelSpeedTimesCdivDSquared + ((params.decelStartDistance + accelCompensationDistance) * (float)(StepTimer::StepClockRateSquared * 2))/dda.deceleration; -#else - const int32_t initialDecelSpeedTimesCdivD = (int32_t)params.topSpeedTimesCdivD - (int32_t)mp.cart.compensationClocks; // signed because it may be negative and we square it - const uint64_t initialDecelSpeedTimesCdivDSquared = isquare64(initialDecelSpeedTimesCdivD); - twoDistanceToStopTimesCsquaredDivD = - initialDecelSpeedTimesCdivDSquared + roundU64(((params.decelStartDistance + accelCompensationDistance) * (float)(StepTimer::StepClockRateSquared * 2))/dda.deceleration); -#endif - - // See whether there is a reverse phase - const float compensationSpeedChange = dda.deceleration * compensationTime; - const uint32_t stepsBeforeReverse = (compensationSpeedChange > dda.topSpeed) - ? mp.cart.decelStartStep - 1 -#if DM_USE_FPU - : (uint32_t)(fTwoDistanceToStopTimesCsquaredDivD/fTwoCsquaredTimesMmPerStepDivD); -#else - : twoDistanceToStopTimesCsquaredDivD/twoCsquaredTimesMmPerStepDivD; -#endif - if (dda.endSpeed < compensationSpeedChange && (int32_t)stepsBeforeReverse > netSteps) + if (forwardSteps >= 1.0) { - reverseStartStep = stepsBeforeReverse + 1; - totalSteps = (uint32_t)((int32_t)(2 * stepsBeforeReverse) - netSteps); -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = (2 * stepsBeforeReverse) * fTwoCsquaredTimesMmPerStepDivD - fTwoDistanceToStopTimesCsquaredDivD; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = - (int64_t)((2 * stepsBeforeReverse) * twoCsquaredTimesMmPerStepDivD) - (int64_t)twoDistanceToStopTimesCsquaredDivD; -#endif + totalSteps = (uint32_t)forwardSteps; + shaper.SetExtrusionPending((forwardDistance - (float)totalSteps * mp.cart.effectiveMmPerStep) * dda.directionVector[drive]); } else { - // There is no reverse phase. Check that we can actually do the last step requested. - if (netSteps > (int32_t)stepsBeforeReverse) - { - netSteps = (int32_t)stepsBeforeReverse; - } - totalSteps = (uint32_t)max<int32_t>(netSteps, 0); - reverseStartStep = totalSteps + 1; -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0; -#endif + // No steps at all, or negative forward steps which I think should be impossible unless the steps/mm is changed + totalSteps = 0; + shaper.SetExtrusionPending(forwardDistance * dda.directionVector[drive]); + return false; } + reverseStartStep = totalSteps + 1; // no reverse phase + } + + currentSegment = dda.unshapedSegments; + timeSoFar = 0.0; + isDelta = false; + isExtruder = true; + + nextStep = 0; // must do this before calling NewExtruderSegment + if (!NewExtruderSegment()) + { + return false; // this should not happen because we have already determined that there are steps to do } // Prepare for the first step - nextStep = 0; nextStepTime = 0; stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step stepsTillRecalc = 0; // so that we don't skip the calculation - state = (mp.cart.accelStopStep > 1) ? DMState::accel0 - : (mp.cart.decelStartStep > 1) ? DMState::steady - : (reverseStartStep > 1) ? DMState::decel0 - : DMState::reversing; - isDelta = false; return CalcNextStepTime(dda); } -#if SUPPORT_REMOTE_COMMANDS - -// Prepare this DM for an extruder move. The caller has already checked that pressure advance is enabled. -//TODO are values in the DDA in the correct units for this code? -bool DriveMovement::PrepareRemoteExtruder(const DDA& dda, const PrepParams& params) noexcept +// Calculate and store the time since the start of the move when the next step for the specified DriveMovement is due. +// We have already incremented nextStep and checked that it does not exceed totalSteps, so at least one more step is due +// Return true if all OK, false to abort this move because the calculation has gone wrong +bool DriveMovement::CalcNextStepTimeFull(const DDA &dda) noexcept +pre(nextStep <= totalSteps; stepsTillRecalc == 0) { - // Calculate the pressure advance parameters - const float compensationTime = reprap.GetPlatform().EutGetRemotePressureAdvance(drive); - const float compensationClocks = compensationTime * (float)StepTimer::StepClockRate; - mp.cart.compensationClocks = roundU32(compensationClocks); - mp.cart.accelCompensationClocks = roundU32(compensationClocks * params.accelCompFactor); - - // Recalculate the net total step count to allow for compensation. It may be negative. - const float compensationDistance = (dda.endSpeed - dda.startSpeed) * compensationTime; - int32_t netSteps = lrintf((1.0 + compensationDistance) * totalSteps); - - // Calculate the acceleration phase parameters - const float accelCompensationDistance = compensationTime * (dda.topSpeed - dda.startSpeed); - mp.cart.accelStopStep = (uint32_t)((params.accelDistance + accelCompensationDistance) * totalSteps) + 1; - -#if DM_USE_FPU - fTwoCsquaredTimesMmPerStepDivA = (double)2.0/((double)totalSteps * (double)dda.acceleration); - fTwoCsquaredTimesMmPerStepDivD = (double)2.0/((double)totalSteps * (double)dda.deceleration); -#else - twoCsquaredTimesMmPerStepDivA = roundU64((double)2.0/((double)totalSteps * (double)dda.acceleration)); - twoCsquaredTimesMmPerStepDivD = roundU64((double)2.0/((double)totalSteps * (double)dda.deceleration)); -#endif - - // Constant speed phase parameters -#if DM_USE_FPU - fMmPerStepTimesCdivtopSpeed = 1.0/(totalSteps * dda.topSpeed); -#else - mmPerStepTimesCKdivtopSpeed = (uint32_t)((float)K1/(totalSteps * dda.topSpeed)); -#endif + uint32_t stepsToLimit = phaseStepLimit - nextStep; - // Calculate the deceleration and reverse phase parameters and update totalSteps - // First check whether there is any deceleration at all, otherwise we may get strange results because of rounding errors - if (params.decelDistance * totalSteps < 0.5) // if less than 1 deceleration step - { - totalSteps = (uint32_t)max<int32_t>(netSteps, 0); - mp.cart.decelStartStep = reverseStartStep = netSteps + 1; -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0.0; - fTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0; - twoDistanceToStopTimesCsquaredDivD = 0; -#endif - } - else + // If there are no more steps left in this segment, skip to the next segment + if (stepsToLimit == 0) { - mp.cart.decelStartStep = (uint32_t)((params.decelStartDistance + accelCompensationDistance) * totalSteps) + 1; -#if DM_USE_FPU - const float initialDecelSpeedTimesCdivD = params.fTopSpeedTimesCdivD - (float)mp.cart.compensationClocks; - const float initialDecelSpeedTimesCdivDSquared = fsquare(initialDecelSpeedTimesCdivD); - fTwoDistanceToStopTimesCsquaredDivD = initialDecelSpeedTimesCdivDSquared + ((params.decelStartDistance + accelCompensationDistance) * 2)/dda.deceleration; -#else - const int32_t initialDecelSpeedTimesCdivD = (int32_t)params.topSpeedTimesCdivD - (int32_t)mp.cart.compensationClocks; // signed because it may be negative and we square it - const uint64_t initialDecelSpeedTimesCdivDSquared = isquare64(initialDecelSpeedTimesCdivD); - twoDistanceToStopTimesCsquaredDivD = initialDecelSpeedTimesCdivDSquared + roundU64(((params.decelStartDistance + accelCompensationDistance) * 2)/dda.deceleration); -#endif - - // See whether there is a reverse phase - const float compensationSpeedChange = dda.deceleration * compensationTime; - const uint32_t stepsBeforeReverse = (compensationSpeedChange > dda.topSpeed) - ? mp.cart.decelStartStep - 1 -#if DM_USE_FPU - : fTwoDistanceToStopTimesCsquaredDivD/fTwoCsquaredTimesMmPerStepDivD; -#else - : twoDistanceToStopTimesCsquaredDivD/twoCsquaredTimesMmPerStepDivD; -#endif - if (dda.endSpeed < compensationSpeedChange && (int32_t)stepsBeforeReverse > netSteps) - { - reverseStartStep = stepsBeforeReverse + 1; - totalSteps = (uint32_t)((int32_t)(2 * stepsBeforeReverse) - netSteps); -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = ((2 * stepsBeforeReverse) *fTwoCsquaredTimesMmPerStepDivD) - fTwoDistanceToStopTimesCsquaredDivD; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = - (int64_t)((2 * stepsBeforeReverse) * twoCsquaredTimesMmPerStepDivD) - (int64_t)twoDistanceToStopTimesCsquaredDivD; -#endif - } - else + currentSegment = currentSegment->GetNext(); + const bool more = (isDelta) ? NewDeltaSegment(dda) + : (isExtruder) ? NewExtruderSegment() + : NewCartesianSegment(); + if (!more) { - // There is no reverse phase. Check that we can actually do the last step requested. - if (netSteps > (int32_t)stepsBeforeReverse) - { - netSteps = (int32_t)stepsBeforeReverse; - } - reverseStartStep = netSteps + 1; - totalSteps = (uint32_t)max<int32_t>(netSteps, 0); -#if DM_USE_FPU - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0.0; -#else - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD = 0; -#endif + state = DMState::stepError; + nextStep += 100000000; // so we can tell what happened in the debug print + return false; } + stepsToLimit = phaseStepLimit - nextStep; } - // Prepare for the first step - nextStep = 0; - nextStepTime = 0; - stepInterval = 999999; // initialise to a large value so that we will calculate the time for just one step - stepsTillRecalc = 0; // so that we don't skip the calculation - state = (mp.cart.accelStopStep > 1) ? DMState::accel0 - : (mp.cart.decelStartStep > 1) ? DMState::steady - : (reverseStartStep > 1) ? DMState::decel0 - : DMState::reversing; - isDelta = false; - return CalcNextStepTime(dda); -} - -#endif + if (reverseStartStep < phaseStepLimit && nextStep < reverseStartStep) + { + stepsToLimit = reverseStartStep - nextStep; + } -void DriveMovement::DebugPrint() const noexcept -{ - char c = (drive < reprap.GetGCodes().GetTotalAxes()) ? reprap.GetGCodes().GetAxisLetters()[drive] : (char)('0' + LogicalDriveToExtruder(drive)); - if (state != DMState::idle) + uint32_t shiftFactor = 0; // assume single stepping + if (stepsToLimit > 1 && stepInterval < DDA::MinCalcInterval) { -#if DM_USE_FPU - debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 - " 2dtstc2diva=%.2f\n", - c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, - (double)fTwoDistanceToStopTimesCsquaredDivD); -#else - debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 - " 2dtstc2diva=%" PRIu64 "\n", - c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, - twoDistanceToStopTimesCsquaredDivD); -#endif - if (isDelta) + if (stepInterval < DDA::MinCalcInterval/4 && stepsToLimit > 8) { -#if DM_USE_FPU - debugPrintf("hmz0s=%.2f minusAaPlusBbTimesS=%.2f dSquaredMinusAsquaredMinusBsquared=%.2f\n" - "2c2mmsda=%.2f 2c2mmsdd=%.2f asds=%.2f dsds=%.2f mmstcdts=%.2f\n", - (double)mp.delta.fHmz0s, (double)mp.delta.fMinusAaPlusBbTimesS, (double)mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared, - (double)fTwoCsquaredTimesMmPerStepDivA, (double)fTwoCsquaredTimesMmPerStepDivD, (double)mp.delta.fAccelStopDs, (double)mp.delta.fDecelStartDs, (double)fMmPerStepTimesCdivtopSpeed - ); -#else - debugPrintf("hmz0sK=%" PRIi32 " minusAaPlusBbTimesKs=%" PRIi32 " dSquaredMinusAsquaredMinusBsquared=%" PRId64 "\n" - "2c2mmsda=%" PRIu64 " 2c2mmsdd=%" PRIu64 " asdsk=%" PRIu32 " dsdsk=%" PRIu32 " mmstcdts=%" PRIu32 "\n", - mp.delta.hmz0sK, mp.delta.minusAaPlusBbTimesKs, mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared, - twoCsquaredTimesMmPerStepDivA, twoCsquaredTimesMmPerStepDivD, mp.delta.accelStopDsK, mp.delta.decelStartDsK, mmPerStepTimesCKdivtopSpeed - ); -#endif + shiftFactor = 3; // octal stepping } - else + else if (stepInterval < DDA::MinCalcInterval/2 && stepsToLimit > 4) { -#if DM_USE_FPU - debugPrintf("accelStopStep=%" PRIu32 " decelStartStep=%" PRIu32 " 2c2mmsda=%.2f 2c2mmsdd=%.2f\n" - "mmPerStepTimesCdivtopSpeed=%.2f fmsdmtstdca2=%.2f cc=%" PRIu32 " acc=%" PRIu32 "\n", - mp.cart.accelStopStep, mp.cart.decelStartStep, (double)fTwoCsquaredTimesMmPerStepDivA, (double)fTwoCsquaredTimesMmPerStepDivD, - (double)fMmPerStepTimesCdivtopSpeed, (double)mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD, mp.cart.compensationClocks, mp.cart.accelCompensationClocks - ); -#else - debugPrintf("accelStopStep=%" PRIu32 " decelStartStep=%" PRIu32 " 2c2mmsda=%" PRIu64 " 2c2mmsdd=%" PRIu64 "\n" - "mmPerStepTimesCdivtopSpeed=%" PRIu32 " fmsdmtstdca2=%" PRId64 " cc=%" PRIu32 " acc=%" PRIu32 "\n", - mp.cart.accelStopStep, mp.cart.decelStartStep, twoCsquaredTimesMmPerStepDivA, twoCsquaredTimesMmPerStepDivD, - mmPerStepTimesCKdivtopSpeed, mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD, mp.cart.compensationClocks, mp.cart.accelCompensationClocks - ); -#endif + shiftFactor = 2; // quad stepping + } + else if (stepsToLimit > 2) + { + shiftFactor = 1; // double stepping } } - else - { - debugPrintf("DM%c: not moving\n", c); - } -} + stepsTillRecalc = (1u << shiftFactor) - 1u; // store number of additional steps to generate -// Calculate and store 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. -// This is also used for extruders on delta machines. -bool DriveMovement::CalcNextStepTimeCartesianFull(const DDA &dda) noexcept -pre(nextStep < totalSteps; stepsTillRecalc == 0) -{ - // Work out how many steps to calculate at a time. - uint32_t shiftFactor = 0; // assume single stepping uint32_t nextCalcStepTime; + + // Work out the time of the step switch (state) { - case DMState::accel0: // acceleration phase - { - const uint32_t stepsToLimit = mp.cart.accelStopStep - nextStep; - if (stepsToLimit == 1) - { - // This is the last step in this phase - state = (mp.cart.decelStartStep > mp.cart.accelStopStep) ? DMState::steady - : (reverseStartStep > mp.cart.accelStopStep) ? DMState::decel0 - : DMState::reversing; - } - else if (stepInterval < DDA::MinCalcIntervalCartesian) - { - if (stepInterval < DDA::MinCalcIntervalCartesian/4 && stepsToLimit > 8) - { - shiftFactor = 3; // octal stepping - } - else if (stepInterval < DDA::MinCalcIntervalCartesian/2 && stepsToLimit > 4) - { - shiftFactor = 2; // quad stepping - } - else if (stepsToLimit > 2) - { - shiftFactor = 1; // double stepping - } - } + case DMState::cartLinear: // linear steady speed + nextCalcStepTime = pB + (float)(nextStep + stepsTillRecalc) * pC; + break; - stepsTillRecalc = (1u << shiftFactor) - 1u; // store number of additional steps to generate - const uint32_t nextCalcStep = nextStep + stepsTillRecalc; -#if DM_USE_FPU - const float adjustedStartSpeedTimesCdivA = (float)(dda.afterPrepare.startSpeedTimesCdivA + mp.cart.compensationClocks); - nextCalcStepTime = (uint32_t)(fastSqrtf(fsquare(adjustedStartSpeedTimesCdivA) + (fTwoCsquaredTimesMmPerStepDivA * nextCalcStep)) - adjustedStartSpeedTimesCdivA); -#else - const uint32_t adjustedStartSpeedTimesCdivA = dda.afterPrepare.startSpeedTimesCdivA + mp.cart.compensationClocks; - nextCalcStepTime = isqrt64(isquare64(adjustedStartSpeedTimesCdivA) + (twoCsquaredTimesMmPerStepDivA * nextCalcStep)) - adjustedStartSpeedTimesCdivA; -#endif - } + case DMState::cartAccel: // Cartesian accelerating + nextCalcStepTime = pB + fastSqrtf(pA + pC * (float)(nextStep + stepsTillRecalc)); break; - case DMState::steady: // steady speed phase + case DMState::cartDecelForwardsReversing: + if (nextStep + stepsTillRecalc < reverseStartStep) { - const uint32_t stepsToLimit = mp.cart.decelStartStep - nextStep; - if (stepsToLimit == 1) - { - state = (reverseStartStep > mp.cart.decelStartStep) ? DMState::decel0 - : DMState::reversing; - } - else if (stepInterval < DDA::MinCalcIntervalCartesian) - { - if (stepInterval < DDA::MinCalcIntervalCartesian/4 && stepsToLimit > 8) - { - shiftFactor = 3; // octal stepping - } - else if (stepInterval < DDA::MinCalcIntervalCartesian/2 && stepsToLimit > 4) - { - shiftFactor = 2; // quad stepping - } - else if (stepsToLimit > 2) - { - shiftFactor = 1; // double stepping - } - } - - stepsTillRecalc = (1u << shiftFactor) - 1u; // store number of additional steps to generate - const uint32_t nextCalcStep = nextStep + stepsTillRecalc; - nextCalcStepTime = -#if DM_USE_FPU - (uint32_t)( (int32_t)(fMmPerStepTimesCdivtopSpeed * nextCalcStep) - + dda.afterPrepare.extraAccelerationClocks - - (int32_t)mp.cart.accelCompensationClocks - ); -#else - (uint32_t)( (int32_t)(((uint64_t)mmPerStepTimesCKdivtopSpeed * nextCalcStep)/K1) - + dda.afterPrepare.extraAccelerationClocks - - (int32_t)mp.cart.accelCompensationClocks - ); -#endif + nextCalcStepTime = pB - fastSqrtf(pA + pC * (float)(nextStep + stepsTillRecalc)); + break; } + + direction = false; + directionChanged = true; + state = DMState::cartDecelReverse; + // no break + case DMState::cartDecelReverse: // Cartesian decelerating, reverse motion + nextCalcStepTime = pB + fastSqrtf(pA + pC * (float)((2 * (reverseStartStep - 1)) - (nextStep + stepsTillRecalc))); + break; + + case DMState::cartDecelNoReverse: // Cartesian accelerating with no reversal + nextCalcStepTime = pB - fastSqrtf(pA + pC * (float)(nextStep + stepsTillRecalc)); break; - case DMState::decel0: // deceleration phase, not reversed yet + case DMState::deltaForwardsReversing: // moving forwards + if (nextStep == reverseStartStep) { - const uint32_t stepsToLimit = reverseStartStep - nextStep; - if (stepsToLimit == 1) + direction = false; + directionChanged = true; + state = DMState::deltaReverse; + } + // no break + case DMState::deltaForwardsNoReverse: + case DMState::deltaReverse: // reversing on this and subsequent steps + // Calculate d*s where d = distance the head has travelled, s = steps/mm for this drive + { + const float steps = (float)(1u << shiftFactor); + if (direction) { - state = DMState::reversing; + mp.delta.fHmz0s += steps; // get new carriage height above Z in steps } - else if (stepInterval < DDA::MinCalcIntervalCartesian) + else { - if (stepInterval < DDA::MinCalcIntervalCartesian/4 && stepsToLimit > 8) - { - shiftFactor = 3; // octal stepping - } - else if (stepInterval < DDA::MinCalcIntervalCartesian/2 && stepsToLimit > 4) - { - shiftFactor = 2; // quad stepping - } - else if (stepsToLimit > 2) - { - shiftFactor = 1; // double stepping - } + mp.delta.fHmz0s -= steps; // get new carriage height above Z in steps } - stepsTillRecalc = (1u << shiftFactor) - 1u; // store number of additional steps to generate - const uint32_t nextCalcStep = nextStep + stepsTillRecalc; - const uint32_t adjustedTopSpeedTimesCdivDPlusDecelStartClocks = dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - mp.cart.compensationClocks; -#if DM_USE_FPU - const float temp = fTwoCsquaredTimesMmPerStepDivD * nextCalcStep; - // Allow for possible rounding error when the end speed is zero or very small - nextCalcStepTime = (temp < fTwoDistanceToStopTimesCsquaredDivD) - ? adjustedTopSpeedTimesCdivDPlusDecelStartClocks - (uint32_t)(fastSqrtf(fTwoDistanceToStopTimesCsquaredDivD - temp)) - : adjustedTopSpeedTimesCdivDPlusDecelStartClocks; -#else - const uint64_t temp = twoCsquaredTimesMmPerStepDivD * nextCalcStep; - // Allow for possible rounding error when the end speed is zero or very small - nextCalcStepTime = (temp < twoDistanceToStopTimesCsquaredDivD) - ? adjustedTopSpeedTimesCdivDPlusDecelStartClocks - isqrt64(twoDistanceToStopTimesCsquaredDivD - temp) - : adjustedTopSpeedTimesCdivDPlusDecelStartClocks; -#endif - } - break; + const float hmz0sc = mp.delta.fHmz0s * dda.directionVector[Z_AXIS]; + const float t1 = mp.delta.fMinusAaPlusBbTimesS + hmz0sc; + const float t2a = mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared - fsquare(mp.delta.fHmz0s) + fsquare(t1); + // Due to rounding error we can end up trying to take the square root of a negative number if we do not take precautions here + const float t2 = (t2a > 0.0) ? fastSqrtf(t2a) : 0.0; + const float ds = (direction) ? t1 - t2 : t1 + t2; - case DMState::reversing: - direction = !direction; - directionChanged = true; - state = DMState::reverse; - // no break - case DMState::reverse: // reverse phase - { - const uint32_t stepsToLimit = totalSteps + 1 - nextStep; - if (stepInterval < DDA::MinCalcIntervalCartesian) + // Now feed ds into the step algorithm for Cartesian motion + if (ds < 0.0) { - if (stepInterval < DDA::MinCalcIntervalCartesian/4 && stepsToLimit > 8) - { - shiftFactor = 3; // octal stepping - } - else if (stepInterval < DDA::MinCalcIntervalCartesian/2 && stepsToLimit > 4) - { - shiftFactor = 2; // quad stepping - } - else if (stepsToLimit > 2) - { - shiftFactor = 1; // double stepping - } + state = DMState::stepError; + nextStep += 110000000; // so that we can tell what happened in the debug print + return false; } - stepsTillRecalc = (1u << shiftFactor) - 1u; // store number of additional steps to generate - const uint32_t nextCalcStep = nextStep + stepsTillRecalc; - const uint32_t adjustedTopSpeedTimesCdivDPlusDecelStartClocks = dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - mp.cart.compensationClocks; - nextCalcStepTime = adjustedTopSpeedTimesCdivDPlusDecelStartClocks -#if DM_USE_FPU - + (uint32_t)(fastSqrtf((fTwoCsquaredTimesMmPerStepDivD * nextCalcStep) - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD)); -#else - + isqrt64((int64_t)(twoCsquaredTimesMmPerStepDivD * nextCalcStep) - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD); -#endif + const float pCds = pC * ds; + nextCalcStepTime = (currentSegment->IsLinear()) ? pB + pCds + : (currentSegment->IsAccelerating()) ? pB + fastSqrtf(pA + pCds) + : pB - fastSqrtf(pA + pCds); + if (currentSegment->IsLinear()) { pA = ds; } //DEBUG } break; @@ -742,172 +624,27 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) return false; } - // When crossing between movement phases with high microstepping, due to rounding errors the next step may appear to be due before the last one - stepInterval = (nextCalcStepTime > nextStepTime) - ? (nextCalcStepTime - nextStepTime) >> shiftFactor // calculate the time per step, ready for next time - : 0; -#if EVEN_STEPS - nextStepTime = nextCalcStepTime - (stepsTillRecalc * stepInterval); -#else - nextStepTime = nextCalcStepTime; -#endif - if (nextCalcStepTime > dda.clocksNeeded) { // The calculation makes this step late. // When the end speed is very low, calculating the time of the last step is very sensitive to rounding error. // So if this is the last step and it is late, bring it forward to the expected finish time. // Very rarely on a delta, the penultimate step may also be calculated late. Allow for that here in case it affects Cartesian axes too. - if (nextStep + 1 >= totalSteps) + if (nextStep + stepsTillRecalc + 1 >= totalSteps) { - nextStepTime = dda.clocksNeeded; + nextCalcStepTime = dda.clocksNeeded; } else { // We don't expect any step except the last to be late state = DMState::stepError; - stepInterval = 10000000 + nextStepTime; // so we can tell what happened in the debug print + nextStep += 120000000 + stepsTillRecalc; // so we can tell what happened in the debug print + stepInterval = nextCalcStepTime; //DEBUG return false; } } - return true; -} - -// 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 -bool DriveMovement::CalcNextStepTimeDeltaFull(const DDA &dda) noexcept -pre(nextStep < totalSteps; stepsTillRecalc == 0) -{ - // Work out how many steps to calculate at a time. - // The last step before reverseStartStep must be single stepped to make sure that we don't reverse the direction too soon. - // The simulator suggests that at 200steps/mm, the minimum step pulse interval for 400mm/sec movement is 4.5us - uint32_t shiftFactor = 0; // assume single stepping - if (stepInterval < DDA::MinCalcIntervalDelta) - { - const uint32_t stepsToLimit = ((nextStep < reverseStartStep && reverseStartStep <= totalSteps) - ? reverseStartStep - : totalSteps - ) - nextStep; - if (stepInterval < DDA::MinCalcIntervalDelta/8 && stepsToLimit > 16) - { - shiftFactor = 4; // hexadecimal stepping - } - else if (stepInterval < DDA::MinCalcIntervalDelta/4 && stepsToLimit > 8) - { - shiftFactor = 3; // octal stepping - } - else if (stepInterval < DDA::MinCalcIntervalDelta/2 && stepsToLimit > 4) - { - shiftFactor = 2; // quad stepping - } - else if (stepsToLimit > 2) - { - shiftFactor = 1; // double stepping - } - } - - stepsTillRecalc = (1u << shiftFactor) - 1; // store number of additional steps to generate - - if (nextStep == reverseStartStep) - { - direction = false; - directionChanged = true; - } - - // Calculate d*s*K as an integer, where d = distance the head has travelled, s = steps/mm for this drive, K = a power of 2 to reduce the rounding errors - // K here means K2 - // mp.delta.hmz0sk = (number of steps by which the carriage is higher than Z) * K2 - { -#if DM_USE_FPU - float steps = float(1u << shiftFactor); - if (!direction) - { - steps = -steps; - } - mp.delta.fHmz0s += steps; // get new carriage height above Z in steps -#else - int32_t shiftedK2 = (int32_t)(K2 << shiftFactor); - if (!direction) - { - shiftedK2 = -shiftedK2; - } - mp.delta.hmz0sK += shiftedK2; // get K2 * (new carriage height above Z in steps) -#endif - } -#if DM_USE_FPU - const float hmz0sc = mp.delta.fHmz0s * dda.directionVector[Z_AXIS]; - const float t1 = mp.delta.fMinusAaPlusBbTimesS + hmz0sc; - // Due to rounding error we can end up trying to take the square root of a negative number if we do not take precautions here - const float t2a = mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared - fsquare(mp.delta.fHmz0s) + fsquare(t1); - const float t2 = (t2a > 0.0) ? fastSqrtf(t2a) : 0.0; - const float ds = (direction) ? t1 - t2 : t1 + t2; -#else - // In the following, cKc is the Z movement fraction of the total move scaled by Kc - const int32_t hmz0scK = (int32_t)(((int64_t)mp.delta.hmz0sK * dda.afterPrepare.cKc)/Kc); - const int32_t t1 = mp.delta.minusAaPlusBbTimesKs + hmz0scK; - // Due to rounding error we can end up trying to take the square root of a negative number if we do not take precautions here - const int64_t t2a = mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared - (int64_t)isquare64(mp.delta.hmz0sK) + (int64_t)isquare64(t1); - const int32_t t2 = (t2a > 0) ? isqrt64(t2a) : 0; - const int32_t dsK = (direction) ? t1 - t2 : t1 + t2; -#endif - - // Now feed dsK into a modified version of the step algorithm for Cartesian motion without elasticity compensation -#if DM_USE_FPU - if (ds < 0.0) -#else - if (dsK < 0) -#endif - { - state = DMState::stepError; - nextStep += 1000000; // so that we can tell what happened in the debug print - return false; - } - - uint32_t nextCalcStepTime; -#if DM_USE_FPU - if (ds < mp.delta.fAccelStopDs) - { - // Acceleration phase - nextCalcStepTime = (uint32_t)(fastSqrtf(fsquare((float)dda.afterPrepare.startSpeedTimesCdivA) + (fTwoCsquaredTimesMmPerStepDivA * ds))) - dda.afterPrepare.startSpeedTimesCdivA; - } - else if (ds < mp.delta.fDecelStartDs) - { - // Steady speed phase - nextCalcStepTime = (uint32_t)(fMmPerStepTimesCdivtopSpeed * ds) + dda.afterPrepare.extraAccelerationClocks; - } - else - { - const float temp = fTwoCsquaredTimesMmPerStepDivD * ds; - // Because of possible rounding error when the end speed is zero or very small, we need to check that the square root will work OK - nextCalcStepTime = (temp < fTwoDistanceToStopTimesCsquaredDivD) - ? dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - lrintf(fastSqrtf(fTwoDistanceToStopTimesCsquaredDivD - temp)) - : dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks; - } -#else - if ((uint32_t)dsK < mp.delta.accelStopDsK) - { - // Acceleration phase - nextCalcStepTime = isqrt64(isquare64(dda.afterPrepare.startSpeedTimesCdivA) + (twoCsquaredTimesMmPerStepDivA * (uint32_t)dsK)/K2) - dda.afterPrepare.startSpeedTimesCdivA; - } - else if ((uint32_t)dsK < mp.delta.decelStartDsK) - { - // Steady speed phase - nextCalcStepTime = (uint32_t)( (int32_t)(((uint64_t)mmPerStepTimesCKdivtopSpeed * (uint32_t)dsK)/(K1 * K2)) - + dda.afterPrepare.extraAccelerationClocks - ); - } - else - { - const uint64_t temp = (twoCsquaredTimesMmPerStepDivD * (uint32_t)dsK)/K2; - // Because of possible rounding error when the end speed is zero or very small, we need to check that the square root will work OK - nextCalcStepTime = (temp < twoDistanceToStopTimesCsquaredDivD) - ? dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - isqrt64(twoDistanceToStopTimesCsquaredDivD - temp) - : dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks; - } -#endif - - // When crossing between movement phases with high microstepping, due to rounding errors the next step may appear to be due before the last one. + // When crossing between movement phases with high microstepping, due to rounding errors the next step may appear to be due before the last one stepInterval = (nextCalcStepTime > nextStepTime) ? (nextCalcStepTime - nextStepTime) >> shiftFactor // calculate the time per step, ready for next time : 0; @@ -917,24 +654,6 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) nextStepTime = nextCalcStepTime; #endif - if (nextCalcStepTime > dda.clocksNeeded) - { - // The calculation makes this step late. - // When the end speed is very low, calculating the time of the last step is very sensitive to rounding error. - // So if this is the last step and it is late, bring it forward to the expected finish time. - // Very rarely, the penultimate step may be calculated late, so allow for that too. - if (nextStep + 1 >= totalSteps) - { - nextStepTime = dda.clocksNeeded; - } - else - { - // We don't expect any steps except the last two to be late - state = DMState::stepError; - stepInterval = 10000000 + nextStepTime; // so we can tell what happened in the debug print - return false; - } - } return true; } diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h index 565894fc..75ad8346 100644 --- a/src/Movement/DriveMovement.h +++ b/src/Movement/DriveMovement.h @@ -10,145 +10,31 @@ #include <RepRapFirmware.h> #include <Platform/Tasks.h> +#include "MoveSegment.h" class LinearDeltaKinematics; +class PrepParams; +class ExtruderShaper; #define DM_USE_FPU (__FPU_USED) -#define EVEN_STEPS (1) // 1 to generate steps at even intervals when doing double/quad/octal stepping -#define ROUND_TO_NEAREST (0) // 1 for round to nearest (as used in 1.20beta10), 0 for round down (as used prior to 1.20beta10) - -// Rounding functions, to improve code clarity. Also allows a quick switch between round-to-nearest and round down in the movement code. -inline uint32_t roundU32(float f) noexcept -{ -#if ROUND_TO_NEAREST - return (uint32_t)lrintf(f); -#else - return (uint32_t)f; -#endif -} - -inline uint32_t roundU32(double d) noexcept -{ -#if ROUND_TO_NEAREST - return lrint(d); -#else - return (uint32_t)d; -#endif -} - -inline int32_t roundS32(float f) noexcept -{ -#if ROUND_TO_NEAREST - return lrintf(f); -#else - return (int32_t)f; -#endif -} - -inline int32_t roundS32(double d) noexcept -{ -#if ROUND_TO_NEAREST - return lrint(d); -#else - return (int32_t)d; -#endif -} - -inline uint64_t roundU64(float f) noexcept -{ -#if ROUND_TO_NEAREST - return (uint64_t)llrintf(f); -#else - return (uint64_t)f; -#endif -} - -inline uint64_t roundU64(double d) noexcept -{ -#if ROUND_TO_NEAREST - return (uint64_t)llrint(d); -#else - return (uint64_t)d; -#endif -} - -inline int64_t roundS64(float f) noexcept -{ -#if ROUND_TO_NEAREST - return llrintf(f); -#else - return (int64_t)f; -#endif -} - -inline int64_t roundS64(double d) noexcept -{ -#if ROUND_TO_NEAREST - return llrint(d); -#else - return (int64_t)d; -#endif -} - -// Struct for passing parameters to the DriveMovement Prepare methods -struct PrepParams -{ - // Parameters used for all types of motion - float totalDistance; - float accelDistance; - float decelDistance; - float acceleration; - float deceleration; - float decelStartDistance; -#if DM_USE_FPU - float fTopSpeedTimesCdivD; -#else - uint32_t topSpeedTimesCdivD; -#endif - - // Parameters used only for extruders - float accelCompFactor; - -#if SUPPORT_CAN_EXPANSION - // Parameters used by CAN expansion - float accelTime, steadyTime, decelTime; - float initialSpeedFraction, finalSpeedFraction; -#endif - - // Parameters used only for delta moves - float initialX, initialY; -#if SUPPORT_CAN_EXPANSION - float finalX, finalY; - float zMovement; -#endif - const LinearDeltaKinematics *dparams; - float a2plusb2; // sum of the squares of the X and Y movement fractions -}; +#define EVEN_STEPS (1) // 1 to generate steps at even intervals when doing double/quad/octal stepping enum class DMState : uint8_t { idle = 0, - stepError = 1, + stepError, + // All higher values are various states of motion - accel0 = 2, - accel1, - accel2, - accel3, - accel4, - accel5, - accel6, - accel7, - steady, - decel0, - decel1, - decel2, - decel3, - decel4, - decel5, - decel6, - decel7, - reversing, - reverse + firstMotionState, + cartAccel = firstMotionState, // linear accelerating motion + cartLinear, // linear steady speed + cartDecelNoReverse, + cartDecelForwardsReversing, // linear decelerating motion, expect reversal + cartDecelReverse, // linear decelerating motion, reversed + + deltaForwardsNoReverse, // moving forwards, no reversal in this segment + deltaForwardsReversing, // moving forwards but reversing in this segment + deltaReverse, // moving in reverse }; // This class describes a single movement of one drive @@ -167,11 +53,7 @@ public: bool CalcNextStepTime(const DDA &dda) noexcept SPEED_CRITICAL; bool PrepareCartesianAxis(const DDA& dda, const PrepParams& params) noexcept SPEED_CRITICAL; bool PrepareDeltaAxis(const DDA& dda, const PrepParams& params) noexcept SPEED_CRITICAL; - bool PrepareExtruder(const DDA& dda, const PrepParams& params, float& extrusionPending, float speedChange, bool doCompensation) noexcept SPEED_CRITICAL; - -#if SUPPORT_REMOTE_COMMANDS - bool PrepareRemoteExtruder(const DDA& dda, const PrepParams& params) noexcept; -#endif + bool PrepareExtruder(const DDA& dda, const PrepParams& params) noexcept SPEED_CRITICAL; void DebugPrint() const noexcept; int32_t GetNetStepsLeft() const noexcept; @@ -181,18 +63,16 @@ public: uint32_t GetStepInterval(uint32_t microstepShift) const noexcept; // Get the current full step interval for this axis or extruder #endif -#if SUPPORT_CAN_EXPANSION - int32_t GetSteps() const noexcept { return (direction) ? totalSteps : -totalSteps; } -#endif - static void InitialAllocate(unsigned int num) noexcept; static unsigned int NumCreated() noexcept { return numCreated; } static DriveMovement *Allocate(size_t p_drive, DMState st) noexcept; static void Release(DriveMovement *item) noexcept; private: - bool CalcNextStepTimeCartesianFull(const DDA &dda) noexcept SPEED_CRITICAL; - bool CalcNextStepTimeDeltaFull(const DDA &dda) noexcept SPEED_CRITICAL; + bool CalcNextStepTimeFull(const DDA &dda) noexcept SPEED_CRITICAL; + bool NewCartesianSegment() noexcept SPEED_CRITICAL; + bool NewExtruderSegment() noexcept SPEED_CRITICAL; + bool NewDeltaSegment(const DDA& dda) noexcept SPEED_CRITICAL; static DriveMovement *freeList; static unsigned int numCreated; @@ -200,87 +80,65 @@ private: // Parameters common to Cartesian, delta and extruder moves DriveMovement *nextDM; // link to next DM that needs a step + MoveSegment *currentSegment; DMState state; // whether this is active or not uint8_t drive; // the drive that this DM controls uint8_t direction : 1, // true=forwards, false=backwards directionChanged : 1, // set by CalcNextStepTime if the direction is changed - fullCurrent : 1, // true if the drivers are set to the full current, false if they are set to the standstill current - isDelta : 1; // true if this DM uses segment-free delta kinematics + isDelta : 1, // true if this DM uses segment-free delta kinematics + isExtruder : 1; // true if this DM is for an extruder (only matters if !isDelta) uint8_t stepsTillRecalc; // how soon we need to recalculate uint32_t totalSteps; // total number of steps for this move // These values change as the step is executed, except for reverseStartStep uint32_t nextStep; // number of steps already done + uint32_t phaseStepLimit; // the first step number of the next phase, or the reverse start step if smaller uint32_t reverseStartStep; // the step number for which we need to reverse direction due to pressure advance or delta movement uint32_t nextStepTime; // how many clocks after the start of this move the next step is due uint32_t stepInterval; // how many clocks between steps + float distanceSoFar; #if DM_USE_FPU - float fMmPerStepTimesCdivtopSpeed; -#else - uint32_t mmPerStepTimesCKdivtopSpeed; -#endif - - // At this point we are 64-bit aligned - // The following only needs to be stored per-drive if we are supporting pressure advance -#if DM_USE_FPU - float fTwoDistanceToStopTimesCsquaredDivD; + float timeSoFar; + float pA, pB, pC; #else - uint64_t twoDistanceToStopTimesCsquaredDivD; -#endif - -#if DM_USE_FPU - float fTwoCsquaredTimesMmPerStepDivA; // 2 * clock^2 * mmPerStepInHyperCuboidSpace / acceleration - float fTwoCsquaredTimesMmPerStepDivD; // 2 * clock^2 * mmPerStepInHyperCuboidSpace / deceleration -#else - uint64_t twoCsquaredTimesMmPerStepDivA; // 2 * clock^2 * mmPerStepInHyperCuboidSpace / acceleration - uint64_t twoCsquaredTimesMmPerStepDivD; // 2 * clock^2 * mmPerStepInHyperCuboidSpace / deceleration + uint32_t iTimeSoFar; + int64_t ipA; + int32_t ipB, ipC; #endif // Parameters unique to a style of move (Cartesian, delta or extruder). Currently, extruders and Cartesian moves use the same parameters. - union MoveParams + union { - struct CartesianParameters // Parameters for Cartesian and extruder movement, including extruder pressure advance - { - // The following depend on how the move is executed, so they must be set up in Prepare() -#if DM_USE_FPU - float fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD; -#else - int64_t fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD; // this one can be negative -#endif - uint32_t accelStopStep; // the first step number at which we are no longer accelerating - uint32_t decelStartStep; // the first step number at which we are decelerating - uint32_t compensationClocks; // the pressure advance time in clocks - uint32_t accelCompensationClocks; // compensationClocks * (1 - startSpeed/topSpeed) - } cart; - struct DeltaParameters // Parameters for delta movement { -#if DM_USE_FPU // The following don't depend on how the move is executed, so they could be set up in Init() if we use fixed acceleration/deceleration + float fTwoA; + float fTwoB; + float h0MinusZ0; // the height subtended by the rod at the start of the move +#if DM_USE_FPU float fDSquaredMinusAsquaredMinusBsquaredTimesSsquared; - float fHmz0s; // the starting step position less the starting Z height, multiplied by the Z movement fraction and K (can go negative) + float fHmz0s; // the starting height less the starting Z height, multiplied by the Z movement fraction (can go negative) float fMinusAaPlusBbTimesS; - - // The following depend on how the move is executed, so they must be set up in Prepare() - float fAccelStopDs; - float fDecelStartDs; + float reverseStartDistance; // the overall move distance at which movement reversal occurs #else - // The following don't depend on how the move is executed, so they could be set up in Init() if we use fixed acceleration/deceleration int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared; int32_t hmz0sK; // the starting step position less the starting Z height, multiplied by the Z movement fraction and K (can go negative) int32_t minusAaPlusBbTimesKs; - - // The following depend on how the move is executed, so they must be set up in Prepare() - uint32_t accelStopDsK; - uint32_t decelStartDsK; #endif } delta; - } mp; - static constexpr uint32_t NoStepTime = 0xFFFFFFFF; // value to indicate that no further steps are needed when calculating the next step time + struct CartesianParameters + { + float pressureAdvanceK; // how much pressure advance is applied to this move + float effectiveStepsPerMm; // the steps/mm multiplied by the movement fraction + float effectiveMmPerStep; // reciprocal of [the steps/mm multiplied by the movement fraction] + float extraExtrusionDistance; // the extra extrusion distance in the acceleration phase + float extrusionBroughtForwards; // the amount of extrusion brought forwards from previous moves. Only needed for debug output. + } cart; + } mp; #if !DM_USE_FPU static constexpr uint32_t K1 = 1024; // a power of 2 used to multiply the value mmPerStepTimesCdivtopSpeed to reduce rounding errors @@ -314,7 +172,7 @@ inline bool DriveMovement::CalcNextStepTime(const DDA &dda) noexcept #endif return true; } - return (isDelta) ? CalcNextStepTimeDeltaFull(dda) : CalcNextStepTimeCartesianFull(dda); + return CalcNextStepTimeFull(dda); } state = DMState::idle; diff --git a/src/Movement/ExtruderShaper.cpp b/src/Movement/ExtruderShaper.cpp new file mode 100644 index 00000000..42d29563 --- /dev/null +++ b/src/Movement/ExtruderShaper.cpp @@ -0,0 +1,16 @@ +/* + * PressureAdvanceShaper.cpp + * + * Created on: 14 May 2021 + * Author: David + */ + +#include "ExtruderShaper.h" + +#include <GCodes/GCodeBuffer/GCodeBuffer.h> +#include <Platform/RepRap.h> +#include "StepTimer.h" +#include "DDA.h" +#include "MoveSegment.h" + +// End diff --git a/src/Movement/ExtruderShaper.h b/src/Movement/ExtruderShaper.h new file mode 100644 index 00000000..210f347d --- /dev/null +++ b/src/Movement/ExtruderShaper.h @@ -0,0 +1,38 @@ +/* + * PressureAdvanceShaper.h + * + * Created on: 14 May 2021 + * Author: David + */ + +#ifndef SRC_MOVEMENT_EXTRUDERSHAPER_H_ +#define SRC_MOVEMENT_EXTRUDERSHAPER_H_ + +#include <RepRapFirmware.h> + +class DDA; +class BasicPrepParams; +class MoveSegment; + +// This class implements MoveSegment generation for extruders with pressure advance. +// It also tracks extrusion that has be commanded but not implemented because less than one full step has been accumulated. +// Currently it only supports linear pressure advance. +class ExtruderShaper +{ +public: + ExtruderShaper() : k(0.0), extrusionPending(0.0) /*, lastSpeed(0.0)*/ { } + + // Temporary functions until we support more sophisticated pressure advance + float GetKclocks() const noexcept { return k; } // get pressure advance in step clocks + float GetKseconds() const noexcept { return k * (1.0/StepClockRate); } + void SetKseconds(float val) noexcept { k = val * StepClockRate; } // set pressure advance in seconds + float GetExtrusionPending() const noexcept { return extrusionPending; } + void SetExtrusionPending(float ep) noexcept { extrusionPending = ep; } + +private: + float k; // the pressure advance constant in step clocks + float extrusionPending; // extrusion we have been asked to do but haven't because it is less than one microstep, in mm +// float lastSpeed; // the speed we were moving at at the end of the last extrusion, needed to implement pressure advance +}; + +#endif /* SRC_MOVEMENT_EXTRUDERSHAPER_H_ */ diff --git a/src/Movement/InputShaper.cpp b/src/Movement/InputShaper.cpp deleted file mode 100644 index cc4d5f9a..00000000 --- a/src/Movement/InputShaper.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * InputShaper.cpp - * - * Created on: 20 Feb 2021 - * Author: David - */ - -#include "InputShaper.h" - -#include <GCodes/GCodeBuffer/GCodeBuffer.h> -#include <Platform/RepRap.h> -#include "StepTimer.h" - -// Object model table and functions -// Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17. -// Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM. - -// Macro to build a standard lambda function that includes the necessary type conversions -#define OBJECT_MODEL_FUNC(...) OBJECT_MODEL_FUNC_BODY(InputShaper, __VA_ARGS__) -#define OBJECT_MODEL_FUNC_IF(...) OBJECT_MODEL_FUNC_IF_BODY(InputShaper, __VA_ARGS__) - -constexpr ObjectModelTableEntry InputShaper::objectModelTable[] = -{ - // Within each group, these entries must be in alphabetical order - // 0. InputShaper members - { "damping", OBJECT_MODEL_FUNC(self->GetFloatDamping(), 2), ObjectModelEntryFlags::none }, - { "frequency", OBJECT_MODEL_FUNC(self->GetFrequency(), 2), ObjectModelEntryFlags::none }, - { "minimumAcceleration", OBJECT_MODEL_FUNC(self->minimumAcceleration, 1), ObjectModelEntryFlags::none }, - { "type", OBJECT_MODEL_FUNC(self->type.ToString()), ObjectModelEntryFlags::none }, -}; - -constexpr uint8_t InputShaper::objectModelTableDescriptor[] = { 1, 4 }; - -DEFINE_GET_OBJECT_MODEL_TABLE(InputShaper) - -InputShaper::InputShaper() noexcept - : halfPeriod((uint16_t)lrintf(StepTimer::StepClockRate/(2 * DefaultFrequency))), - damping(lrintf(DefaultDamping * 65536)), - minimumAcceleration(DefaultMinimumAcceleration), - type(InputShaperType::none) -{ -} - -// Process M593 -GCodeResult InputShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) -{ - const float MinimumInputShapingFrequency = (float)StepTimer::StepClockRate/(2 * 65535); // we use a 16-bit number of step clocks to represent half the input shaping period - const float MaximumInputShapingFrequency = 1000.0; - bool seen = false; - if (gb.Seen('F')) - { - seen = true; - halfPeriod = (float)StepTimer::StepClockRate/(2 * gb.GetLimitedFValue('F', MinimumInputShapingFrequency, MaximumInputShapingFrequency)); - } - if (gb.Seen('L')) - { - seen = true; - minimumAcceleration = max<float>(gb.GetFValue(), 1.0); // very low accelerations cause problems with the maths - } - if (gb.Seen('S')) - { - seen = true; - damping = (uint16_t)lrintf(63336 * gb.GetLimitedFValue('S', 0.0, 0.99)); - } - - if (gb.Seen('P')) - { - String<StringLength20> shaperName; - gb.GetReducedString(shaperName.GetRef()); - const InputShaperType newType(shaperName.c_str()); - if (!newType.IsValid()) - { - reply.printf("Unsupported input shaper type '%s'", shaperName.c_str()); - return GCodeResult::error; - } - seen = true; - type = newType; - } - else if (seen && type == InputShaperType::none) - { - // For backwards compatibility, if we have set input shaping parameters but not defined shaping type, default to DAA for now. Change this when we support better types of input shaping. - type = InputShaperType::daa; - } - - if (seen) - { - reprap.MoveUpdated(); - } - else if (type != InputShaperType::none) - { - reply.printf("Input shaping '%s' at %.1fHz damping factor %.2f, min. acceleration %.1f", - type.ToString(), (double)GetFrequency(), (double)GetFloatDamping(), (double)minimumAcceleration); - } - else - { - reply.copy("Input shaping is disabled"); - } - return GCodeResult::ok; -} - -// Return the full period in seconds -float InputShaper::GetFullPeriod() const noexcept -{ - return (float)halfPeriod/(float)(StepTimer::StepClockRate/2); -} - -float InputShaper::GetFrequency() const noexcept -{ - return (float)StepTimer::StepClockRate/(2.0 * (float)halfPeriod); -} - -float InputShaper::GetFloatDamping() const noexcept -{ - return ((float)damping)/65536; -} - -// End diff --git a/src/Movement/InputShaper.h b/src/Movement/InputShaper.h deleted file mode 100644 index 068c0a24..00000000 --- a/src/Movement/InputShaper.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * InputShaper.h - * - * Created on: 20 Feb 2021 - * Author: David - */ - -#ifndef SRC_MOVEMENT_INPUTSHAPER_H_ -#define SRC_MOVEMENT_INPUTSHAPER_H_ - -#include <RepRapFirmware.h> -#include <General/NamedEnum.h> -#include <ObjectModel/ObjectModel.h> - -// These names must be in alphabetical order and lowercase -NamedEnum(InputShaperType, uint8_t, - daa, - none, -); - -class InputShaper INHERIT_OBJECT_MODEL -{ -public: - InputShaper() noexcept; - - uint16_t GetHalfPeriodClocks() const noexcept { return halfPeriod; } // return the half period in step clocks - float GetFullPeriod() const noexcept; // return the full period in seconds - float GetFrequency() const noexcept; - float GetFloatDamping() const noexcept; - float GetMinimumAcceleration() const noexcept { return minimumAcceleration; } - InputShaperType GetType() const noexcept { return type; } - - GCodeResult Configure(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // process M593 - -protected: - DECLARE_OBJECT_MODEL - -private: - static constexpr float DefaultFrequency = 40.0; - static constexpr float DefaultDamping = 0.2; - static constexpr float DefaultMinimumAcceleration = 10.0; - - uint16_t halfPeriod; // half the period of ringing that we don't want to excite, in step clocks - uint16_t damping; // damping factor of the ringing as a 16-bit fractional number - float minimumAcceleration; // the minimum value that we reduce acceleration to - InputShaperType type; -}; - -#endif /* SRC_MOVEMENT_INPUTSHAPER_H_ */ diff --git a/src/Movement/InputShaperPlan.h b/src/Movement/InputShaperPlan.h new file mode 100644 index 00000000..e013103a --- /dev/null +++ b/src/Movement/InputShaperPlan.h @@ -0,0 +1,35 @@ +/* + * InputShaperPlan.h + * + * Created on: 7 Jun 2021 + * Author: David + */ + +#ifndef SRC_MOVEMENT_INPUTSHAPERPLAN_H_ +#define SRC_MOVEMENT_INPUTSHAPERPLAN_H_ + +#include <cstdint> + +union InputShaperPlan +{ + struct + { + uint32_t shapeAccelStart : 1, + shapeAccelEnd : 1, + shapeAccelOverlapped : 1, + shapeDecelStart : 1, + shapeDecelEnd : 1, + shapeDecelOverlapped : 1, + debugPrint : 1; + }; + uint32_t all; + + InputShaperPlan() noexcept : all(0) { } + + void Clear() noexcept { all = 0; } + + bool IsShaped() const noexcept { return shapeAccelStart || shapeAccelEnd || shapeAccelOverlapped || shapeDecelStart || shapeDecelEnd || shapeDecelOverlapped; } +}; + + +#endif /* SRC_MOVEMENT_INPUTSHAPERPLAN_H_ */ diff --git a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp index 83c9b3ae..a60498a0 100644 --- a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp +++ b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp @@ -823,7 +823,7 @@ void FiveBarScaraKinematics::MotorStepsToCartesian(const int32_t motorPos[], con } // Return true if the specified XY position is reachable by the print head reference point. -bool FiveBarScaraKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept +bool FiveBarScaraKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { if (axes.IsBitSet(X_AXIS) && axes.IsBitSet(Y_AXIS)) { @@ -835,7 +835,7 @@ bool FiveBarScaraKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap a } axes.ClearBit(X_AXIS); axes.ClearBit(Y_AXIS); - return Kinematics::IsReachable(axesCoords, axes, isCoordinated); + return Kinematics::IsReachable(axesCoords, axes); } // Return the initial Cartesian coordinates we assume after switching to this kinematics diff --git a/src/Movement/Kinematics/FiveBarScaraKinematics.h b/src/Movement/Kinematics/FiveBarScaraKinematics.h index 8fc137eb..6e7e9b3b 100644 --- a/src/Movement/Kinematics/FiveBarScaraKinematics.h +++ b/src/Movement/Kinematics/FiveBarScaraKinematics.h @@ -30,7 +30,7 @@ public: bool Configure(unsigned int mCode, GCodeBuffer& gb, const StringRef& reply, bool& error) THROWS(GCodeException) override; bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept override; - bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept override; + bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept override; LimitPositionResult LimitPosition(float coords[], const float * null initialCoords, size_t numVisibleAxes, AxesBitmap axesToLimit, bool isCoordinated, bool applyM208Limits) const noexcept override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const noexcept override; const char* HomingButtonNames() const noexcept override { return "PDZUVWABC"; } diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp index 850308b5..f75a5fd2 100644 --- a/src/Movement/Kinematics/Kinematics.cpp +++ b/src/Movement/Kinematics/Kinematics.cpp @@ -67,7 +67,7 @@ bool Kinematics::TryConfigureSegmentation(GCodeBuffer& gb) noexcept // 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 axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept +bool Kinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { const Platform& platform = reprap.GetPlatform(); return axes.IterateWhile([&platform, axesCoords](unsigned int axis, unsigned int count) -> bool { diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h index 6b1eaae1..5ef95b11 100644 --- a/src/Movement/Kinematics/Kinematics.h +++ b/src/Movement/Kinematics/Kinematics.h @@ -136,7 +136,7 @@ public: // Return true if the positions specified for the axes in the AxesBitmap are reachable by the print head reference point. // The default implementation assumes a rectangular reachable area, so it just uses the bed dimensions give in the M208 commands. - virtual bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept; + virtual bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept; // 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. diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp index 79935f35..b3c04a85 100644 --- a/src/Movement/Kinematics/PolarKinematics.cpp +++ b/src/Movement/Kinematics/PolarKinematics.cpp @@ -147,7 +147,7 @@ void PolarKinematics::MotorStepsToCartesian(const int32_t motorPos[], const floa } // Return true if the specified XY position is reachable by the print head reference point. -bool PolarKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept +bool PolarKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { if (axes.IsBitSet(X_AXIS) && axes.IsBitSet(Y_AXIS)) { @@ -159,7 +159,7 @@ bool PolarKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bo } axes.ClearBit(X_AXIS); axes.ClearBit(Y_AXIS); - return Kinematics::IsReachable(axesCoords, axes, isCoordinated); + return Kinematics::IsReachable(axesCoords, axes); } // Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed diff --git a/src/Movement/Kinematics/PolarKinematics.h b/src/Movement/Kinematics/PolarKinematics.h index 52a94d6c..f4652745 100644 --- a/src/Movement/Kinematics/PolarKinematics.h +++ b/src/Movement/Kinematics/PolarKinematics.h @@ -20,7 +20,7 @@ public: bool Configure(unsigned int mCode, GCodeBuffer& gb, const StringRef& reply, bool& error) THROWS(GCodeException) override; bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept override; - bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept override; + bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept override; LimitPositionResult LimitPosition(float finalCoords[], const float * null initialCoords, size_t numAxes, AxesBitmap axesToLimit, bool isCoordinated, bool applyM208Limits) const noexcept override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const noexcept override; const char* HomingButtonNames() const noexcept override { return "RTZUVWABC"; } diff --git a/src/Movement/Kinematics/RoundBedKinematics.cpp b/src/Movement/Kinematics/RoundBedKinematics.cpp index c35d68e3..b091e565 100644 --- a/src/Movement/Kinematics/RoundBedKinematics.cpp +++ b/src/Movement/Kinematics/RoundBedKinematics.cpp @@ -16,7 +16,7 @@ RoundBedKinematics::RoundBedKinematics(KinematicsType t, SegmentationType segTyp // Return true if the specified XY position is reachable by the print head reference point. -bool RoundBedKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept +bool RoundBedKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { if (axes.IsBitSet(X_AXIS) && axes.IsBitSet(Y_AXIS) && (fsquare(axesCoords[X_AXIS]) + fsquare(axesCoords[Y_AXIS]) >= printRadiusSquared)) { @@ -24,7 +24,7 @@ bool RoundBedKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, } axes.ClearBit(X_AXIS); axes.ClearBit(Y_AXIS); - return Kinematics::IsReachable(axesCoords, axes, isCoordinated); + return Kinematics::IsReachable(axesCoords, axes); } // Limit the speed and acceleration of a move to values that the mechanics can handle. diff --git a/src/Movement/Kinematics/RoundBedKinematics.h b/src/Movement/Kinematics/RoundBedKinematics.h index 29db1b7e..c378a6af 100644 --- a/src/Movement/Kinematics/RoundBedKinematics.h +++ b/src/Movement/Kinematics/RoundBedKinematics.h @@ -15,7 +15,7 @@ class RoundBedKinematics : public Kinematics { public: - bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept override; + bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept override; void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept override; AxesBitmap GetLinearAxes() const noexcept override; protected: diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp index 29b9650d..4a80872c 100644 --- a/src/Movement/Kinematics/ScaraKinematics.cpp +++ b/src/Movement/Kinematics/ScaraKinematics.cpp @@ -239,8 +239,8 @@ bool ScaraKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const Strin } } -// Return true if the specified XY position is reachable by the print head reference point, ignoring M208 limits. -bool ScaraKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept +// Return true if the specified XY position is reachable by the print head reference point +bool ScaraKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { if (axes.IsBitSet(X_AXIS) && axes.IsBitSet(Y_AXIS)) { @@ -248,14 +248,14 @@ bool ScaraKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bo float coords[2] = {axesCoords[X_AXIS], axesCoords[Y_AXIS]}; float theta, psi; bool armMode = currentArmMode; - if (!CalculateThetaAndPsi(coords, isCoordinated, theta, psi, armMode)) + if (!CalculateThetaAndPsi(coords, false, theta, psi, armMode)) { return false; } } axes.ClearBit(X_AXIS); axes.ClearBit(Y_AXIS); - return Kinematics::IsReachable(axesCoords, axes, isCoordinated); + return Kinematics::IsReachable(axesCoords, axes); } // Limit the Cartesian position that the user wants to move to, returning true if any coordinates were changed diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h index af9f52ed..003e483f 100644 --- a/src/Movement/Kinematics/ScaraKinematics.h +++ b/src/Movement/Kinematics/ScaraKinematics.h @@ -30,7 +30,7 @@ public: bool Configure(unsigned int mCode, GCodeBuffer& gb, const StringRef& reply, bool& error) THROWS(GCodeException) override; bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept override; void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const noexcept override; - bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, bool isCoordinated) const noexcept override; + bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept override; LimitPositionResult LimitPosition(float finalCoords[], const float * null initialCoords, size_t numAxes, AxesBitmap axesToLimit, bool isCoordinated, bool applyM208Limits) const noexcept override; void GetAssumedInitialPosition(size_t numAxes, float positions[]) const noexcept override; size_t NumHomingButtons(size_t numVisibleAxes) const noexcept override; diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp index b6fae1fa..11903961 100644 --- a/src/Movement/Move.cpp +++ b/src/Movement/Move.cpp @@ -47,12 +47,11 @@ #if SUPPORT_CAN_EXPANSION # include <CAN/CanMotion.h> +# include <CAN/CanInterface.h> #endif Task<Move::MoveTaskStackWords> Move::moveTask; -constexpr uint32_t MoveTimeout = 20; // normal timeout when the Move process is waiting for a new move - // Object model table and functions // Note: if using GCC version 7.3.1 20180622 and lambda functions are used in this table, you must compile this file with option -std=gnu++17. // Otherwise the table will be allocated in RAM instead of flash, which wastes too much RAM. @@ -86,69 +85,69 @@ constexpr ObjectModelTableEntry Move::objectModelTable[] = { // Within each group, these entries must be in alphabetical order // 0. Move members - { "axes", OBJECT_MODEL_FUNC_NOSELF(&axesArrayDescriptor), ObjectModelEntryFlags::live }, - { "calibration", OBJECT_MODEL_FUNC(self, 3), ObjectModelEntryFlags::none }, - { "compensation", OBJECT_MODEL_FUNC(self, 6), ObjectModelEntryFlags::none }, - { "currentMove", OBJECT_MODEL_FUNC(self, 2), ObjectModelEntryFlags::live }, - { "extruders", OBJECT_MODEL_FUNC_NOSELF(&extrudersArrayDescriptor), ObjectModelEntryFlags::live }, - { "idle", OBJECT_MODEL_FUNC(self, 1), ObjectModelEntryFlags::none }, - { "kinematics", OBJECT_MODEL_FUNC(self->kinematics), ObjectModelEntryFlags::none }, - { "printingAcceleration", OBJECT_MODEL_FUNC(self->maxPrintingAcceleration, 1), ObjectModelEntryFlags::none }, - { "queue", OBJECT_MODEL_FUNC_NOSELF(&queueArrayDescriptor), ObjectModelEntryFlags::none }, - { "shaping", OBJECT_MODEL_FUNC(&self->shaper, 0), ObjectModelEntryFlags::none }, - { "speedFactor", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetSpeedFactor(), 2), ObjectModelEntryFlags::none }, - { "travelAcceleration", OBJECT_MODEL_FUNC(self->maxTravelAcceleration, 1), ObjectModelEntryFlags::none }, - { "virtualEPos", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetVirtualExtruderPosition(), 5), ObjectModelEntryFlags::live }, + { "axes", OBJECT_MODEL_FUNC_NOSELF(&axesArrayDescriptor), ObjectModelEntryFlags::live }, + { "calibration", OBJECT_MODEL_FUNC(self, 3), ObjectModelEntryFlags::none }, + { "compensation", OBJECT_MODEL_FUNC(self, 6), ObjectModelEntryFlags::none }, + { "currentMove", OBJECT_MODEL_FUNC(self, 2), ObjectModelEntryFlags::live }, + { "extruders", OBJECT_MODEL_FUNC_NOSELF(&extrudersArrayDescriptor), ObjectModelEntryFlags::live }, + { "idle", OBJECT_MODEL_FUNC(self, 1), ObjectModelEntryFlags::none }, + { "kinematics", OBJECT_MODEL_FUNC(self->kinematics), ObjectModelEntryFlags::none }, + { "printingAcceleration", OBJECT_MODEL_FUNC(InverseConvertAcceleration(self->maxPrintingAcceleration), 1), ObjectModelEntryFlags::none }, + { "queue", OBJECT_MODEL_FUNC_NOSELF(&queueArrayDescriptor), ObjectModelEntryFlags::none }, + { "shaping", OBJECT_MODEL_FUNC(&self->axisShaper, 0), ObjectModelEntryFlags::none }, + { "speedFactor", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetSpeedFactor(), 2), ObjectModelEntryFlags::none }, + { "travelAcceleration", OBJECT_MODEL_FUNC(InverseConvertAcceleration(self->maxTravelAcceleration), 1), ObjectModelEntryFlags::none }, + { "virtualEPos", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetVirtualExtruderPosition(), 5), ObjectModelEntryFlags::live }, { "workplaceNumber", OBJECT_MODEL_FUNC_NOSELF((int32_t)reprap.GetGCodes().GetWorkplaceCoordinateSystemNumber() - 1), ObjectModelEntryFlags::none }, - { "workspaceNumber", OBJECT_MODEL_FUNC_NOSELF((int32_t)reprap.GetGCodes().GetWorkplaceCoordinateSystemNumber()), ObjectModelEntryFlags::obsolete }, + { "workspaceNumber", OBJECT_MODEL_FUNC_NOSELF((int32_t)reprap.GetGCodes().GetWorkplaceCoordinateSystemNumber()), ObjectModelEntryFlags::obsolete }, // 1. Move.Idle members - { "factor", OBJECT_MODEL_FUNC_NOSELF(reprap.GetPlatform().GetIdleCurrentFactor(), 1), ObjectModelEntryFlags::none }, - { "timeout", OBJECT_MODEL_FUNC(0.001f * (float)self->idleTimeout, 1), ObjectModelEntryFlags::none }, + { "factor", OBJECT_MODEL_FUNC_NOSELF(reprap.GetPlatform().GetIdleCurrentFactor(), 1), ObjectModelEntryFlags::none }, + { "timeout", OBJECT_MODEL_FUNC(0.001f * (float)self->idleTimeout, 1), ObjectModelEntryFlags::none }, // 2. move.currentMove members - { "acceleration", OBJECT_MODEL_FUNC(self->GetAcceleration(), 1), ObjectModelEntryFlags::live }, - { "deceleration", OBJECT_MODEL_FUNC(self->GetDeceleration(), 1), ObjectModelEntryFlags::live }, + { "acceleration", OBJECT_MODEL_FUNC(InverseConvertAcceleration(self->GetAcceleration()), 1), ObjectModelEntryFlags::live }, + { "deceleration", OBJECT_MODEL_FUNC(InverseConvertAcceleration(self->GetDeceleration()), 1), ObjectModelEntryFlags::live }, # if SUPPORT_LASER { "laserPwm", OBJECT_MODEL_FUNC_IF_NOSELF(reprap.GetGCodes().GetMachineType() == MachineType::laser, - reprap.GetPlatform().GetLaserPwm(), 2), ObjectModelEntryFlags::live }, + reprap.GetPlatform().GetLaserPwm(), 2), ObjectModelEntryFlags::live }, # endif - { "requestedSpeed", OBJECT_MODEL_FUNC(self->GetRequestedSpeed(), 1), ObjectModelEntryFlags::live }, - { "topSpeed", OBJECT_MODEL_FUNC(self->GetTopSpeed(), 1), ObjectModelEntryFlags::live }, + { "requestedSpeed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->GetRequestedSpeed()), 1), ObjectModelEntryFlags::live }, + { "topSpeed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->GetTopSpeed()), 1), ObjectModelEntryFlags::live }, // 3. move.calibration members - { "final", OBJECT_MODEL_FUNC(self, 5), ObjectModelEntryFlags::none }, - { "initial", OBJECT_MODEL_FUNC(self, 4), ObjectModelEntryFlags::none }, - { "numFactors", OBJECT_MODEL_FUNC((int32_t)self->numCalibratedFactors), ObjectModelEntryFlags::none }, + { "final", OBJECT_MODEL_FUNC(self, 5), ObjectModelEntryFlags::none }, + { "initial", OBJECT_MODEL_FUNC(self, 4), ObjectModelEntryFlags::none }, + { "numFactors", OBJECT_MODEL_FUNC((int32_t)self->numCalibratedFactors), ObjectModelEntryFlags::none }, // 4. move.calibration.initialDeviation members - { "deviation", OBJECT_MODEL_FUNC(self->initialCalibrationDeviation.GetDeviationFromMean(), 3), ObjectModelEntryFlags::none }, - { "mean", OBJECT_MODEL_FUNC(self->initialCalibrationDeviation.GetMean(), 3), ObjectModelEntryFlags::none }, + { "deviation", OBJECT_MODEL_FUNC(self->initialCalibrationDeviation.GetDeviationFromMean(), 3), ObjectModelEntryFlags::none }, + { "mean", OBJECT_MODEL_FUNC(self->initialCalibrationDeviation.GetMean(), 3), ObjectModelEntryFlags::none }, // 5. move.calibration.finalDeviation members - { "deviation", OBJECT_MODEL_FUNC(self->latestCalibrationDeviation.GetDeviationFromMean(), 3), ObjectModelEntryFlags::none }, - { "mean", OBJECT_MODEL_FUNC(self->latestCalibrationDeviation.GetMean(), 3), ObjectModelEntryFlags::none }, + { "deviation", OBJECT_MODEL_FUNC(self->latestCalibrationDeviation.GetDeviationFromMean(), 3), ObjectModelEntryFlags::none }, + { "mean", OBJECT_MODEL_FUNC(self->latestCalibrationDeviation.GetMean(), 3), ObjectModelEntryFlags::none }, // 6. move.compensation members { "fadeHeight", OBJECT_MODEL_FUNC((self->useTaper) ? self->taperHeight : std::numeric_limits<float>::quiet_NaN(), 1), ObjectModelEntryFlags::none }, #if HAS_MASS_STORAGE || HAS_LINUX_INTERFACE - { "file", OBJECT_MODEL_FUNC_IF(self->usingMesh, self->heightMap.GetFileName()), ObjectModelEntryFlags::none }, + { "file", OBJECT_MODEL_FUNC_IF(self->usingMesh, self->heightMap.GetFileName()), ObjectModelEntryFlags::none }, #endif - { "liveGrid", OBJECT_MODEL_FUNC_IF(self->usingMesh, (const GridDefinition *)&self->GetGrid()), ObjectModelEntryFlags::none }, - { "meshDeviation", OBJECT_MODEL_FUNC_IF(self->usingMesh, self, 7), ObjectModelEntryFlags::none }, - { "probeGrid", OBJECT_MODEL_FUNC_NOSELF((const GridDefinition *)&reprap.GetGCodes().GetDefaultGrid()), ObjectModelEntryFlags::none }, - { "skew", OBJECT_MODEL_FUNC(self, 8), ObjectModelEntryFlags::none }, - { "type", OBJECT_MODEL_FUNC(self->GetCompensationTypeString()), ObjectModelEntryFlags::none }, + { "liveGrid", OBJECT_MODEL_FUNC_IF(self->usingMesh, (const GridDefinition *)&self->GetGrid()), ObjectModelEntryFlags::none }, + { "meshDeviation", OBJECT_MODEL_FUNC_IF(self->usingMesh, self, 7), ObjectModelEntryFlags::none }, + { "probeGrid", OBJECT_MODEL_FUNC_NOSELF((const GridDefinition *)&reprap.GetGCodes().GetDefaultGrid()), ObjectModelEntryFlags::none }, + { "skew", OBJECT_MODEL_FUNC(self, 8), ObjectModelEntryFlags::none }, + { "type", OBJECT_MODEL_FUNC(self->GetCompensationTypeString()), ObjectModelEntryFlags::none }, // 7. move.compensation.meshDeviation members - { "deviation", OBJECT_MODEL_FUNC(self->latestMeshDeviation.GetDeviationFromMean(), 3), ObjectModelEntryFlags::none }, - { "mean", OBJECT_MODEL_FUNC(self->latestMeshDeviation.GetMean(), 3), ObjectModelEntryFlags::none }, + { "deviation", OBJECT_MODEL_FUNC(self->latestMeshDeviation.GetDeviationFromMean(), 3), ObjectModelEntryFlags::none }, + { "mean", OBJECT_MODEL_FUNC(self->latestMeshDeviation.GetMean(), 3), ObjectModelEntryFlags::none }, // 8. move.compensation.skew members - { "compensateXY", OBJECT_MODEL_FUNC(self->compensateXY), ObjectModelEntryFlags::none }, - { "tanXY", OBJECT_MODEL_FUNC(self->tanXY, 4), ObjectModelEntryFlags::none }, - { "tanXZ", OBJECT_MODEL_FUNC(self->tanXZ, 4), ObjectModelEntryFlags::none }, - { "tanYZ", OBJECT_MODEL_FUNC(self->tanYZ, 4), ObjectModelEntryFlags::none }, + { "compensateXY", OBJECT_MODEL_FUNC(self->compensateXY), ObjectModelEntryFlags::none }, + { "tanXY", OBJECT_MODEL_FUNC(self->tanXY, 4), ObjectModelEntryFlags::none }, + { "tanXZ", OBJECT_MODEL_FUNC(self->tanXZ, 4), ObjectModelEntryFlags::none }, + { "tanYZ", OBJECT_MODEL_FUNC(self->tanYZ, 4), ObjectModelEntryFlags::none }, }; constexpr uint8_t Move::objectModelTableDescriptor[] = { 9, 15, 2, 4 + SUPPORT_LASER, 3, 2, 2, 6 + (HAS_MASS_STORAGE || HAS_LINUX_INTERFACE), 2, 4 }; @@ -166,7 +165,7 @@ Move::Move() noexcept #if SUPPORT_ASYNC_MOVES heightController(nullptr), #endif - maxPrintingAcceleration(10000.0), maxTravelAcceleration(10000.0), + maxPrintingAcceleration(ConvertAcceleration(DefaultPrintingAcceleration)), maxTravelAcceleration(ConvertAcceleration(DefaultTravelAcceleration)), jerkPolicy(0), numCalibratedFactors(0) { @@ -302,7 +301,7 @@ void Move::Exit() noexcept } // Let the DDA ring process moves. Better to have a few moves in the queue so that we can do lookahead, hence the test on idleCount and idleTime. - mainDDARing.Spin(simulationMode, !canAddMove || millis() - idleStartTime >= mainDDARing.GetGracePeriod()); + uint32_t nextPrepareDelay = mainDDARing.Spin(simulationMode, !canAddMove || millis() - idleStartTime >= mainDDARing.GetGracePeriod()); #if SUPPORT_ASYNC_MOVES if (auxMoveAvailable && auxDDARing.CanAddMove()) @@ -313,7 +312,11 @@ void Move::Exit() noexcept } auxMoveAvailable = false; } - auxDDARing.Spin(simulationMode, true); // let the DDA ring process moves + const uint32_t auxPrepareDelay = auxDDARing.Spin(simulationMode, true); // let the DDA ring process moves + if (auxPrepareDelay < nextPrepareDelay) + { + nextPrepareDelay = auxPrepareDelay; + } #endif // Reduce motor current to standby if the rings have been idle for long enough @@ -343,7 +346,7 @@ void Move::Exit() noexcept if (!moveRead) { - TaskBase::Take(MoveTimeout); + TaskBase::Take(nextPrepareDelay); } } } @@ -407,14 +410,7 @@ bool Move::IsRawMotorMove(uint8_t moveType) const noexcept // Return true if the specified point is accessible to the Z probe bool Move::IsAccessibleProbePoint(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept { - const auto zp = reprap.GetPlatform().GetEndstops().GetZProbe(reprap.GetGCodes().GetCurrentZProbeNumber()); - if (zp.IsNotNull()) - { - axes.Iterate([axesCoords, &zp](unsigned int axis, unsigned int) { - axesCoords[axis] -= zp->GetOffset(axis); - }); - } - return kinematics->IsReachable(axesCoords, axes, false); + return kinematics->IsReachable(axesCoords, axes); } // 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. @@ -444,8 +440,8 @@ void Move::Diagnostics(MessageType mtype) noexcept scratchString.copy(GetCompensationTypeString()); Platform& p = reprap.GetPlatform(); - p.MessageF(mtype, "=== Move ===\nDMs created %u, maxWait %" PRIu32 "ms, bed compensation in use: %s, comp offset %.3f\n", - DriveMovement::NumCreated(), longestGcodeWaitInterval, scratchString.c_str(), (double)zShift); + p.MessageF(mtype, "=== Move ===\nDMs created %u, segments created %u, maxWait %" PRIu32 "ms, bed compensation in use: %s, comp offset %.3f\n", + DriveMovement::NumCreated(), MoveSegment::NumCreated(), longestGcodeWaitInterval, scratchString.c_str(), (double)zShift); longestGcodeWaitInterval = 0; #if 0 // debug only @@ -973,17 +969,17 @@ GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) { // For backwards compatibility with old versions of Marlin (e.g. for Cura and the Prusa fork of slic3r), set both accelerations seen = true; - maxTravelAcceleration = maxPrintingAcceleration = gb.GetFValue(); + maxTravelAcceleration = maxPrintingAcceleration = gb.GetAcceleration(); } if (gb.Seen('P')) { seen = true; - maxPrintingAcceleration = gb.GetFValue(); + maxPrintingAcceleration = gb.GetAcceleration(); } if (gb.Seen('T')) { seen = true; - maxTravelAcceleration = gb.GetFValue(); + maxTravelAcceleration = gb.GetAcceleration(); } if (seen) { @@ -991,7 +987,8 @@ GCodeResult Move::ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) } else { - reply.printf("Maximum printing acceleration %.1f, maximum travel acceleration %.1f", (double)maxPrintingAcceleration, (double)maxTravelAcceleration); + reply.printf("Maximum printing acceleration %.1f, maximum travel acceleration %.1f mm/sec^2", + (double)InverseConvertAcceleration(maxPrintingAcceleration), (double)InverseConvertAcceleration(maxTravelAcceleration)); } return GCodeResult::ok; } @@ -1003,6 +1000,127 @@ GCodeResult Move::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply return rings[ringNumber].ConfigureMovementQueue(gb, reply); } +// Process M572 +GCodeResult Move::ConfigurePressureAdvance(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException) +{ + if (gb.Seen('S')) + { + const float advance = gb.GetFValue(); + if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb)) + { + return GCodeResult::notFinished; + } + + GCodeResult rslt = GCodeResult::ok; + +#if SUPPORT_CAN_EXPANSION + CanDriversData<float> canDriversToUpdate; +#endif + if (gb.Seen('D')) + { + uint32_t eDrive[MaxExtruders]; + size_t eCount = MaxExtruders; + gb.GetUnsignedArray(eDrive, eCount, false); +#if SUPPORT_CAN_EXPANSION + Platform& platform = reprap.GetPlatform(); +#endif + for (size_t i = 0; i < eCount; i++) + { + const uint32_t extruder = eDrive[i]; + if (extruder >= reprap.GetGCodes().GetNumExtruders()) + { + reply.printf("Invalid extruder number '%" PRIu32 "'", extruder); + rslt = GCodeResult::error; + break; + } + extruderShapers[extruder].SetKseconds(advance); +#if SUPPORT_CAN_EXPANSION + const DriverId did = platform.GetExtruderDriver(extruder); + if (did.IsRemote()) + { + canDriversToUpdate.AddEntry(did, advance); + } +#endif + } + } + else + { + const Tool * const ct = reprap.GetCurrentTool(); + if (ct == nullptr) + { + reply.copy("No tool selected"); + rslt = GCodeResult::error; + } + else + { +#if SUPPORT_CAN_EXPANSION + ct->IterateExtruders([this, advance, &canDriversToUpdate](unsigned int extruder) + { + extruderShapers[extruder].SetKseconds(advance); + const DriverId did = reprap.GetPlatform().GetExtruderDriver(extruder); + if (did.IsRemote()) + { + canDriversToUpdate.AddEntry(did, advance); + } + } + ); +#else + ct->IterateExtruders([this, advance](unsigned int extruder) + { + extruderShapers[extruder].SetKseconds(advance); + } + ); +#endif + } + } + +#if SUPPORT_CAN_EXPANSION + return max(rslt, CanInterface::SetRemotePressureAdvance(canDriversToUpdate, reply)); +#else + return rslt; +#endif + } + + reply.copy("Extruder pressure advance"); + char c = ':'; + for (size_t i = 0; i < reprap.GetGCodes().GetNumExtruders(); ++i) + { + reply.catf("%c %.3f", c, (double)extruderShapers[i].GetKseconds()); + c = ','; + } + return GCodeResult::ok; +} + +#if SUPPORT_REMOTE_COMMANDS + +GCodeResult Move::EutSetRemotePressureAdvance(const CanMessageMultipleDrivesRequest<float>& msg, size_t dataLength, const StringRef& reply) noexcept +{ + const auto drivers = Bitmap<uint16_t>::MakeFromRaw(msg.driversToUpdate); + if (dataLength < msg.GetActualDataLength(drivers.CountSetBits())) + { + reply.copy("bad data length"); + return GCodeResult::error; + } + + GCodeResult rslt = GCodeResult::ok; + drivers.Iterate([this, &msg, &reply, &rslt](unsigned int driver, unsigned int count) -> void + { + if (driver >= NumDirectDrivers) + { + reply.lcatf("No such driver %u.%u", CanInterface::GetCanAddress(), driver); + rslt = GCodeResult::error; + } + else + { + extruderShapers[driver].SetKseconds(msg.values[count]); + } + } + ); + return rslt; +} + +#endif + // Return the current live XYZ and extruder coordinates // Interrupts are assumed enabled on entry float Move::LiveCoordinate(unsigned int axisOrExtruder, const Tool *tool) noexcept diff --git a/src/Movement/Move.h b/src/Movement/Move.h index 19df4f89..1e5db53d 100644 --- a/src/Movement/Move.h +++ b/src/Movement/Move.h @@ -9,7 +9,8 @@ #define MOVE_H_ #include <RepRapFirmware.h> -#include "InputShaper.h" +#include "AxisShaper.h" +#include "ExtruderShaper.h" #include "DDARing.h" #include "DDA.h" // needed because of our inline functions #include "BedProbing/RandomProbePointSet.h" @@ -93,10 +94,18 @@ public: GCodeResult ConfigureAccelerations(GCodeBuffer&gb, const StringRef& reply) THROWS(GCodeException); // process M204 GCodeResult ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // process M595 + GCodeResult ConfigurePressureAdvance(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); // process M572 + + float GetPressureAdvanceClocks(size_t extruder) const noexcept; + +#if SUPPORT_REMOTE_COMMANDS + GCodeResult EutSetRemotePressureAdvance(const CanMessageMultipleDrivesRequest<float>& msg, size_t dataLength, const StringRef& reply) noexcept; +#endif float GetMaxPrintingAcceleration() const noexcept { return maxPrintingAcceleration; } float GetMaxTravelAcceleration() const noexcept { return maxTravelAcceleration; } - InputShaper& GetShaper() noexcept { return shaper; } + AxisShaper& GetAxisShaper() noexcept { return axisShaper; } + ExtruderShaper& GetExtruderShaper(size_t extruder) noexcept { return extruderShapers[extruder]; } void Diagnostics(MessageType mtype) noexcept; // Report useful stuff @@ -195,10 +204,17 @@ public: static const TaskBase *GetMoveTaskHandle() noexcept { return &moveTask; } #if SUPPORT_REMOTE_COMMANDS +# if USE_REMOTE_INPUT_SHAPING + void AddShapeddMoveFromRemote(const CanMessageMovementLinearShaped& msg) noexcept // add a move from the ATE to the movement queue + { + mainDDARing.AddMoveFromRemote(msg); + } +# else void AddMoveFromRemote(const CanMessageMovementLinear& msg) noexcept // add a move from the ATE to the movement queue { mainDDARing.AddMoveFromRemote(msg); } +# endif #endif protected: @@ -273,7 +289,13 @@ private: Kinematics *kinematics; // What kinematics we are using - InputShaper shaper; + AxisShaper axisShaper; + +#if SUPPORT_REMOTE_COMMANDS + ExtruderShaper extruderShapers[max<size_t>(MaxExtruders, NumDirectDrivers)]; +#else + ExtruderShaper extruderShapers[MaxExtruders]; +#endif float latestLiveCoordinates[MaxAxesPlusExtruders]; float specialMoveCoords[MaxDriversPerAxis]; // Amounts by which to move individual Z motors (leadscrew adjustment move) @@ -315,6 +337,11 @@ inline void Move::ResetExtruderPositions() noexcept mainDDARing.ResetExtruderPositions(); } +inline float Move::GetPressureAdvanceClocks(size_t extruder) const noexcept +{ + return (extruder < MaxExtruders) ? extruderShapers[extruder].GetKclocks() : 0.0; +} + #if HAS_SMART_DRIVERS // Get the current step interval for this axis or extruder, or 0 if it is not moving diff --git a/src/Movement/MoveSegment.cpp b/src/Movement/MoveSegment.cpp new file mode 100644 index 00000000..f2f8c244 --- /dev/null +++ b/src/Movement/MoveSegment.cpp @@ -0,0 +1,64 @@ +/* + * MoveSegment.cpp + * + * Created on: 26 Feb 2021 + * Author: David + */ + +#include "MoveSegment.h" + +// Static members + +MoveSegment *MoveSegment::freeList = nullptr; +unsigned int MoveSegment::numCreated = 0; + +void MoveSegment::InitialAllocate(unsigned int num) noexcept +{ + while (num > numCreated) + { + freeList = new MoveSegment(freeList); + ++numCreated; + } +} + +// Allocate a MoveSegment, from the freelist if possible, else create a new one. Not thread-safe. Clears the flags. +MoveSegment *MoveSegment::Allocate(MoveSegment *next) noexcept +{ + MoveSegment * ms = freeList; + if (ms != nullptr) + { + freeList = ms->GetNext(); + ms->nextAndFlags = reinterpret_cast<uint32_t>(next); + } + else + { + ms = new MoveSegment(next); + ++numCreated; + } + return ms; +} + +void MoveSegment::AddToTail(MoveSegment *tail) noexcept +{ + MoveSegment *seg = this; + while (seg->GetNext() != nullptr) + { + seg = seg->GetNext(); + } + seg->SetNext(tail); +} + +void MoveSegment::DebugPrint(char ch) const noexcept +{ + debugPrintf("%c d=%.3e t=%" PRIu32 " ", ch, (double)segmentLength, (uint32_t)segTime); + if (IsLinear()) + { + debugPrintf("c=%.3e\n", (double)c); + } + else + { + debugPrintf("b=%.3e c=%.3e\n", (double)b, (double)c); + } +} + +// End diff --git a/src/Movement/MoveSegment.h b/src/Movement/MoveSegment.h new file mode 100644 index 00000000..10917f74 --- /dev/null +++ b/src/Movement/MoveSegment.h @@ -0,0 +1,285 @@ +/* + * MoveSegment.h + * + * Created on: 26 Feb 2021 + * Author: David + * + * This class holds the parameters of a segment of a move. The idea is that when preparing the move, we pre-calculate as much as possible so that the step time calculation in the step ISR is fast. + * In particular, we wish to avoid division as much as possible when generating steps. + * + * To save memory, we wish to share move segments between motors as far as possible. We can do this if all axes use the same input shaping, at the expense of an additional multiplication in the step ISR. + * The move segments for axes refer to the move as a whole e.g. the composite linear axis movement. For each axis, when starting the move segment we scale the C factor by 1.0/(f*m) + * where f is the move fraction and m s the steps/mm. We can do this using a multiplication if we precalculate 1.0/(f*m) when preparing the move and store it in the DM. + * + * To handle extruders with pressure advance, we have a few options: + * 1. Use separate MoveSegments for the extruders. Each extruder will need its own MoveSegment chain, except that multiple extruders using the same pressure advance constant could share a chain if we + * scale by 1.0/(f*m) again. We don't need to apply input shaping to extruders because we assume that extruders don't react quickly enough. to acceleration changes lasting up to ~30ms. + * 2. Use the axis move segments. When starting a new segment, each extruder will need to calculate and store that modified A B C coefficients to take account of PA. When input shaping is used, + * shaping will be applied to extruders (so there will be more MoveSegments to process). This would save memory by using fewer MoveSegments, but require more computation. + * 3. Use a common MoveSegment chain without input shaping for all extruders (which could be shared with the axes when the move does not use input shaping), and adjust the A B C coefficients to account for PA. + * + * To handle delta movement, we convert the step count to distance along the overall linear trajectory (accounting for the possible reversal), then convert that distance to time using the axis MoveSegments. + * + * Let: + * s = distance travelled by this axis or extruder since the start of the entire move + * s0 = distance travelled by this axis or extruder at the start of this segment + * a = acceleration of the move as a whole taking multiple axes into account + * d = deceleration of the move as a whole taking multiple axes into account (so d is positive) + * u = initial speed of the move segment as a whole + * f = the fraction of the move that this axis/extruder uses + * m = the steps/mm for this axis or extruder + * n = number of steps taken since start of move + * t = time since start of move + * ts = segment start time + * The basic motion equation for an acceleration segment is: + * s = s0 + u*f*(t - ts) + 0.5*a*f*(t - ts)t^2 + * Solving for t: + * t = ts - u/a + sqrt((-u/a)^2 + 2*(s-s0)/(a*f)) + * If we take s0 = S0 * f: + * t = ts - u/a + sqrt((-u/a)^2 + 2*s/(a*f) - 2*S0/a) + * Substituting s = n/m: + * t = ts - u/a + sqrt((-u/a)^2 + 2*n/(f*m*a) - 2*S0/a) + * For a deceleration segment, just set a = -d: + * t = ts + u/d + sqrt((u/d)^2 - 2*n/(f*m*d) + 2*S0/d) + * For a linear segment: + * s = s0 + u*f*(t - ts) + * from which: + * t = ts + (s - s0)/(u*f) + * If we take s0 = S0 * f: + * t = ts - S0/u + s/(u*f) + * Substituting s = n/m: + * t = n/(u*f*m) + ts - S0/u + * + * Now add pressure advance with constant k seconds. + * For the acceleration segment, u is replaced by u+(k*a): + * t = ts - u/a - k + sqrt((-u/a - k)^2 + 2*n/(f*m*a) - 2*S0/a) + * For the deceleration segment, u is replaced by u-(k*d). Additionally, S0 is replaced by S0 + EAD where EAD stands for extra acceleration distance, EAD = (vaccel-uaccel)*k = a*T*k where T was the acceleration time: + * t = ts + u/d - k + sqrt((u/d - k)^2 - 2*n/(f*m*d) + 2*(S0 + EAD)/d) + * For the linear segment, S0 must include EAD again: + * t = ts - (S0 + EAD)/u + n/(u*f*m) + * Now assume that there is also a fractional step p brought forward, where 0 <= p < 1.0. This must be subtracted from s0. Equivalently, add p/f to S0. + * Acceleration segment: + * t = ts - u/a - k + sqrt((-u/a - k)^2 + 2*n/(f*m*a) - 2*(S0 + p/f)/a) + * Deceleration segment: + * t = ts + u/d - k + sqrt((u/d - k)^2 - 2*n/(f*m*d) + 2*(S0 + p/f + EAD)/d) + * Linear segment: + * t = ts - (S0 + p/f + EAD)/u + n/(u*f*m) + * + * We can summarise like this. For an acceleration or deceleration segment: + * t = ts + B + sqrt(A + C*n/(f*m)) + * where for an acceleration segment: + * B = -u/a - k + * A = B^2 - 2*(S0 + p/f)/a = B^2 - C * (S0 + p/f) + * C = 2/a + * and for a deceleration segment: + * B = u/d - k + * A = B^2 + 2*(S0 + EAD + p/f)/d = B^2 - C * (S0 + p/f) + * C = -2/d + * For a linear segment: + * t = ts + B + C*n/(f*m) + * where: + * B = -(S0 + EAD + p/f)/u + * C = 1/u + * + * For accelerating moves we can interpret (-A/C) as the (negative) distance at which the move reversed, and B is the time at which it reversed (negative so in the past) + * For decelerating moves we can interpret (A/C) as the distance at which the move will reverse, B is the time at which it will reverse (positive so in the future) + * + * The move segment stores a flag indicating whether the segment is accel/decel or linear, the length of this segment D = (S - S0) + EAD, and the time T for this segment in step clocks. + * For an accel/decel segment it also stores the coefficients B and C. For a linear segment, just C. + * From the distance limit we can work out the step limit N = D/(f*m). + * From the segment times we can accumulate the start time ts of each segment. + * + * When starting a new axis segment we calculate actual coefficients A' B' C' as follows (Dprev is the distance limit of the previous segment, i.e. total length of previous moves): + * For accel/decel segments: + * A' = B^2 - C*Dprev + * B' = B + ts + * C' = C * 1/(f*m) + * (for axis segments we could instead include ts in B, but we can't do that for common extruder segments, see later) + * For linear segments: + * B' = Dprev*C + ts + * C' = C * 1/(f*m) + * + * Assuming we share common segments between all extruders and account for PA and fractional steps when starting them, then when starting a segment we need to calculate the coefficients as follows: + * For accel/decel segments: + * A' = (B - k)^2 + C*(Dprev + p/f) + * B' = (B - k) + ts + * C' = C * 1/(f*m) + * For linear segments: + * B' = -(Dprev + p/f)*C + ts + * C' = C * 1/(f*m) + * + * When preparing a move we need to: + * Set up the axis and extruder move segments + * For each linear axis and extruder, calculate and store 1/(f*m) and store in the DM + * For each delta axis, also calculate and store in the DM additional parameters needed for delta movement + * If we use common extruder segments, for each extruder also calculate and store in the DM the following: 2*(p/f)/a, p/f + EAD, 2*(p/f + EAD)/d + * If we use common extruder segments, either make sure we have fast access to k, or for each extruder store k + * + * When starting a new segment we need to: + * Calculate the segment start time ts (by accumulating the durations of previous segments) + * Compute A' B' C' for this segment and motor + * + * If the MCU supports hardware floating point then it is more efficient to use FP arithmetic because of the speed of FP divide and sqrt operations. + * If it doesn't then for accel/decel segments we probably need to compute sqrt(A + C*n/(f*m)) using 64-bit maths before we take the sqrt: + * A is in step_clocks^2 so it can be stored directly in 64 bits + * B is in step_clocks so it can be stored directly in 32 bits + * C for accel/decel is in step clocks^2/mm so it can probably be stored directly in 64 bits (check - does this work for very low f ?) + * C for linear motion is in step clocks/mm so can be stored directly in 32 bits + * 1/(f*m) is in mm/step. We will have to store it multiplied by e.g. 2^24 as 32 bits, and after multiplying it by C to get a 64-bit result, shift it right to divide by 2^24. + */ + +#ifndef SRC_MOVEMENT_MOVESEGMENT_H_ +#define SRC_MOVEMENT_MOVESEGMENT_H_ + +#include <RepRapFirmware.h> +#include <Platform/Tasks.h> + +class MoveSegment +{ +public: + void* operator new(size_t count) { return Tasks::AllocPermanent(count); } + void* operator new(size_t count, std::align_val_t align) { return Tasks::AllocPermanent(count, align); } + void operator delete(void* ptr) noexcept {} + void operator delete(void* ptr, std::align_val_t align) noexcept {} + + MoveSegment(MoveSegment *p_next) noexcept; + + float GetSegmentLength() const noexcept { return segmentLength; } + float GetSegmentTime() const noexcept { return segTime; } + float CalcNonlinearA(float startDistance) const noexcept; + float CalcNonlinearA(float startDistance, float pressureAdvanceK) const noexcept; + float CalcNonlinearB(float startTime) const noexcept; + float CalcNonlinearB(float startTime, float pressureAdvanceK) const noexcept; + float CalcLinearB(float startDistance, float startTime) const noexcept; + float CalcC(float mmPerStep) const noexcept; + float GetC() const noexcept { return c; } + + MoveSegment *GetNext() const noexcept; + bool IsLinear() const noexcept; + bool IsAccelerating() const noexcept pre(!IsLinear()); + bool IsLast() const noexcept; + + void SetNext(MoveSegment *p_next) noexcept; + void SetLinear(float pSegmentLength, float p_segTime, float p_c) noexcept; + void SetNonLinear(float pSegmentLength, float p_segTime, float p_b, float p_c) noexcept; + void SetReverse() noexcept; + + void AddToTail(MoveSegment *tail) noexcept; + + void DebugPrint(char ch) const noexcept; + + // Allocate a MoveSegment, clearing the Linear and Last flags + static MoveSegment *Allocate(MoveSegment *next) noexcept; + + // Release a MoveSegment + static void Release(MoveSegment *item) noexcept; + + static void InitialAllocate(unsigned int num) noexcept; + static unsigned int NumCreated() noexcept { return numCreated; } + +private: + static constexpr uint32_t LinearFlag = 0x01; + static constexpr uint32_t AllFlags = 0x03; + + static MoveSegment *freeList; + static unsigned int numCreated; + + static_assert(sizeof(MoveSegment*) == sizeof(uint32_t)); + + // The 'next' field is a MoveSegment pointer with two flag bits in the bottom two bits + uint32_t nextAndFlags; // pointer to the next segment, plus flag bits + float segmentLength; // the length of this segment before applying the movement fraction + float segTime; // the time in step clocks at which this move ends + float b, c; // the move parameters (b is not needed for linear moves) +}; + +// Create a new one, leaving the flags clear +inline MoveSegment::MoveSegment(MoveSegment *p_next) noexcept + : nextAndFlags(reinterpret_cast<uint32_t>(p_next)) // this also clears the flags +{ + // remaining fields are not initialised +} + +inline MoveSegment *MoveSegment::GetNext() const noexcept +{ + return reinterpret_cast<MoveSegment*>(nextAndFlags & (~AllFlags)); +} + +inline void MoveSegment::SetNext(MoveSegment *p_next) noexcept +{ + nextAndFlags = (nextAndFlags & AllFlags) | reinterpret_cast<uint32_t>(p_next); +} + +inline bool MoveSegment::IsLinear() const noexcept +{ + return nextAndFlags & LinearFlag; +} + + +inline bool MoveSegment::IsLast() const noexcept +{ + return GetNext() == nullptr; +} + +inline void MoveSegment::SetLinear(float pSegmentLength, float p_segTime, float p_c) noexcept +{ + segmentLength = pSegmentLength; + segTime = p_segTime; + b = 0.0; + c = p_c; + nextAndFlags |= LinearFlag; +} + +// Set up an accelerating or decelerating move. We assume that the 'linear' flag is already clear. +inline void MoveSegment::SetNonLinear(float pSegmentLength, float p_segTime, float p_b, float p_c) noexcept +{ + segmentLength = pSegmentLength; + segTime = p_segTime; + b = p_b; + c = p_c; +} + +inline float MoveSegment::CalcNonlinearA(float startDistance) const noexcept +{ + return fsquare(b) - startDistance * c; +} + +inline float MoveSegment::CalcNonlinearA(float startDistance, float pressureAdvanceK) const noexcept +{ + return fsquare(b - pressureAdvanceK) - startDistance * c; +} + +inline float MoveSegment::CalcNonlinearB(float startTime) const noexcept +{ + return b + startTime; +} + +inline float MoveSegment::CalcNonlinearB(float startTime, float pressureAdvanceK) const noexcept +{ + return (b - pressureAdvanceK) + startTime; +} + +inline float MoveSegment::CalcLinearB(float startDistance, float startTime) const noexcept +{ + return startTime - (startDistance * c); +} + +inline float MoveSegment::CalcC(float mmPerStep) const noexcept +{ + return c * mmPerStep; +} + +// Given that this is an accelerating or decelerating move, return true if it is accelerating +inline bool MoveSegment::IsAccelerating() const noexcept +{ + return c > 0; +} + +// Release a single MoveSegment. Not thread-safe. +inline void MoveSegment::Release(MoveSegment *item) noexcept +{ + item->nextAndFlags = reinterpret_cast<uint32_t>(freeList); + freeList = item; +} + +#endif /* SRC_MOVEMENT_MOVESEGMENT_H_ */ diff --git a/src/Movement/StepTimer.h b/src/Movement/StepTimer.h index 8a2fcda6..f0080cd7 100644 --- a/src/Movement/StepTimer.h +++ b/src/Movement/StepTimer.h @@ -63,18 +63,7 @@ public: // ISR called from StepTimer static void Interrupt() noexcept; -#if SAME70 || SAME5x - // All Duet 3 boards use a common step clock rate of 750kHz so that we can sync the clocks over CAN - static constexpr uint32_t StepClockRate = 48000000/64; // 750kHz -#elif defined(__LPC17xx__) - static constexpr uint32_t StepClockRate = 1000000; // 1MHz -#else - static constexpr uint32_t StepClockRate = SystemCoreClockFreq/128; // Duet 2 and Maestro: use just under 1MHz -#endif - - static constexpr uint64_t StepClockRateSquared = (uint64_t)StepClockRate * StepClockRate; - static constexpr float StepClocksToMillis = 1000.0/(float)StepClockRate; - static constexpr uint32_t MinInterruptInterval = 6; // about 6us + static constexpr uint32_t MinInterruptInterval = 6; // Minimum interval between step timer interrupts, in step clocks; about 6us #if SUPPORT_REMOTE_COMMANDS static uint32_t GetLocalTimeOffset() noexcept { return localTimeOffset; } diff --git a/src/Movement/StepperDrivers/TMC22xx.cpp b/src/Movement/StepperDrivers/TMC22xx.cpp index 240e9a5a..158f33db 100644 --- a/src/Movement/StepperDrivers/TMC22xx.cpp +++ b/src/Movement/StepperDrivers/TMC22xx.cpp @@ -884,7 +884,7 @@ inline void TmcDriverState::SetupDMARead(uint8_t regNum) noexcept // Update the maximum step pulse interval at which we consider open load detection to be reliable void TmcDriverState::UpdateMaxOpenLoadStepInterval() noexcept { - const uint32_t defaultMaxInterval = StepTimer::StepClockRate/MinimumOpenLoadFullStepsPerSec; + const uint32_t defaultMaxInterval = StepClockRate/MinimumOpenLoadFullStepsPerSec; if ((writeRegisters[WriteGConf] & GCONF_SPREAD_CYCLE) != 0) { maxOpenLoadStepInterval = defaultMaxInterval; @@ -896,7 +896,7 @@ void TmcDriverState::UpdateMaxOpenLoadStepInterval() noexcept // tpwmthrs is the 20-bit interval between 1/256 microsteps threshold, in clock cycles @ 12MHz. // We need to convert it to the interval between full steps, measured in our step clocks, less about 20% to allow some margin. // So multiply by the step clock rate divided by 12MHz, also multiply by 256 less 20%. - constexpr uint32_t conversionFactor = ((256 - 51) * (StepTimer::StepClockRate/1000000))/12; + constexpr uint32_t conversionFactor = ((256 - 51) * (StepClockRate/1000000))/12; const uint32_t fullStepClocks = tpwmthrs * conversionFactor; maxOpenLoadStepInterval = min<uint32_t>(fullStepClocks, defaultMaxInterval); } diff --git a/src/Movement/StepperDrivers/TMC2660.cpp b/src/Movement/StepperDrivers/TMC2660.cpp index c7c7de99..a4cdf4c8 100644 --- a/src/Movement/StepperDrivers/TMC2660.cpp +++ b/src/Movement/StepperDrivers/TMC2660.cpp @@ -669,7 +669,7 @@ void TmcDriverState::SetStallDetectFilter(bool sgFilter) noexcept void TmcDriverState::SetStallMinimumStepsPerSecond(unsigned int stepsPerSecond) noexcept { - maxStallStepInterval = StepTimer::StepClockRate/max<unsigned int>(stepsPerSecond, 1); + maxStallStepInterval = StepClockRate/max<unsigned int>(stepsPerSecond, 1); } void TmcDriverState::AppendStallConfig(const StringRef& reply) const noexcept @@ -680,7 +680,7 @@ void TmcDriverState::AppendStallConfig(const StringRef& reply) const noexcept { threshold -= 128; } - const uint32_t fullstepsPerSecond = StepTimer::StepClockRate/maxStallStepInterval; + const uint32_t fullstepsPerSecond = StepClockRate/maxStallStepInterval; const float speed = ((fullstepsPerSecond << microstepShiftFactor)/reprap.GetPlatform().DriveStepsPerUnit(axisNumber)); reply.catf("stall threshold %d, filter %s, steps/sec %" PRIu32 " (%.1f mm/sec), coolstep %" PRIx32, threshold, ((filtered) ? "on" : "off"), fullstepsPerSecond, (double)speed, registers[SmartEnable] & 0xFFFF); @@ -778,7 +778,7 @@ inline void TmcDriverState::TransferDone() noexcept if ( (status & TMC_RR_STST) != 0 || interval == 0 - || interval > StepTimer::StepClockRate/MinimumOpenLoadFullStepsPerSec + || interval > StepClockRate/MinimumOpenLoadFullStepsPerSec || (registers[StallGuardConfig] & TMC_SGCSCONF_CS_MASK) < MinimumOpenLoadCsBits ) { diff --git a/src/Movement/StepperDrivers/TMC51xx.cpp b/src/Movement/StepperDrivers/TMC51xx.cpp index 5f1c72ed..e796a0f4 100644 --- a/src/Movement/StepperDrivers/TMC51xx.cpp +++ b/src/Movement/StepperDrivers/TMC51xx.cpp @@ -829,7 +829,7 @@ void TmcDriverState::SetStallDetectFilter(bool sgFilter) noexcept void TmcDriverState::SetStallMinimumStepsPerSecond(unsigned int stepsPerSecond) noexcept { //TODO use hardware facility instead - maxStallStepInterval = StepTimer::StepClockRate/max<unsigned int>(stepsPerSecond, 1); + maxStallStepInterval = StepClockRate/max<unsigned int>(stepsPerSecond, 1); } void TmcDriverState::AppendStallConfig(const StringRef& reply) const noexcept @@ -840,7 +840,7 @@ void TmcDriverState::AppendStallConfig(const StringRef& reply) const noexcept { threshold -= 128; } - const uint32_t fullstepsPerSecond = StepTimer::StepClockRate/maxStallStepInterval; + const uint32_t fullstepsPerSecond = StepClockRate/maxStallStepInterval; const float speed1 = ((fullstepsPerSecond << microstepShiftFactor)/reprap.GetPlatform().DriveStepsPerUnit(axisNumber)); const uint32_t tcoolthrs = writeRegisters[WriteTcoolthrs] & ((1ul << 20) - 1u); bool bdummy; @@ -929,7 +929,7 @@ void TmcDriverState::TransferSucceeded(const uint8_t *rcvDataBlock) noexcept { if ( (regVal & TMC_RR_STST) != 0 || interval == 0 - || interval > StepTimer::StepClockRate/MinimumOpenLoadFullStepsPerSec + || interval > StepClockRate/MinimumOpenLoadFullStepsPerSec || motorCurrent < MinimumOpenLoadMotorCurrent ) { diff --git a/src/Networking/Network.cpp b/src/Networking/Network.cpp index 3da1528c..887b3f78 100644 --- a/src/Networking/Network.cpp +++ b/src/Networking/Network.cpp @@ -551,7 +551,7 @@ void Network::Diagnostics(MessageType mtype) noexcept #if HAS_NETWORKING platform.Message(mtype, "=== Network ===\n"); - platform.MessageF(mtype, "Slowest loop: %.2fms; fastest: %.2fms\n", (double)(slowLoop * StepTimer::StepClocksToMillis), (double)(fastLoop * StepTimer::StepClocksToMillis)); + platform.MessageF(mtype, "Slowest loop: %.2fms; fastest: %.2fms\n", (double)(slowLoop * StepClocksToMillis), (double)(fastLoop * StepClocksToMillis)); fastLoop = UINT32_MAX; slowLoop = 0; diff --git a/src/ObjectModel/ObjectModel.cpp b/src/ObjectModel/ObjectModel.cpp index 6a1dfd51..6328b1a6 100644 --- a/src/ObjectModel/ObjectModel.cpp +++ b/src/ObjectModel/ObjectModel.cpp @@ -84,7 +84,7 @@ void ExpressionValue::AppendAsString(const StringRef& str) const noexcept case TypeCode::DriverId: #if SUPPORT_CAN_EXPANSION - str.catf("%u.%u", (unsigned int)(uVal >> 8), (unsigned int)(uVal & 0xFF)); + str.catf("%u.%u", (unsigned int)param, (unsigned int)uVal); #else str.catf("%u", (unsigned int)uVal); #endif @@ -709,7 +709,7 @@ void ObjectModel::ReportItemAsJsonFull(OutputBuffer *buf, ObjectExplorationConte case TypeCode::DriverId: #if SUPPORT_CAN_EXPANSION - buf->catf("\"%u.%u\"", (unsigned int)(val.uVal >> 8), (unsigned int)(val.uVal & 0xFF)); + buf->catf("\"%u.%u\"", (unsigned int)val.param, (unsigned int)val.uVal); #else buf->catf("\"%u\"", (unsigned int)val.uVal); #endif diff --git a/src/ObjectModel/ObjectModel.h b/src/ObjectModel/ObjectModel.h index 6a2858e0..af576473 100644 --- a/src/ObjectModel/ObjectModel.h +++ b/src/ObjectModel/ObjectModel.h @@ -123,7 +123,18 @@ struct ExpressionValue explicit constexpr ExpressionValue(IPAddress ip) noexcept : type((uint32_t)TypeCode::IPAddress), param(0), uVal(ip.GetV4LittleEndian()) { } explicit constexpr ExpressionValue(nullptr_t dummy) noexcept : type((uint32_t)TypeCode::None), param(0), uVal(0) { } explicit ExpressionValue(DateTime t) noexcept : type((t.tim == 0) ? (uint32_t)TypeCode::None : (uint32_t)TypeCode::DateTime) { Set56BitValue(t.tim); } - explicit ExpressionValue(DriverId id) noexcept : type((uint32_t)TypeCode::DriverId), param(0), uVal(id.AsU32()) { } + + explicit ExpressionValue(DriverId id) noexcept + : type((uint32_t)TypeCode::DriverId), +#if SUPPORT_CAN_EXPANSION + param(id.boardAddress), +#else + param(0), +#endif + uVal(id.localDriver) + { + } + explicit ExpressionValue(Bitmap<uint16_t> bm) noexcept : type((uint32_t)TypeCode::Bitmap16), param(0), uVal(bm.GetRaw()) { } explicit ExpressionValue(Bitmap<uint32_t> bm) noexcept : type((uint32_t)TypeCode::Bitmap32), param(0), uVal(bm.GetRaw()) { } explicit ExpressionValue(Bitmap<uint64_t> bm) noexcept : type((uint32_t)TypeCode::Bitmap64) { Set56BitValue(bm.GetRaw()); } @@ -150,6 +161,18 @@ struct ExpressionValue void Set(float f) noexcept { Release(); type = (uint32_t)TypeCode::Float; fVal = f; param = MaxFloatDigitsDisplayedAfterPoint; } void Set(float f, uint32_t digits) noexcept { Release(); type = (uint32_t)TypeCode::Float; fVal = f; param = digits; } void Set(const char *s) noexcept { Release(); type = (uint32_t)TypeCode::CString; sVal = s; } + void Set(DriverId did) noexcept + { + Release(); + type = (uint32_t)TypeCode::DriverId; +#if SUPPORT_CAN_EXPANSION + param = did.boardAddress; +#else + param = 0; +#endif + uVal = did.localDriver; + } + void Set(StringHandle sh) noexcept { Release(); type = (uint32_t)TypeCode::HeapString; shVal = sh; } void Set(nullptr_t dummy) noexcept { Release(); type = (uint32_t)TypeCode::None; } @@ -159,6 +182,14 @@ struct ExpressionValue // Extract a 56-bit value that we have stored. Used to retrieve date/times and large bitmaps. uint64_t Get56BitValue() const noexcept { return ((uint64_t)param << 32) | uVal; } + // Extract a driver ID value + DriverId GetDriverIdValue() const noexcept +#if SUPPORT_CAN_EXPANSION + { return DriverId(param, uVal); } +#else + { return DriverId(uVal); } +#endif + // Get the format string to use assuming this is a floating point number const char *GetFloatFormatString() const noexcept { return ::GetFloatFormatString(param); } @@ -216,5 +216,6 @@ // Define SUPPORT_REMOTE_COMMANDS according to whether this hardware accepts commands over CAN // For now we exclude Duet 3 MB6HC because CoreNG doesn't support analog callbacks #define SUPPORT_REMOTE_COMMANDS (SUPPORT_CAN_EXPANSION && !defined(DUET3_ATE)) +#define USE_REMOTE_INPUT_SHAPING (0) //TODO temporary!!! #endif // PINS_H__ diff --git a/src/Platform/Platform.cpp b/src/Platform/Platform.cpp index 15905d28..235a1a81 100644 --- a/src/Platform/Platform.cpp +++ b/src/Platform/Platform.cpp @@ -265,55 +265,55 @@ constexpr ObjectModelTableEntry Platform::objectModelTable[] = #endif // 3. move.axes[] members - { "acceleration", OBJECT_MODEL_FUNC(self->Acceleration(context.GetLastIndex()), 1), ObjectModelEntryFlags::none }, - { "babystep", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetTotalBabyStepOffset(context.GetLastIndex()), 3), ObjectModelEntryFlags::none }, - { "current", OBJECT_MODEL_FUNC((int32_t)lrintf(self->GetMotorCurrent(context.GetLastIndex(), 906))), ObjectModelEntryFlags::none }, - { "drivers", OBJECT_MODEL_FUNC_NOSELF(&axisDriversArrayDescriptor), ObjectModelEntryFlags::none }, - { "homed", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().IsAxisHomed(context.GetLastIndex())), ObjectModelEntryFlags::none }, - { "jerk", OBJECT_MODEL_FUNC(MinutesToSeconds * self->GetInstantDv(context.GetLastIndex()), 1), ObjectModelEntryFlags::none }, - { "letter", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetAxisLetters()[context.GetLastIndex()]), ObjectModelEntryFlags::none }, + { "acceleration", OBJECT_MODEL_FUNC(InverseConvertAcceleration(self->Acceleration(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, + { "babystep", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetTotalBabyStepOffset(context.GetLastIndex()), 3), ObjectModelEntryFlags::none }, + { "current", OBJECT_MODEL_FUNC((int32_t)lrintf(self->GetMotorCurrent(context.GetLastIndex(), 906))), ObjectModelEntryFlags::none }, + { "drivers", OBJECT_MODEL_FUNC_NOSELF(&axisDriversArrayDescriptor), ObjectModelEntryFlags::none }, + { "homed", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().IsAxisHomed(context.GetLastIndex())), ObjectModelEntryFlags::none }, + { "jerk", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerMin(self->GetInstantDv(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, + { "letter", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetAxisLetters()[context.GetLastIndex()]), ObjectModelEntryFlags::none }, { "machinePosition", OBJECT_MODEL_FUNC_NOSELF(reprap.GetMove().LiveCoordinate(context.GetLastIndex(), reprap.GetCurrentTool()), 3), ObjectModelEntryFlags::live }, - { "max", OBJECT_MODEL_FUNC(self->AxisMaximum(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, - { "maxProbed", OBJECT_MODEL_FUNC(self->axisMaximaProbed.IsBitSet(context.GetLastIndex())), ObjectModelEntryFlags::none }, - { "microstepping", OBJECT_MODEL_FUNC(self, 7), ObjectModelEntryFlags::none }, - { "min", OBJECT_MODEL_FUNC(self->AxisMinimum(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, - { "minProbed", OBJECT_MODEL_FUNC(self->axisMinimaProbed.IsBitSet(context.GetLastIndex())), ObjectModelEntryFlags::none }, - { "speed", OBJECT_MODEL_FUNC(MinutesToSeconds * self->MaxFeedrate(context.GetLastIndex()), 1), ObjectModelEntryFlags::none }, - { "stepsPerMm", OBJECT_MODEL_FUNC(self->driveStepsPerUnit[context.GetLastIndex()], 2), ObjectModelEntryFlags::none }, - { "userPosition", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetUserCoordinate(context.GetLastIndex()), 3), ObjectModelEntryFlags::live }, - { "visible", OBJECT_MODEL_FUNC_NOSELF(context.GetLastIndex() < (int32_t)reprap.GetGCodes().GetVisibleAxes()), ObjectModelEntryFlags::none }, - { "workplaceOffsets", OBJECT_MODEL_FUNC_NOSELF(&workplaceOffsetsArrayDescriptor), ObjectModelEntryFlags::none }, + { "max", OBJECT_MODEL_FUNC(self->AxisMaximum(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, + { "maxProbed", OBJECT_MODEL_FUNC(self->axisMaximaProbed.IsBitSet(context.GetLastIndex())), ObjectModelEntryFlags::none }, + { "microstepping", OBJECT_MODEL_FUNC(self, 7), ObjectModelEntryFlags::none }, + { "min", OBJECT_MODEL_FUNC(self->AxisMinimum(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, + { "minProbed", OBJECT_MODEL_FUNC(self->axisMinimaProbed.IsBitSet(context.GetLastIndex())), ObjectModelEntryFlags::none }, + { "speed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerMin(self->MaxFeedrate(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, + { "stepsPerMm", OBJECT_MODEL_FUNC(self->driveStepsPerUnit[context.GetLastIndex()], 2), ObjectModelEntryFlags::none }, + { "userPosition", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetUserCoordinate(context.GetLastIndex()), 3), ObjectModelEntryFlags::live }, + { "visible", OBJECT_MODEL_FUNC_NOSELF(context.GetLastIndex() < (int32_t)reprap.GetGCodes().GetVisibleAxes()), ObjectModelEntryFlags::none }, + { "workplaceOffsets", OBJECT_MODEL_FUNC_NOSELF(&workplaceOffsetsArrayDescriptor), ObjectModelEntryFlags::none }, // 4. move.extruders[] members - { "acceleration", OBJECT_MODEL_FUNC(self->Acceleration(ExtruderToLogicalDrive(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, - { "current", OBJECT_MODEL_FUNC((int32_t)lrintf(self->GetMotorCurrent(ExtruderToLogicalDrive(context.GetLastIndex()), 906))), ObjectModelEntryFlags::none }, - { "driver", OBJECT_MODEL_FUNC(self->extruderDrivers[context.GetLastIndex()]), ObjectModelEntryFlags::none }, - { "factor", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetExtrusionFactor(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, - { "filament", OBJECT_MODEL_FUNC_NOSELF(GetFilamentName(context.GetLastIndex())), ObjectModelEntryFlags::none }, - { "jerk", OBJECT_MODEL_FUNC(MinutesToSeconds * self->GetInstantDv(ExtruderToLogicalDrive(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, - { "microstepping", OBJECT_MODEL_FUNC(self, 8), ObjectModelEntryFlags::none }, - { "nonlinear", OBJECT_MODEL_FUNC(self, 5), ObjectModelEntryFlags::none }, + { "acceleration", OBJECT_MODEL_FUNC(self->Acceleration(ExtruderToLogicalDrive(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, + { "current", OBJECT_MODEL_FUNC((int32_t)lrintf(self->GetMotorCurrent(ExtruderToLogicalDrive(context.GetLastIndex()), 906))), ObjectModelEntryFlags::none }, + { "driver", OBJECT_MODEL_FUNC(self->extruderDrivers[context.GetLastIndex()]), ObjectModelEntryFlags::none }, + { "factor", OBJECT_MODEL_FUNC_NOSELF(reprap.GetGCodes().GetExtrusionFactor(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, + { "filament", OBJECT_MODEL_FUNC_NOSELF(GetFilamentName(context.GetLastIndex())), ObjectModelEntryFlags::none }, + { "jerk", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerMin(self->GetInstantDv(ExtruderToLogicalDrive(context.GetLastIndex()))), 1), ObjectModelEntryFlags::none }, + { "microstepping", OBJECT_MODEL_FUNC(self, 8), ObjectModelEntryFlags::none }, + { "nonlinear", OBJECT_MODEL_FUNC(self, 5), ObjectModelEntryFlags::none }, { "position", OBJECT_MODEL_FUNC_NOSELF(ExpressionValue(reprap.GetMove().LiveCoordinate(ExtruderToLogicalDrive(context.GetLastIndex()), reprap.GetCurrentTool()), 1)), ObjectModelEntryFlags::live }, - { "pressureAdvance", OBJECT_MODEL_FUNC(self->GetPressureAdvance(context.GetLastIndex()), 2), ObjectModelEntryFlags::none }, - { "rawPosition", OBJECT_MODEL_FUNC_NOSELF(ExpressionValue(reprap.GetGCodes().GetRawExtruderTotalByDrive(context.GetLastIndex()), 1)), ObjectModelEntryFlags::live }, - { "speed", OBJECT_MODEL_FUNC(MinutesToSeconds * self->MaxFeedrate(ExtruderToLogicalDrive(context.GetLastIndex())), 1), ObjectModelEntryFlags::none }, - { "stepsPerMm", OBJECT_MODEL_FUNC(self->driveStepsPerUnit[ExtruderToLogicalDrive(context.GetLastIndex())], 2), ObjectModelEntryFlags::none }, + { "pressureAdvance", OBJECT_MODEL_FUNC_NOSELF(reprap.GetMove().GetPressureAdvanceClocks(context.GetLastIndex()) * StepClockRate, 2), ObjectModelEntryFlags::none }, + { "rawPosition", OBJECT_MODEL_FUNC_NOSELF(ExpressionValue(reprap.GetGCodes().GetRawExtruderTotalByDrive(context.GetLastIndex()), 1)), ObjectModelEntryFlags::live }, + { "speed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerMin(self->MaxFeedrate(ExtruderToLogicalDrive(context.GetLastIndex()))), 1), ObjectModelEntryFlags::none }, + { "stepsPerMm", OBJECT_MODEL_FUNC(self->driveStepsPerUnit[ExtruderToLogicalDrive(context.GetLastIndex())], 2), ObjectModelEntryFlags::none }, // 5. move.extruders[].nonlinear members - { "a", OBJECT_MODEL_FUNC(self->nonlinearExtrusionA[context.GetLastIndex()], 3), ObjectModelEntryFlags::none }, - { "b", OBJECT_MODEL_FUNC(self->nonlinearExtrusionB[context.GetLastIndex()], 3), ObjectModelEntryFlags::none }, - { "upperLimit", OBJECT_MODEL_FUNC(self->nonlinearExtrusionLimit[context.GetLastIndex()], 2), ObjectModelEntryFlags::none }, + { "a", OBJECT_MODEL_FUNC(self->nonlinearExtrusionA[context.GetLastIndex()], 3), ObjectModelEntryFlags::none }, + { "b", OBJECT_MODEL_FUNC(self->nonlinearExtrusionB[context.GetLastIndex()], 3), ObjectModelEntryFlags::none }, + { "upperLimit", OBJECT_MODEL_FUNC(self->nonlinearExtrusionLimit[context.GetLastIndex()], 2), ObjectModelEntryFlags::none }, #if HAS_12V_MONITOR // 6. v12 members - { "current", OBJECT_MODEL_FUNC(self->GetV12Voltages().current, 1), ObjectModelEntryFlags::live }, - { "max", OBJECT_MODEL_FUNC(self->GetV12Voltages().max, 1), ObjectModelEntryFlags::none }, - { "min", OBJECT_MODEL_FUNC(self->GetV12Voltages().min, 1), ObjectModelEntryFlags::none }, + { "current", OBJECT_MODEL_FUNC(self->GetV12Voltages().current, 1), ObjectModelEntryFlags::live }, + { "max", OBJECT_MODEL_FUNC(self->GetV12Voltages().max, 1), ObjectModelEntryFlags::none }, + { "min", OBJECT_MODEL_FUNC(self->GetV12Voltages().min, 1), ObjectModelEntryFlags::none }, #endif // 7. move.axes[].microstepping members - { "interpolated", OBJECT_MODEL_FUNC((self->microstepping[context.GetLastIndex()] & 0x8000) != 0), ObjectModelEntryFlags::none }, - { "value", OBJECT_MODEL_FUNC((int32_t)(self->microstepping[context.GetLastIndex()] & 0x7FFF)), ObjectModelEntryFlags::none }, + { "interpolated", OBJECT_MODEL_FUNC((self->microstepping[context.GetLastIndex()] & 0x8000) != 0), ObjectModelEntryFlags::none }, + { "value", OBJECT_MODEL_FUNC((int32_t)(self->microstepping[context.GetLastIndex()] & 0x7FFF)), ObjectModelEntryFlags::none }, // 8. move.extruders[].microstepping members { "interpolated", OBJECT_MODEL_FUNC((self->microstepping[ExtruderToLogicalDrive(context.GetLastIndex())] & 0x8000) != 0), ObjectModelEntryFlags::none }, @@ -590,7 +590,7 @@ void Platform::Init() noexcept instantDvs[drive] = DefaultEInstantDv; } - minimumMovementSpeed = DefaultMinFeedrate; + minimumMovementSpeed = ConvertSpeedFromMmPerSec(DefaultMinFeedrate); axisMaximaProbed.Clear(); axisMinimaProbed.Clear(); idleCurrentFactor = DefaultIdleCurrentFactor; @@ -615,9 +615,6 @@ void Platform::Init() noexcept directions[driver] = true; // drive moves forwards by default enableValues[driver] = 0; // assume active low enable signal -#if SUPPORT_REMOTE_COMMANDS - remotePressureAdvance[driver] = 0.0; -#endif // Set up the control pins pinMode(STEP_PINS[driver], OUTPUT_LOW); pinMode(DIRECTION_PINS[driver], OUTPUT_LOW); @@ -661,7 +658,6 @@ void Platform::Init() noexcept { extruderDrivers[extr].SetLocal(extr + MinAxes); // set up default extruder drive mapping driveDriverBits[ExtruderToLogicalDrive(extr)] = StepPins::CalcDriverBitmap(extr + MinAxes); - pressureAdvance[extr] = 0.0; #if SUPPORT_NONLINEAR_EXTRUSION nonlinearExtrusionA[extr] = nonlinearExtrusionB[extr] = 0.0; nonlinearExtrusionLimit[extr] = DefaultNonlinearExtrusionLimit; @@ -2298,11 +2294,11 @@ GCodeResult Platform::DiagnosticTest(GCodeBuffer& gb, const StringRef& reply, Ou case (unsigned int)DiagnosticTestType::PrintObjectSizes: reply.printf( - "DDA %u, DM %u, Tool %u, GCodeBuffer %u, heater %u" + "DDA %u, DM %u, MS %u, Tool %u, GCodeBuffer %u, heater %u" #if HAS_NETWORKING ", HTTP resp %u, FTP resp %u, Telnet resp %u" #endif - , sizeof(DDA), sizeof(DriveMovement), sizeof(Tool), sizeof(GCodeBuffer), sizeof(Heater) + , sizeof(DDA), sizeof(DriveMovement), sizeof(MoveSegment), sizeof(Tool), sizeof(GCodeBuffer), sizeof(Heater) #if HAS_NETWORKING , sizeof(HttpResponder), sizeof(FtpResponder), sizeof(TelnetResponder) #endif @@ -3195,7 +3191,7 @@ void Platform::SetDriverStepTiming(size_t driver, const float microseconds[4]) n if (microseconds[i] > MinStepPulseTiming) { slowDriversBitmap |= StepPins::CalcDriverBitmap(driver); // this drive does need extended timing - const uint32_t clocks = (uint32_t)(((float)StepTimer::StepClockRate * microseconds[i] * 0.000001) + 0.99); // convert microseconds to step clocks, rounding up + const uint32_t clocks = (uint32_t)(((float)StepClockRate * microseconds[i] * 0.000001) + 0.99); // convert microseconds to step clocks, rounding up if (clocks > slowDriverStepTimingClocks[i]) { slowDriverStepTimingClocks[i] = clocks; @@ -3211,7 +3207,7 @@ bool Platform::GetDriverStepTiming(size_t driver, float microseconds[4]) const n for (size_t i = 0; i < 4; ++i) { microseconds[i] = (isSlowDriver) - ? (float)slowDriverStepTimingClocks[i] * 1000000.0/(float)StepTimer::StepClockRate + ? (float)slowDriverStepTimingClocks[i] * 1000000.0/(float)StepClockRate : 0.0; } return isSlowDriver; @@ -3735,74 +3731,6 @@ void Platform::AtxPowerOff(bool defer) noexcept } } -GCodeResult Platform::SetPressureAdvance(float advance, GCodeBuffer& gb, const StringRef& reply) -{ - GCodeResult rslt = GCodeResult::ok; - -#if SUPPORT_CAN_EXPANSION - CanDriversData<float> canDriversToUpdate; -#endif - - if (gb.Seen('D')) - { - uint32_t eDrive[MaxExtruders]; - size_t eCount = MaxExtruders; - gb.GetUnsignedArray(eDrive, eCount, false); - for (size_t i = 0; i < eCount; i++) - { - const uint32_t extruder = eDrive[i]; - if (extruder >= reprap.GetGCodes().GetNumExtruders()) - { - reply.printf("Invalid extruder number '%" PRIu32 "'", extruder); - rslt = GCodeResult::error; - break; - } - pressureAdvance[extruder] = advance; -#if SUPPORT_CAN_EXPANSION - if (extruderDrivers[extruder].IsRemote()) - { - canDriversToUpdate.AddEntry(extruderDrivers[extruder], advance); - } -#endif - } - } - else - { - const Tool * const ct = reprap.GetCurrentTool(); - if (ct == nullptr) - { - reply.copy("No tool selected"); - rslt = GCodeResult::error; - } - else - { -#if SUPPORT_CAN_EXPANSION - ct->IterateExtruders([this, advance, &canDriversToUpdate](unsigned int extruder) - { - pressureAdvance[extruder] = advance; - if (extruderDrivers[extruder].IsRemote()) - { - canDriversToUpdate.AddEntry(extruderDrivers[extruder], advance); - } - } - ); -#else - ct->IterateExtruders([this, advance](unsigned int extruder) - { - pressureAdvance[extruder] = advance; - } - ); -#endif - } - } - -#if SUPPORT_CAN_EXPANSION - return max(rslt, CanInterface::SetRemotePressureAdvance(canDriversToUpdate, reply)); -#else - return rslt; -#endif -} - #if SUPPORT_NONLINEAR_EXTRUSION bool Platform::GetExtrusionCoefficients(size_t extruder, float& a, float& b, float& limit) const noexcept @@ -4877,37 +4805,6 @@ GCodeResult Platform::EutHandleSetDriverStates(const CanMessageMultipleDrivesReq return GCodeResult::ok; } -float Platform::EutGetRemotePressureAdvance(size_t driver) const noexcept -{ - return (driver < ARRAY_SIZE(remotePressureAdvance)) ? remotePressureAdvance[driver] : 0.0; -} - -GCodeResult Platform::EutSetRemotePressureAdvance(const CanMessageMultipleDrivesRequest<float>& msg, size_t dataLength, const StringRef& reply) noexcept -{ - const auto drivers = Bitmap<uint16_t>::MakeFromRaw(msg.driversToUpdate); - if (dataLength < msg.GetActualDataLength(drivers.CountSetBits())) - { - reply.copy("bad data length"); - return GCodeResult::error; - } - - GCodeResult rslt = GCodeResult::ok; - drivers.Iterate([this, &msg, &reply, &rslt](unsigned int driver, unsigned int count) -> void - { - if (driver >= NumDirectDrivers) - { - reply.lcatf("No such driver %u.%u", CanInterface::GetCanAddress(), driver); - rslt = GCodeResult::error; - } - else - { - remotePressureAdvance[driver] = msg.values[count]; - } - } - ); - return rslt; -} - GCodeResult Platform::EutProcessM569(const CanMessageGeneric& msg, const StringRef& reply) noexcept { CanMessageGenericParser parser(msg, M569Params); diff --git a/src/Platform/Platform.h b/src/Platform/Platform.h index 41e48c54..041869ff 100644 --- a/src/Platform/Platform.h +++ b/src/Platform/Platform.h @@ -458,7 +458,7 @@ public: const float* MaxFeedrates() const noexcept { return maxFeedrates; } void SetMaxFeedrate(size_t axisOrExtruder, float value) noexcept; float MinMovementSpeed() const noexcept { return minimumMovementSpeed; } - void SetMinMovementSpeed(float value) noexcept { minimumMovementSpeed = max<float>(value, 0.01); } + void SetMinMovementSpeed(float value) noexcept; float GetInstantDv(size_t axis) const noexcept; void SetInstantDv(size_t axis, float value) noexcept; float AxisMaximum(size_t axis) const noexcept; @@ -467,9 +467,6 @@ public: void SetAxisMinimum(size_t axis, float value, bool byProbing) noexcept; float AxisTotalLength(size_t axis) const noexcept; - float GetPressureAdvance(size_t extruder) const noexcept; - GCodeResult SetPressureAdvance(float advance, GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException); - inline AxesBitmap GetLinearAxes() const noexcept { return linearAxes; } inline AxesBitmap GetRotationalAxes() const noexcept { return rotationalAxes; } inline bool IsAxisRotational(size_t axis) const noexcept { return rotationalAxes.IsBitSet(axis); } @@ -631,8 +628,6 @@ public: GCodeResult EutSetMotorCurrents(const CanMessageMultipleDrivesRequest<float>& msg, size_t dataLength, const StringRef& reply) noexcept; GCodeResult EutSetStepsPerMmAndMicrostepping(const CanMessageMultipleDrivesRequest<StepsPerUnitAndMicrostepping>& msg, size_t dataLength, const StringRef& reply) noexcept; GCodeResult EutHandleSetDriverStates(const CanMessageMultipleDrivesRequest<DriverStateControl>& msg, const StringRef& reply) noexcept; - float EutGetRemotePressureAdvance(size_t driver) const noexcept; - GCodeResult EutSetRemotePressureAdvance(const CanMessageMultipleDrivesRequest<float>& msg, size_t dataLength, const StringRef& reply) noexcept; GCodeResult EutProcessM569(const CanMessageGeneric& msg, const StringRef& reply) noexcept; #endif @@ -661,10 +656,6 @@ private: void IterateLocalDrivers(size_t axisOrExtruder, function_ref<void(uint8_t) /*noexcept*/ > func) noexcept { IterateDrivers(axisOrExtruder, func); } #endif -#if SUPPORT_REMOTE_COMMANDS - float remotePressureAdvance[NumDirectDrivers]; -#endif - #if HAS_SMART_DRIVERS void ReportDrivers(MessageType mt, DriversBitmap& whichDrivers, const char* text, bool& reported) noexcept; #endif @@ -716,10 +707,10 @@ private: uint16_t microstepping[MaxAxesPlusExtruders]; // the microstepping used for each axis or extruder, top bit is set if interpolation enabled volatile DriverStatus driverState[MaxAxesPlusExtruders]; - float maxFeedrates[MaxAxesPlusExtruders]; - float accelerations[MaxAxesPlusExtruders]; + float maxFeedrates[MaxAxesPlusExtruders]; // max feed rates in mm per step clock + float accelerations[MaxAxesPlusExtruders]; // max accelerations in mm per step clock squared float driveStepsPerUnit[MaxAxesPlusExtruders]; - float instantDvs[MaxAxesPlusExtruders]; + float instantDvs[MaxAxesPlusExtruders]; // max jerk in mm per step clock uint32_t driveDriverBits[MaxAxesPlusExtruders + NumDirectDrivers]; // the bitmap of local driver port bits for each axis or extruder, followed by the bitmaps for the individual Z motors AxisDriversConfig axisDrivers[MaxAxes]; // the driver numbers assigned to each axis @@ -730,7 +721,6 @@ private: AxesBitmap shortcutAxes; // axes that wrap modulo 360 and for which G0 may choose the shortest direction #endif - float pressureAdvance[MaxExtruders]; #if SUPPORT_NONLINEAR_EXTRUSION float nonlinearExtrusionA[MaxExtruders], nonlinearExtrusionB[MaxExtruders], nonlinearExtrusionLimit[MaxExtruders]; #endif @@ -740,7 +730,7 @@ private: uint32_t slowDriversBitmap; // bitmap of driver port bits that need extended step pulse timing uint32_t steppingEnabledDriversBitmap; // mask of driver bits that we haven't disabled temporarily float idleCurrentFactor; - float minimumMovementSpeed; + float minimumMovementSpeed; // minimum allowed movement speed in mm per step clock #if HAS_SMART_DRIVERS size_t numSmartDrivers; // the number of TMC drivers we have, the remaining are simple enable/step/dir drivers @@ -925,7 +915,7 @@ inline const float* Platform::Accelerations() const noexcept inline void Platform::SetAcceleration(size_t drive, float value) noexcept { - accelerations[drive] = max<float>(value, 1.0); // don't allow zero or negative + accelerations[drive] = max<float>(value, ConvertAcceleration(MinimumAcceleration)); // don't allow zero or negative } inline float Platform::MaxFeedrate(size_t drive) const noexcept @@ -935,17 +925,22 @@ inline float Platform::MaxFeedrate(size_t drive) const noexcept inline void Platform::SetMaxFeedrate(size_t drive, float value) noexcept { - maxFeedrates[drive] = max<float>(value, minimumMovementSpeed); // don't allow zero or negative, but do allow small values + maxFeedrates[drive] = max<float>(value, minimumMovementSpeed); // don't allow zero or negative, but do allow small values } -inline float Platform::GetInstantDv(size_t drive) const noexcept +inline void Platform::SetInstantDv(size_t drive, float value) noexcept { - return instantDvs[drive]; + instantDvs[drive] = max<float>(value, ConvertSpeedFromMmPerSec(MinimumJerk)); // don't allow zero or negative values, they causes Move to loop indefinitely } -inline void Platform::SetInstantDv(size_t drive, float value) noexcept +inline void Platform::SetMinMovementSpeed(float value) noexcept { - instantDvs[drive] = max<float>(value, 0.1); // don't allow zero or negative values, they causes Move to loop indefinitely + minimumMovementSpeed = max<float>(value, ConvertSpeedFromMmPerSec(AbsoluteMinFeedrate)); +} + +inline float Platform::GetInstantDv(size_t drive) const noexcept +{ + return instantDvs[drive]; } inline size_t Platform::GetNumActualDirectDrivers() const noexcept @@ -1063,11 +1058,6 @@ inline IPAddress Platform::GateWay() const noexcept return gateWay; } -inline float Platform::GetPressureAdvance(size_t extruder) const noexcept -{ - return (extruder < MaxExtruders) ? pressureAdvance[extruder] : 0.0; -} - inline float Platform::GetFilamentWidth() const noexcept { return filamentWidth; diff --git a/src/Platform/PortControl.cpp b/src/Platform/PortControl.cpp index 0632f0a5..26256efe 100644 --- a/src/Platform/PortControl.cpp +++ b/src/Platform/PortControl.cpp @@ -60,7 +60,7 @@ uint32_t PortControl::UpdatePorts() noexcept { SetBasePriority(0); UpdatePorts(cdda->GetIoBits()); - return (moveEndTime - now + StepTimer::StepClockRate/1000 - 1)/(StepTimer::StepClockRate/1000); + return (moveEndTime - now + StepClockRate/1000 - 1)/(StepClockRate/1000); } cdda = cdda->GetNext(); st = cdda->GetState(); @@ -96,7 +96,18 @@ bool PortControl::Configure(GCodeBuffer& gb, const StringRef& reply) { seen = true; advanceMillis = (unsigned int)constrain<int>(gb.GetIValue(), 0, 1000); - advanceClocks = (advanceMillis * (uint64_t)StepTimer::StepClockRate)/1000; + if constexpr (StepClockRate % 1000 == 0) + { + advanceClocks = advanceMillis * (StepClockRate/1000); + } + else if constexpr (StepClockRate % 500 == 0) + { + advanceClocks = (advanceMillis * (StepClockRate/500))/2; + } + else + { + advanceClocks = (advanceMillis * (uint64_t)StepClockRate)/1000; + } } if (!seen) { diff --git a/src/Platform/RepRap.cpp b/src/Platform/RepRap.cpp index a007c5d3..7df57d79 100644 --- a/src/Platform/RepRap.cpp +++ b/src/Platform/RepRap.cpp @@ -839,7 +839,7 @@ void RepRap::Spin() noexcept void RepRap::Timing(MessageType mtype) noexcept { - platform->MessageF(mtype, "Slowest loop: %.2fms; fastest: %.2fms\n", (double)(slowLoop * StepTimer::StepClocksToMillis), (double)(fastLoop * StepTimer::StepClocksToMillis)); + platform->MessageF(mtype, "Slowest loop: %.2fms; fastest: %.2fms\n", (double)(slowLoop * StepClocksToMillis), (double)(fastLoop * StepClocksToMillis)); fastLoop = UINT32_MAX; slowLoop = 0; } @@ -1356,7 +1356,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source) con AppendFloatArray(response, "extr", GetExtrudersInUse(), [this](size_t extruder) noexcept { return move->LiveCoordinate(ExtruderToLogicalDrive(extruder), currentTool); }, 1); // Current speeds - response->catf("},\"speeds\":{\"requested\":%.1f,\"top\":%.1f}", (double)move->GetRequestedSpeed(), (double)move->GetTopSpeed()); + response->catf("},\"speeds\":{\"requested\":%.1f,\"top\":%.1f}", (double)InverseConvertSpeedToMmPerSec(move->GetRequestedSpeed()), (double)InverseConvertSpeedToMmPerSec(move->GetTopSpeed())); // Current tool number response->catf(",\"currentTool\":%d", GetCurrentToolNumber()); @@ -1839,7 +1839,7 @@ OutputBuffer *RepRap::GetConfigResponse() noexcept // Accelerations response->cat(','); - AppendFloatArray(response, "accelerations", MaxAxesPlusExtruders, [this](size_t drive) noexcept { return platform->Acceleration(drive); }, 2); + AppendFloatArray(response, "accelerations", MaxAxesPlusExtruders, [this](size_t drive) noexcept { return InverseConvertAcceleration(platform->Acceleration(drive)); }, 2); // Motor currents response->cat(','); @@ -1886,11 +1886,11 @@ OutputBuffer *RepRap::GetConfigResponse() noexcept response->catf(",\"idleTimeout\":%.1f,", (double)(move->IdleTimeout())); // Minimum feedrates - AppendFloatArray(response, "minFeedrates", MaxAxesPlusExtruders, [this](size_t drive) noexcept { return platform->GetInstantDv(drive); }, 2); + AppendFloatArray(response, "minFeedrates", MaxAxesPlusExtruders, [this](size_t drive) noexcept { return InverseConvertSpeedToMmPerSec(platform->GetInstantDv(drive)); }, 2); // Maximum feedrates response->cat(','); - AppendFloatArray(response, "maxFeedrates", MaxAxesPlusExtruders, [this](size_t drive) noexcept { return platform->MaxFeedrate(drive); }, 2); + AppendFloatArray(response, "maxFeedrates", MaxAxesPlusExtruders, [this](size_t drive) noexcept { return InverseConvertSpeedToMmPerSec(platform->MaxFeedrate(drive)); }, 2); // Config file is no longer included, because we can use rr_configfile or M503 instead response->cat('}'); diff --git a/src/RepRapFirmware.h b/src/RepRapFirmware.h index edc921e0..2b96fba9 100644 --- a/src/RepRapFirmware.h +++ b/src/RepRapFirmware.h @@ -169,7 +169,7 @@ struct DriverId DriverId() noexcept : localDriver(0), boardAddress(CanInterface::GetCanAddress()) { } - // Constructor used by ATE configurations + // Constructor used by ATE configurations and object model DriverId(CanAddress addr, uint8_t drv) noexcept : localDriver(drv), boardAddress(addr) { } void SetFromBinary(uint32_t val) noexcept @@ -212,6 +212,9 @@ struct DriverId DriverId() noexcept : localDriver(0) { } + // Constructor used by object model + DriverId(uint8_t drv) noexcept : localDriver(drv) { } + // Set the driver ID from the binary value, returning true if there was a nonzero board number so that the caller knows the address is not valid bool SetFromBinary(uint32_t val) noexcept { @@ -494,6 +497,7 @@ const AxesBitmap XyAxes = AxesBitmap::MakeLowestNBits(XY_AXES); // Common conversion factors constexpr float MinutesToSeconds = 60.0; +constexpr uint32_t iMinutesToSeconds = 60; constexpr float SecondsToMinutes = 1.0/MinutesToSeconds; constexpr float SecondsToMillis = 1000.0; constexpr float MillisToSeconds = 0.001; @@ -503,6 +507,61 @@ constexpr float TwoPi = 3.141592653589793 * 2; constexpr float DegreesToRadians = 3.141592653589793/180.0; constexpr float RadiansToDegrees = 180.0/3.141592653589793; +// The step clock is used for timing step pulses and oyther fine-resolution timer purposes + +#if SAME70 || SAME5x +// All Duet 3 boards use a common step clock rate of 750kHz so that we can sync the clocks over CAN +constexpr uint32_t StepClockRate = 48000000/64; // 750kHz +#elif defined(__LPC17xx__) +constexpr uint32_t StepClockRate = 1000000; // 1MHz +#else +constexpr uint32_t StepClockRate = SystemCoreClockFreq/128; // Duet 2 and Maestro: use just under 1MHz +#endif + +constexpr uint64_t StepClockRateSquared = (uint64_t)StepClockRate * StepClockRate; +constexpr float StepClocksToMillis = 1000.0/(float)StepClockRate; + +// Functions to convert speeds and accelerations between seconds and step clocks +static inline constexpr float ConvertSpeedFromMmPerSec(float speed) noexcept +{ + return speed * 1.0/StepClockRate; +} + +static inline constexpr float ConvertSpeedFromMmPerMin(float speed) noexcept +{ + return speed * (1.0/(StepClockRate * iMinutesToSeconds)); +} + +static inline constexpr float ConvertSpeedFromMm(float speed, bool useSeconds) noexcept +{ + return speed * ((useSeconds) ? 1.0/StepClockRate : 1.0/(StepClockRate * iMinutesToSeconds)); +} + +static inline constexpr float InverseConvertSpeedToMmPerSec(float speed) noexcept +{ + return speed * StepClockRate; +} + +static inline constexpr float InverseConvertSpeedToMmPerMin(float speed) noexcept +{ + return speed * (StepClockRate * iMinutesToSeconds); +} + +static inline constexpr float InverseConvertSpeedToMm(float speed, bool useSeconds) noexcept +{ + return speed * ((useSeconds) ? StepClockRate : StepClockRate * iMinutesToSeconds); +} + +static inline constexpr float ConvertAcceleration(float accel) noexcept +{ + return accel * (1.0/StepClockRateSquared); +} + +static inline constexpr float InverseConvertAcceleration(float accel) noexcept +{ + return accel * StepClockRateSquared; +} + constexpr unsigned int MaxFloatDigitsDisplayedAfterPoint = 7; const char *GetFloatFormatString(unsigned int numDigitsAfterPoint) noexcept; diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp index 20ff5f09..53732326 100644 --- a/src/Tools/Tool.cpp +++ b/src/Tools/Tool.cpp @@ -94,30 +94,30 @@ constexpr ObjectModelTableEntry Tool::objectModelTable[] = { // Within each group, these entries must be in alphabetical order // 0. Tool members - { "active", OBJECT_MODEL_FUNC_NOSELF(&activeTempsArrayDescriptor), ObjectModelEntryFlags::live }, - { "axes", OBJECT_MODEL_FUNC_NOSELF(&axesArrayDescriptor), ObjectModelEntryFlags::none }, - { "extruders", OBJECT_MODEL_FUNC_NOSELF(&extrudersArrayDescriptor), ObjectModelEntryFlags::none }, - { "fans", OBJECT_MODEL_FUNC(self->fanMapping), ObjectModelEntryFlags::none }, - { "filamentExtruder", OBJECT_MODEL_FUNC((int32_t)self->filamentExtruder), ObjectModelEntryFlags::none }, - { "heaters", OBJECT_MODEL_FUNC_NOSELF(&heatersArrayDescriptor), ObjectModelEntryFlags::none }, - { "isRetracted", OBJECT_MODEL_FUNC(self->IsRetracted()), ObjectModelEntryFlags::live }, - { "mix", OBJECT_MODEL_FUNC_NOSELF(&mixArrayDescriptor), ObjectModelEntryFlags::none }, - { "name", OBJECT_MODEL_FUNC(self->name), ObjectModelEntryFlags::none }, - { "number", OBJECT_MODEL_FUNC((int32_t)self->myNumber), ObjectModelEntryFlags::none }, - { "offsets", OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), ObjectModelEntryFlags::none }, - { "offsetsProbed", OBJECT_MODEL_FUNC((int32_t)self->axisOffsetsProbed.GetRaw()), ObjectModelEntryFlags::none }, - { "retraction", OBJECT_MODEL_FUNC(self, 1), ObjectModelEntryFlags::none }, - { "spindle", OBJECT_MODEL_FUNC((int32_t)self->spindleNumber), ObjectModelEntryFlags::none }, - { "spindleRpm", OBJECT_MODEL_FUNC((int32_t)self->spindleRpm), ObjectModelEntryFlags::none }, - { "standby", OBJECT_MODEL_FUNC_NOSELF(&standbyTempsArrayDescriptor), ObjectModelEntryFlags::live }, - { "state", OBJECT_MODEL_FUNC(self->state.ToString()), ObjectModelEntryFlags::live }, + { "active", OBJECT_MODEL_FUNC_NOSELF(&activeTempsArrayDescriptor), ObjectModelEntryFlags::live }, + { "axes", OBJECT_MODEL_FUNC_NOSELF(&axesArrayDescriptor), ObjectModelEntryFlags::none }, + { "extruders", OBJECT_MODEL_FUNC_NOSELF(&extrudersArrayDescriptor), ObjectModelEntryFlags::none }, + { "fans", OBJECT_MODEL_FUNC(self->fanMapping), ObjectModelEntryFlags::none }, + { "filamentExtruder", OBJECT_MODEL_FUNC((int32_t)self->filamentExtruder), ObjectModelEntryFlags::none }, + { "heaters", OBJECT_MODEL_FUNC_NOSELF(&heatersArrayDescriptor), ObjectModelEntryFlags::none }, + { "isRetracted", OBJECT_MODEL_FUNC(self->IsRetracted()), ObjectModelEntryFlags::live }, + { "mix", OBJECT_MODEL_FUNC_NOSELF(&mixArrayDescriptor), ObjectModelEntryFlags::none }, + { "name", OBJECT_MODEL_FUNC(self->name), ObjectModelEntryFlags::none }, + { "number", OBJECT_MODEL_FUNC((int32_t)self->myNumber), ObjectModelEntryFlags::none }, + { "offsets", OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), ObjectModelEntryFlags::none }, + { "offsetsProbed", OBJECT_MODEL_FUNC((int32_t)self->axisOffsetsProbed.GetRaw()), ObjectModelEntryFlags::none }, + { "retraction", OBJECT_MODEL_FUNC(self, 1), ObjectModelEntryFlags::none }, + { "spindle", OBJECT_MODEL_FUNC((int32_t)self->spindleNumber), ObjectModelEntryFlags::none }, + { "spindleRpm", OBJECT_MODEL_FUNC((int32_t)self->spindleRpm), ObjectModelEntryFlags::none }, + { "standby", OBJECT_MODEL_FUNC_NOSELF(&standbyTempsArrayDescriptor), ObjectModelEntryFlags::live }, + { "state", OBJECT_MODEL_FUNC(self->state.ToString()), ObjectModelEntryFlags::live }, // 1. Tool.retraction members - { "extraRestart", OBJECT_MODEL_FUNC(self->retractExtra, 1), ObjectModelEntryFlags::none }, - { "length", OBJECT_MODEL_FUNC(self->retractLength, 1), ObjectModelEntryFlags::none }, - { "speed" , OBJECT_MODEL_FUNC(self->retractSpeed, 1), ObjectModelEntryFlags::none }, - { "unretractSpeed", OBJECT_MODEL_FUNC(self->unRetractSpeed, 1), ObjectModelEntryFlags::none }, - { "zHop", OBJECT_MODEL_FUNC(self->retractHop, 2), ObjectModelEntryFlags::none }, + { "extraRestart", OBJECT_MODEL_FUNC(self->retractExtra, 1), ObjectModelEntryFlags::none }, + { "length", OBJECT_MODEL_FUNC(self->retractLength, 1), ObjectModelEntryFlags::none }, + { "speed" , OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->retractSpeed), 1), ObjectModelEntryFlags::none }, + { "unretractSpeed", OBJECT_MODEL_FUNC(InverseConvertSpeedToMmPerSec(self->unRetractSpeed), 1), ObjectModelEntryFlags::none }, + { "zHop", OBJECT_MODEL_FUNC(self->retractHop, 2), ObjectModelEntryFlags::none }, }; constexpr uint8_t Tool::objectModelTableDescriptor[] = { 2, 17, 5 }; @@ -217,7 +217,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE(Tool) t->retractLength = DefaultRetractLength; t->retractExtra = 0.0; t->retractHop = 0.0; - t->retractSpeed = t->unRetractSpeed = DefaultRetractSpeed * SecondsToMinutes; + t->retractSpeed = t->unRetractSpeed = ConvertSpeedFromMmPerMin(DefaultRetractSpeed); t->isRetracted = false; t->spindleNumber = spindleNo; t->spindleRpm = 0; @@ -777,12 +777,12 @@ GCodeResult Tool::SetFirmwareRetraction(GCodeBuffer &gb, const StringRef &reply, } if (gb.Seen('F')) { - unRetractSpeed = retractSpeed = max<float>(gb.GetFValue(), 60.0) * SecondsToMinutes; + unRetractSpeed = retractSpeed = max<float>(gb.GetSpeedFromMm(false), ConvertSpeedFromMmPerMin(MinRetractSpeed)); seen = true; } if (gb.Seen('T')) // must do this one after 'F' { - unRetractSpeed = max<float>(gb.GetFValue(), 60.0) * SecondsToMinutes; + unRetractSpeed = max<float>(gb.GetSpeedFromMm(false), ConvertSpeedFromMmPerMin(MinRetractSpeed)); seen = true; } if (gb.Seen('Z')) @@ -803,7 +803,7 @@ GCodeResult Tool::SetFirmwareRetraction(GCodeBuffer &gb, const StringRef &reply, return GCodeResult::notFinished; } outBuf->lcatf("Tool %u retract/reprime: length %.2f/%.2fmm, speed %.1f/%.1fmm/sec, Z hop %.2fmm", - myNumber, (double)retractLength, (double)(retractLength + retractExtra), (double)retractSpeed, (double)unRetractSpeed, (double)retractHop); + myNumber, (double)retractLength, (double)(retractLength + retractExtra), (double)InverseConvertSpeedToMmPerSec(retractSpeed), (double)InverseConvertSpeedToMmPerSec(unRetractSpeed), (double)retractHop); } return GCodeResult::ok; } diff --git a/src/Tools/Tool.h b/src/Tools/Tool.h index 01c26ffc..a2f63d2a 100644 --- a/src/Tools/Tool.h +++ b/src/Tools/Tool.h @@ -157,8 +157,8 @@ private: // Firmware retraction settings float retractLength, retractExtra; // retraction length and extra length to un-retract - float retractSpeed; // retract speed in mm/min - float unRetractSpeed; // un=retract speed in mm/min + float retractSpeed; // retract speed in mm per step clock + float unRetractSpeed; // un-retract speed in mm per step clock float retractHop; // Z hop when retracting FansBitmap fanMapping; diff --git a/src/Version.h b/src/Version.h index 93ce94e7..c3d8d37e 100644 --- a/src/Version.h +++ b/src/Version.h @@ -9,7 +9,7 @@ #define SRC_VERSION_H_ #ifndef VERSION -# define MAIN_VERSION "3.4.0beta2" +# define MAIN_VERSION "3.4.0beta2-inputshaping" # ifdef USE_CAN0 # define VERSION_SUFFIX " (CAN0)" # else |