diff options
Diffstat (limited to 'src/Movement/Move.cpp')
| -rw-r--r-- | src/Movement/Move.cpp | 270 |
1 files changed, 90 insertions, 180 deletions
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp index de7a2658..e8c09e76 100644 --- a/src/Movement/Move.cpp +++ b/src/Movement/Move.cpp @@ -30,8 +30,6 @@ Move::Move() : currentDda(NULL), scheduledMoves(0), completedMoves(0) void Move::Init() { - deltaProbing = false; - // Empty the ring ddaRingGetPointer = ddaRingCheckPointer = ddaRingAddPointer; DDA *dda = ddaRingAddPointer; @@ -183,88 +181,85 @@ void Move::Spin() } } - if (!deltaProbing) + // See whether we need to kick off a move + if (currentDda == nullptr) { - // See whether we need to kick off a move - if (currentDda == nullptr) + // No DDA is executing, so start executing a new one if possible + if (idleCount > 10) // better to have a few moves in the queue so that we can do lookahead { - // No DDA is executing, so start executing a new one if possible - if (idleCount > 10) // better to have a few moves in the queue so that we can do lookahead + DDA *dda = ddaRingGetPointer; + if (dda->GetState() == DDA::provisional) + { + dda->Prepare(); + } + if (dda->GetState() == DDA::frozen) { - DDA *dda = ddaRingGetPointer; - if (dda->GetState() == DDA::provisional) + if (simulationMode != 0) { - dda->Prepare(); + currentDda = dda; // pretend we are executing this move } - if (dda->GetState() == DDA::frozen) + else { - if (simulationMode != 0) + Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure + if (StartNextMove(Platform::GetInterruptClocks())) // start the next move { - currentDda = dda; // pretend we are executing this move + Interrupt(); } - else - { - Platform::DisableStepInterrupt(); // should be disabled already because we weren't executing a move, but make sure - if (StartNextMove(Platform::GetInterruptClocks())) // start the next move - { - Interrupt(); - } - } - iState = IdleState::busy; - } - else if (!simulationMode != 0 && iState == IdleState::busy && !reprap.GetGCodes().IsPaused() && idleTimeout > 0.0) - { - lastMoveTime = reprap.GetPlatform().Time(); // record when we first noticed that the machine was idle - iState = IdleState::timing; - } - else if (!simulationMode != 0 && iState == IdleState::timing && reprap.GetPlatform().Time() - lastMoveTime >= idleTimeout) - { - reprap.GetPlatform().SetDriversIdle(); // put all drives in idle hold - iState = IdleState::idle; } + iState = IdleState::busy; + } + else if (!simulationMode != 0 && iState == IdleState::busy && !reprap.GetGCodes().IsPaused() && idleTimeout > 0.0) + { + lastMoveTime = reprap.GetPlatform().Time(); // record when we first noticed that the machine was idle + iState = IdleState::timing; + } + else if (!simulationMode != 0 && iState == IdleState::timing && reprap.GetPlatform().Time() - lastMoveTime >= idleTimeout) + { + reprap.GetPlatform().SetDriversIdle(); // put all drives in idle hold + iState = IdleState::idle; } } + } - DDA *cdda = currentDda; // currentDda is volatile, so copy it - if (cdda != nullptr) + DDA *cdda = currentDda; // currentDda is volatile, so copy it + if (cdda != nullptr) + { + // See whether we need to prepare any moves + int32_t preparedTime = 0; + uint32_t preparedCount = 0; + DDA::DDAState st; + while ((st = cdda->GetState()) == DDA::completed || st == DDA::executing || st == DDA::frozen) { - // See whether we need to prepare any moves - int32_t preparedTime = 0; - uint32_t preparedCount = 0; - DDA::DDAState st; - while ((st = cdda->GetState()) == DDA::completed || st == DDA::executing || st == DDA::frozen) + preparedTime += cdda->GetTimeLeft(); + ++preparedCount; + cdda = cdda->GetNext(); + if (cdda == ddaRingAddPointer) { - preparedTime += cdda->GetTimeLeft(); - ++preparedCount; - cdda = cdda->GetNext(); - if (cdda == ddaRingAddPointer) - { - break; - } + break; } + } - // If the number of prepared moves will execute in less than the minimum time, prepare another move - while (st == DDA::provisional - && preparedTime < (int32_t)(DDA::stepClockRate/8) // prepare moves one eighth of a second ahead of when they will be needed - && preparedCount < DdaRingLength/2 // but don't prepare more than half the ring - ) - { - cdda->Prepare(); - preparedTime += cdda->GetTimeLeft(); - ++preparedCount; - cdda = cdda->GetNext(); - st = cdda->GetState(); - } + // If the number of prepared moves will execute in less than the minimum time, prepare another move + while (st == DDA::provisional + && preparedTime < (int32_t)(DDA::stepClockRate/8) // prepare moves one eighth of a second ahead of when they will be needed + && preparedCount < DdaRingLength/2 // but don't prepare more than half the ring + ) + { + cdda->Prepare(); + preparedTime += cdda->GetTimeLeft(); + ++preparedCount; + cdda = cdda->GetNext(); + st = cdda->GetState(); + } - // If we are simulating and the move pipeline is reasonably full, simulate completion of the current move - if (simulationMode != 0 && idleCount >= 10) - { - // Simulate completion of the current move + // If we are simulating and the move pipeline is reasonably full, simulate completion of the current move + if (simulationMode != 0 && idleCount >= 10) + { + // Simulate completion of the current move //DEBUG //currentDda->DebugPrint(); - simulationTime += currentDda->CalcTime(); - CurrentMoveCompleted(); - } + simulationTime += currentDda->CalcTime(); + CurrentMoveCompleted(); } } @@ -355,7 +350,7 @@ FilePosition Move::PausePrint(float positions[DRIVES], float& pausedFeedRate, ui if (ddaRingAddPointer != savedDdaRingAddPointer) { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); // We are going to skip some moves. dda points to the last move we are going to print. for (size_t axis = 0; axis < numAxes; ++axis) @@ -444,8 +439,8 @@ void Move::Diagnostics(MessageType mtype) char ch = ' '; for (size_t i = 0; i < DDA::numLoggedProbePositions; ++i) { - float xyzPos[CART_AXES]; - MotorStepsToCartesian(DDA::loggedProbePositions + (CART_AXES * i), CART_AXES, xyzPos); + float xyzPos[XYZ_AXES]; + MotorStepsToCartesian(DDA::loggedProbePositions + (XYZ_AXES * i), XYZ_AXES, XYZ_AXES, xyzPos); p.MessageF(mtype, "%c%.2f,%.2f", ch, xyzPos[X_AXIS], xyzPos[Y_AXIS]); ch = ','; } @@ -480,7 +475,7 @@ void Move::EndPointToMachine(const float coords[], int32_t ep[], size_t numDrive { if (CartesianToMotorSteps(coords, ep)) { - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetTotalAxes(); for (size_t drive = numAxes; drive < numDrives; ++drive) { ep[drive] = MotorEndPointToMachine(drive, coords[drive]); @@ -496,16 +491,16 @@ int32_t Move::MotorEndPointToMachine(size_t drive, float coord) // Convert motor coordinates to machine coordinates. Used after homing and after individual motor moves. // This is computationally expensive on a delta or SCARA machine, so only call it when necessary, and never from the step ISR. -void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numDrives, float machinePos[]) const +void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const { - kinematics->MotorStepsToCartesian(motorPos, reprap.GetPlatform().GetDriveStepsPerUnit(), numDrives, machinePos); + kinematics->MotorStepsToCartesian(motorPos, reprap.GetPlatform().GetDriveStepsPerUnit(), numVisibleAxes, numTotalAxes, machinePos); } // Convert Cartesian coordinates to motor steps, axes only, returning true if successful. // Used to perform movement and G92 commands. -bool Move::CartesianToMotorSteps(const float machinePos[MAX_AXES], int32_t motorPos[MAX_AXES]) const +bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const { - const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), reprap.GetGCodes().GetNumAxes(), motorPos); + const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos); if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda)) { if (b) @@ -520,7 +515,7 @@ bool Move::CartesianToMotorSteps(const float machinePos[MAX_AXES], int32_t motor return b; } -void Move::AxisAndBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes, bool useBedCompensation) const +void Move::AxisAndBedTransform(float xyzPoint[MaxAxes], uint32_t xAxes, bool useBedCompensation) const { AxisTransform(xyzPoint); if (useBedCompensation) @@ -529,14 +524,14 @@ void Move::AxisAndBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes, bool us } } -void Move::InverseAxisAndBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const +void Move::InverseAxisAndBedTransform(float xyzPoint[MaxAxes], uint32_t xAxes) const { InverseBedTransform(xyzPoint, xAxes); InverseAxisTransform(xyzPoint); } // Do the Axis transform BEFORE the bed transform -void Move::AxisTransform(float xyzPoint[MAX_AXES]) const +void Move::AxisTransform(float xyzPoint[MaxAxes]) const { //TODO should we transform U axis instead of/as well as X if we are in dual carriage mode? xyzPoint[X_AXIS] += tanXY*xyzPoint[Y_AXIS] + tanXZ*xyzPoint[Z_AXIS]; @@ -544,7 +539,7 @@ void Move::AxisTransform(float xyzPoint[MAX_AXES]) const } // Invert the Axis transform AFTER the bed transform -void Move::InverseAxisTransform(float xyzPoint[MAX_AXES]) const +void Move::InverseAxisTransform(float xyzPoint[MaxAxes]) const { //TODO should we transform U axis instead of/as well as X if we are in dual carriage mode? xyzPoint[Y_AXIS] -= tanYZ*xyzPoint[Z_AXIS]; @@ -552,12 +547,12 @@ void Move::InverseAxisTransform(float xyzPoint[MAX_AXES]) const } // Do the bed transform AFTER the axis transform -void Move::BedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const +void Move::BedTransform(float xyzPoint[MaxAxes], uint32_t xAxes) const { if (!useTaper || xyzPoint[Z_AXIS] < taperHeight) { float zCorrection = 0.0; - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); unsigned int numXAxes = 0; // Transform the Z coordinate based on the average correction for each axis used as an X axis. @@ -589,10 +584,10 @@ void Move::BedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const } // Invert the bed transform BEFORE the axis transform -void Move::InverseBedTransform(float xyzPoint[MAX_AXES], uint32_t xAxes) const +void Move::InverseBedTransform(float xyzPoint[MaxAxes], uint32_t xAxes) const { float zCorrection = 0.0; - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); unsigned int numXAxes = 0; // Transform the Z coordinate based on the average correction for each axis used as an X axis. @@ -759,49 +754,6 @@ void Move::AdjustMotorPositions(const float_t adjustment[], size_t numMotors) liveCoordinatesValid = false; // force the live XYZ position to be recalculated } -static void ShortDelay() -{ - for (unsigned int i = 0; i < 10; ++i) - { - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - asm volatile("nop"); - } -} - -// This is the function that is called by the timer interrupt to step the motors when we are using the experimental delta probe. -// The movements are quite slow so it is not time-critical. -void Move::DeltaProbeInterrupt() -{ - bool again; - do - { - if (reprap.GetPlatform().GetZProbeResult() == EndStopHit::lowHit) - { - deltaProbe.Trigger(); - } - - const bool dir = deltaProbe.GetDirection(); - Platform& platform = reprap.GetPlatform(); - platform.SetDirection(X_AXIS, dir); - platform.SetDirection(Y_AXIS, dir); - platform.SetDirection(Z_AXIS, dir); - ShortDelay(); - const uint32_t steppersMoving = platform.GetDriversBitmap(X_AXIS) | platform.GetDriversBitmap(Y_AXIS) | platform.GetDriversBitmap(Z_AXIS); - Platform::StepDriversHigh(steppersMoving); - ShortDelay(); - Platform::StepDriversLow(); - const uint32_t tim = deltaProbe.CalcNextStepTime(); - again = (tim != 0xFFFFFFFF && platform.ScheduleInterrupt(tim + deltaProbingStartTime)); - } while (again); -} - // This is called from the step ISR when the current move has been completed void Move::CurrentMoveCompleted() { @@ -837,7 +789,7 @@ bool Move::TryStartNextMove(uint32_t startTime) // This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'. void Move::HitLowStop(size_t axis, DDA* hitDDA) { - if (axis < reprap.GetGCodes().GetNumAxes() && !IsDeltaMode()) // should always be true + if (axis < reprap.GetGCodes().GetTotalAxes() && !IsDeltaMode()) // should always be true { JustHomed(axis, reprap.GetPlatform().AxisMinimum(axis), hitDDA); } @@ -846,7 +798,7 @@ void Move::HitLowStop(size_t axis, DDA* hitDDA) // This is called from the step ISR. Any variables it modifies that are also read by code outside the ISR must be declared 'volatile'. void Move::HitHighStop(size_t axis, DDA* hitDDA) { - if (axis < reprap.GetGCodes().GetNumAxes()) // should always be true + if (axis < reprap.GetGCodes().GetTotalAxes()) // should always be true { const float hitPoint = (IsDeltaMode()) ? ((LinearDeltaKinematics*)kinematics)->GetHomedCarriageHeight(axis) // this is a delta printer, so the motor is at the homed carriage height for this drive @@ -860,13 +812,13 @@ void Move::JustHomed(size_t axisHomed, float hitPoint, DDA* hitDDA) { if (IsCoreXYAxis(axisHomed)) { - float tempCoordinates[CART_AXES]; - for (size_t axis = 0; axis < CART_AXES; ++axis) + float tempCoordinates[XYZ_AXES]; + for (size_t axis = 0; axis < XYZ_AXES; ++axis) { tempCoordinates[axis] = hitDDA->GetEndCoordinate(axis, false); } tempCoordinates[axisHomed] = hitPoint; - hitDDA->SetPositions(tempCoordinates, CART_AXES); + hitDDA->SetPositions(tempCoordinates, XYZ_AXES); } else { @@ -887,7 +839,7 @@ void Move::ZProbeTriggered(DDA* hitDDA) void Move::GetCurrentMachinePosition(float m[DRIVES], bool disableMotorMapping) const { DDA * const lastQueuedMove = ddaRingAddPointer->GetPrevious(); - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); + const size_t numAxes = reprap.GetGCodes().GetVisibleAxes(); for (size_t i = 0; i < DRIVES; i++) { if (i < numAxes) @@ -930,7 +882,7 @@ void Move::GetCurrentUserPosition(float m[DRIVES], uint8_t moveType, uint32_t xA void Move::LiveCoordinates(float m[DRIVES], uint32_t xAxes) { // The live coordinates and live endpoints are modified by the ISR, so be careful to get a self-consistent set of them - const size_t numAxes = reprap.GetGCodes().GetNumAxes(); // do this before we disable interrupts + const size_t numVisibleAxes = reprap.GetGCodes().GetVisibleAxes(); // do this before we disable interrupts cpu_irq_disable(); if (liveCoordinatesValid) { @@ -940,19 +892,20 @@ void Move::LiveCoordinates(float m[DRIVES], uint32_t xAxes) } else { + const size_t numTotalAxes = reprap.GetGCodes().GetTotalAxes(); // do this before we disable interrupts // Only the extruder coordinates are valid, so we need to convert the motor endpoints to coordinates - memcpy(m + numAxes, const_cast<const float *>(liveCoordinates + numAxes), sizeof(m[0]) * (DRIVES - numAxes)); - int32_t tempEndPoints[MAX_AXES]; + memcpy(m + numTotalAxes, const_cast<const float *>(liveCoordinates + numTotalAxes), sizeof(m[0]) * (DRIVES - numTotalAxes)); + int32_t tempEndPoints[MaxAxes]; memcpy(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints)); cpu_irq_enable(); - MotorStepsToCartesian(tempEndPoints, numAxes, m); // this is slow, so do it with interrupts enabled + MotorStepsToCartesian(tempEndPoints, numVisibleAxes, numTotalAxes, m); // this is slow, so do it with interrupts enabled // If the ISR has not updated the endpoints, store the live coordinates back so that we don't need to do it again cpu_irq_disable(); if (memcmp(tempEndPoints, const_cast<const int32_t*>(liveEndPoints), sizeof(tempEndPoints)) == 0) { - memcpy(const_cast<float *>(liveCoordinates), m, sizeof(m[0]) * numAxes); + memcpy(const_cast<float *>(liveCoordinates), m, sizeof(m[0]) * numVisibleAxes); liveCoordinatesValid = true; } cpu_irq_enable(); @@ -969,13 +922,13 @@ void Move::SetLiveCoordinates(const float coords[DRIVES]) liveCoordinates[drive] = coords[drive]; } liveCoordinatesValid = true; - EndPointToMachine(coords, const_cast<int32_t *>(liveEndPoints), reprap.GetGCodes().GetNumAxes()); + EndPointToMachine(coords, const_cast<int32_t *>(liveEndPoints), reprap.GetGCodes().GetVisibleAxes()); } void Move::ResetExtruderPositions() { cpu_irq_disable(); - for (size_t eDrive = reprap.GetGCodes().GetNumAxes(); eDrive < DRIVES; eDrive++) + for (size_t eDrive = reprap.GetGCodes().GetTotalAxes(); eDrive < DRIVES; eDrive++) { liveCoordinates[eDrive] = 0.0; } @@ -1055,47 +1008,4 @@ bool Move::IsCoreXYAxis(size_t axis) const } } -// Do a delta probe returning -1 if still probing, 0 if failed, 1 if success -int Move::DoDeltaProbe(float frequency, float amplitude, float rate, float distance) -{ - if (deltaProbing) - { - if (deltaProbe.Finished()) - { - deltaProbing = false; - return (deltaProbe.Overran()) ? 0 : 1; - } - } - else - { - if (currentDda != nullptr || !DDARingEmpty()) - { - return 0; - } - if (!deltaProbe.Init(frequency, amplitude, rate, distance)) - { - return 0; - } - - const uint32_t firstInterruptTime = deltaProbe.Start(); - if (firstInterruptTime != 0xFFFFFFFF) - { - Platform& platform = reprap.GetPlatform(); - platform.EnableDrive(X_AXIS); - platform.EnableDrive(Y_AXIS); - platform.EnableDrive(Z_AXIS); - deltaProbing = true; - iState = IdleState::busy; - const irqflags_t flags = cpu_irq_save(); - deltaProbingStartTime = platform.GetInterruptClocks(); - if (platform.ScheduleInterrupt(firstInterruptTime + deltaProbingStartTime)) - { - Interrupt(); - } - cpu_irq_restore(flags); - } - } - return -1; -} - // End |