diff options
author | David Crocker <dcrocker@eschertech.com> | 2021-07-05 14:07:44 +0300 |
---|---|---|
committer | David Crocker <dcrocker@eschertech.com> | 2021-07-05 14:07:44 +0300 |
commit | 98b8ff19dc0f1ca9ac94739c5403cb1dc3a1d0cb (patch) | |
tree | 129cab3e320a2d57b30f10cf6714b8dbee7f9f43 /src | |
parent | 22ba858569ca50fd7d787884f49b83bc31c08ffd (diff) |
Corrections to CAN motion for input shaping
Diffstat (limited to 'src')
-rw-r--r-- | src/CAN/CanMotion.cpp | 51 | ||||
-rw-r--r-- | src/CAN/CanMotion.h | 4 |
2 files changed, 54 insertions, 1 deletions
diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp index 8ba75919..662cf8da 100644 --- a/src/CAN/CanMotion.cpp +++ b/src/CAN/CanMotion.cpp @@ -79,7 +79,11 @@ CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexce buf->next = movementBufferList; movementBufferList = buf; +#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) @@ -93,16 +97,26 @@ CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexce else { // 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->numDrivers = canDriver.localDriver + 1; +#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) @@ -110,10 +124,17 @@ CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexce move->perDrive[drive].Init(); } } - else if (canDriver.localDriver >= buf->msg.moveLinear.numDrivers) +#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; } @@ -123,10 +144,16 @@ void CanMotion::AddMovement(const PrepParams& params, DriverId canDriver, int32_ 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; +#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); @@ -138,6 +165,8 @@ void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver } } +#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 { @@ -154,11 +183,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 @@ -248,7 +285,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 @@ -293,11 +334,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 33430252..1edddd10 100644 --- a/src/CAN/CanMotion.h +++ b/src/CAN/CanMotion.h @@ -14,12 +14,16 @@ #include <Movement/DDA.h> +#define USE_REMOTE_INPUT_SHAPING (0) + namespace CanMotion { void Init() noexcept; void StartMovement() noexcept; void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps) noexcept; +#if USE_REMOTE_INPUT_SHAPING void AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept; +#endif uint32_t FinishMovement(uint32_t moveStartTime) noexcept; bool CanPrepareMove() noexcept; CanMessageBuffer *GetUrgentMessage() noexcept; |