diff options
Diffstat (limited to 'src/Movement/DeltaProbe.cpp')
-rw-r--r-- | src/Movement/DeltaProbe.cpp | 139 |
1 files changed, 0 insertions, 139 deletions
diff --git a/src/Movement/DeltaProbe.cpp b/src/Movement/DeltaProbe.cpp deleted file mode 100644 index 37a2f0d8..00000000 --- a/src/Movement/DeltaProbe.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* - * DeltaProbe.cpp - * - * Created on: 20 Apr 2015 - * Author: David - */ - -#include "DeltaProbe.h" -#include "DDA.h" -#include "Platform.h" -#include "RepRap.h" - -// Set up to probe -bool DeltaProbe::Init(float frequency, float amplitude, float rate, float height) -{ -debugPrintf("Start probe f=%.1f a=%.2f r=%.2f h=%.1f\n", frequency, amplitude, rate, height); - // Sanity check the inputs (we check the max amplitude later) - if (frequency < 50.0 || frequency > 1000.0 || amplitude < 0.02 || rate < 0.1 || rate > 10.0 || height < 0.5) - { - return false; - } - -debugPrintf("ok so far\n"); - // Calculate the number of steps for the peak to peak amplitude - const float zRate = reprap.GetPlatform().DriveStepsPerUnit(Z_AXIS); - normalSteps = (size_t)(amplitude * zRate); - if (normalSteps > MaxSteps) - { - return false; - } - -debugPrintf("normalSteps=%u\n", normalSteps); - // Build the tables of step times for sinusoidal motion - const float recipOmega = (float)DDA::stepClockRate/(frequency * 2.0 * PI); - - for (size_t i = 0; i < normalSteps - 1; ++i) - { - normalStepTable[i] = acos(1.0 - (float)(2 * (i + 1))/(float)normalSteps) * recipOmega; - } - - for (size_t i = 0; i < normalSteps; ++i) - { - incStepTable[i] = acos(1.0 - (float)(2 * (i + 1))/(float)(normalSteps + 1)) * recipOmega; - } - - halfCycleTime = (uint32_t)((float)DDA::stepClockRate/(2.0 * frequency)); - incStepTable[normalSteps] = normalStepTable[normalSteps - 1] = halfCycleTime; - - halfCyclesPerIncrement = 2 * (unsigned int)((frequency / (rate * zRate)) + 0.5); - if (halfCyclesPerIncrement < 4) - { - halfCyclesPerIncrement = 4; - } - maxIncrements = height * zRate; - -const float peakAccel = fsquare(2.0 * PI * frequency) * amplitude * 0.5; -debugPrintf("halfCycleTime=%u halfCyclesPerIncrement=%u peak accel=%.1f\n", halfCycleTime, halfCyclesPerIncrement, peakAccel); -debugPrintf("normalTable="); -for (unsigned int i = 0; i < normalSteps; ++i) -{ - debugPrintf(" %u", normalStepTable[i]); -} -debugPrintf(" incStepTable="); -for (unsigned int i = 0; i <= normalSteps; ++i) -{ - debugPrintf(" %u", incStepTable[i]); -} -debugPrintf("\n"); - return true; -} - -// Start probing, and return the time that the next step is due -uint32_t DeltaProbe::Start() -{ - // Initialise the dynamic values - stepsDone = 0; - halfCycleCount = 0; - numIncrements = 0; - incrementing = false; - state = State::normal; - return normalStepTable[0]; -} - -bool DeltaProbe::GetDirection() const -{ - return (halfCycleCount & 1) ? FORWARDS : BACKWARDS; -} - -// Calculate the next step time. Returns 0xFFFFFFFF to stop. -uint32_t DeltaProbe::CalcNextStepTime() -{ - if (state == State::stopped || state == State::overran) - { - return 0xFFFFFFFF; - } - - ++stepsDone; - if (stepsDone == ((incrementing) ? normalSteps + 1 : normalSteps)) - { - stepsDone = 0; - ++halfCycleCount; - if (state == State::stopping && (halfCycleCount & 1) == 0) - { - state = State::stopped; - return 0xFFFFFFFF; - } - - if (incrementing) - { - ++numIncrements; - incrementing = false; - } - - if (halfCycleCount == halfCyclesPerIncrement) - { - if (numIncrements == maxIncrements) - { - state = State::overran; // another increment is due, but we have already gone down as far as we were asked to - return 0xFFFFFFFF; - } - halfCycleCount = 0; - incrementing = true; - } - } - - return (incrementing) - ? (halfCyclesPerIncrement * numIncrements * halfCycleTime) + incStepTable[stepsDone] - : (halfCyclesPerIncrement * numIncrements * halfCycleTime) + normalStepTable[stepsDone]; -} - -void DeltaProbe::Trigger() -{ - if (state == State::normal) - { - state = State::stopping; - } -} - -// End |