Status Stroke::traverse(Ticks tCurrent, QuadStepper &stepper) {
Quad<StepCoord> dGoal = goalPos(tCurrent);
if (tStart <= 0) {
return STATUS_STROKE_START;
}
#ifdef TEST
Ticks endTicks = tCurrent - (tStart + dtTotal);
if (endTicks > -5) {
TESTCOUT2("traverse(", endTicks, ") ", dGoal.toString());
}
#endif
Status status = STATUS_BUSY_MOVING;
Quad<StepDV> pulse;
#define PULSE_BLOCK 32 /* pulses emitted without limit checks */
Quad<StepCoord> dPosSeg = dGoal - dPos;
for (bool done = true; ; done = true) {
for (QuadIndex i = 0; i < QUAD_ELEMENTS; i++) {
StepCoord dp = dPosSeg.value[i];
if (dp < -PULSE_BLOCK) {
pulse.value[i] = -PULSE_BLOCK;
done = false;
} else if (dp > PULSE_BLOCK) {
pulse.value[i] = PULSE_BLOCK;
done = false;
} else if (dp) {
pulse.value[i] = dp;
done = false;
} else {
pulse.value[i] = 0;
}
dPosSeg.value[i] -= (StepCoord) pulse.value[i];
dPos.value[i] += (StepCoord) pulse.value[i];
}
if (done) {
break;
}
if (0 > (status = stepper.stepDirection(pulse))) {
return status;
}
if (0 > (status = stepper.stepFast(pulse))) {
return status;
}
}
if (tCurrent >= tStart + dtTotal) {
TESTCOUT3("Stroke::traverse() tCurrent:", tCurrent, " tStart:", tStart, " dtTotal:", dtTotal);
return STATUS_OK;
}
return STATUS_BUSY_MOVING;
}
/**
* Create a line by scaling a known PH5Curve to match the requested linear
* offset.
*/
Status StrokeBuilder::buildLine(Stroke & stroke, Quad<StepCoord> relPos) {
PH5TYPE K[QUAD_ELEMENTS];
PH5TYPE Ksqrt[QUAD_ELEMENTS];
StepCoord pulses = 0;
QuadIndex iMax = 0;
// Determine scale K for each Quad dimension
for (QuadIndex i = 0; i < QUAD_ELEMENTS; i++) {
K[i] = relPos.value[i] / 6400.0;
TESTCOUT2("K[", i, "]:", K[i]);
Ksqrt[i] = sqrt(abs(K[i]));
if (pulses < abs(relPos.value[i])) {
pulses = abs(relPos.value[i]);
iMax = i;
}
}
// Generate scaled PH5Curve coefficients
TESTCOUT1("buildLine:", relPos.toString());
#define Z6400 56.568542495
PHVECTOR<Complex<PH5TYPE> > z[QUAD_ELEMENTS];
PHVECTOR<Complex<PH5TYPE> > q[QUAD_ELEMENTS];
for (QuadIndex i = 0; i < QUAD_ELEMENTS; i++) {
z[i].push_back(Complex<PH5TYPE>());
if (K[i] < 0) {
z[i].push_back(Complex<PH5TYPE>(0, Z6400 * Ksqrt[i]));
z[i].push_back(Complex<PH5TYPE>(0, Z6400 * Ksqrt[i]));
} else {
z[i].push_back(Complex<PH5TYPE>(Z6400 * Ksqrt[i]));
z[i].push_back(Complex<PH5TYPE>(Z6400 * Ksqrt[i]));
}
q[i].push_back(Complex<PH5TYPE>());
q[i].push_back(Complex<PH5TYPE>(3200 * K[i]));
q[i].push_back(Complex<PH5TYPE>(6400 * K[i]));
}
// Use the longest PH5Curve to determine the parametric value for all
PH5Curve<PH5TYPE> phMax(z[iMax], q[iMax]);
PHFeed<PH5TYPE> phfMax(phMax, vMax, vMaxSeconds);
PH5TYPE tS = phfMax.get_tS();
// Calculate the optimal number of segments using weird heuristics
#define MS_PER_SEGMENT 30
int16_t N = 1000 * tS / MS_PER_SEGMENT;
int16_t minSegs = minSegments;
if (minSegs == 0) {
minSegs = 5; // minimum number of acceleration segments
minSegs = (float)minSegs * (float)abs(pulses) / (vMax * vMaxSeconds);
TESTCOUT4("pulses:", pulses, " minSegs:", minSegs, " vMax:", vMax, " vMaxSeconds:", vMaxSeconds);
int16_t minSegsK = abs(pulses) / 200.0;
if (minSegs < minSegsK) {
TESTCOUT1("minSegsK:", minSegsK);
minSegs = minSegsK;
}
minSegs = max((int16_t)16, min((int16_t)(STROKE_SEGMENTS - 1), minSegs));
}
N = max(minSegs, min(maxSegments, (int16_t)N));
if (N >= STROKE_SEGMENTS) {
return STATUS_STROKE_MAXLEN;
}
// Build the stroke dimension by dimension
stroke.clear();
stroke.setTimePlanned(tS);
stroke.length = N;
#define SCALE 2
stroke.scale = SCALE;
PH5TYPE E[STROKE_SEGMENTS + 1];
E[0] = phfMax.Ekt(0, 0);
for (int16_t iSeg = 1; iSeg <= N; iSeg++) {
PH5TYPE fSeg = iSeg / (PH5TYPE)N;
E[iSeg] = phfMax.Ekt(E[iSeg - 1], fSeg);
}
for (QuadIndex i = 0; i < QUAD_ELEMENTS; i++) {
PH5Curve<PH5TYPE> ph(z[i], q[i]);
StepCoord s = 0;
StepCoord v = 0;
for (int16_t iSeg = 1; iSeg <= N; iSeg++) {
PH5TYPE pos = ph.r(E[iSeg]).Re();
if (iSeg == N) {
stroke.dEndPos.value[i] = pos < 0 ? pos - 0.5 : pos + 0.5;
TESTCOUT2("dEndPos.value[", (int) i, "] ", stroke.dEndPos.value[i]);
}
pos /= SCALE;
StepCoord sNew = pos < 0 ? pos - 0.5 : pos + 0.5;
StepCoord vNew = sNew - s;
stroke.vPeak = max(stroke.vPeak, (int32_t)abs(vNew*SCALE));
StepCoord dv = vNew - v;
//TESTCOUT4("iSeg:", iSeg, " sNew:", sNew, " vNew:", vNew, " dv:", dv);
if (dv < (StepCoord) - 127 || (StepCoord) 127 < dv) {
TESTCOUT3(" STATUS_STROKE_SEGPULSES pulses:", dv, " i:", (int)i, " N:", (int)N);
return STATUS_STROKE_SEGPULSES;
}
stroke.seg[iSeg - 1].value[i] = dv;
v = vNew;
s = sNew;
}
}
leastFreeRam = min(leastFreeRam, freeRam());
TESTCOUT3(" N:", N, " tS:", tS, " dEndPos:", stroke.dEndPos.toString());
return STATUS_OK;
}
void f(Quad& q) {
cout << q.toString() << endl
<< q.area() << endl
<< q.perimeter() << endl;
}
