136 lines
3.4 KiB
C++
136 lines
3.4 KiB
C++
/*
|
|
* DeltaProbe.cpp
|
|
*
|
|
* Created on: 20 Apr 2015
|
|
* Author: David
|
|
*/
|
|
|
|
#include "RepRapFirmware.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
|