1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
|
/*
* 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
|
