4cc0a512b4
Merged in zpl's latest changes to Network and Print Monitor modules, providing DHCP and Netbios name support Added command M999 S4321 to unlock flash memory, reset and boot to BOSSA port Z dive height is now relative to Z probe trigger height and default is reduced to 3mm Added experimental support for acoustic probe for delta printers Bug fix: firmware once again prevents Z homing before X and Y are homed, if the Z probe is used for Z homing Various code tidying
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
|