// Print what is wrong with the grid, appending it to the existing string
void GridDefinition::PrintError(float originalXrange, float originalYrange, StringRef& r) const
{
if (spacing < MinSpacing)
{
r.cat("Spacing too small");
}
else if (numX == 0)
{
r.cat("X range too small");
}
else if (numY == 0)
{
r.cat("Y range too small");
}
else if ( numX > MaxXGridPoints
|| numX > MaxGridProbePoints || numY > MaxGridProbePoints // check X and Y individually in case X*Y overflows
|| NumPoints() > MaxGridProbePoints
)
{
const float totalRange = originalXrange + originalYrange;
const float area = originalXrange * originalYrange;
const float minSpacing = (totalRange + sqrtf(fsquare(totalRange) + 4.0 * (MaxGridProbePoints - 1) * area))/(2.0 * (MaxGridProbePoints - 1));
const float minXspacing = originalXrange/(MaxXGridPoints - 1);
r.catf("Too many grid points; suggest increase spacing to %.1fmm", max<float>(minSpacing, minXspacing));
}
else
{
// The only thing left is a bad radius
r.cat("Bad radius");
}
}
/* Input: Q, Q', Q-Q'
* Output: 2Q, Q+Q'
*
* x2 z3: long form
* x3 z3: long form
* x z: short form, destroyed
* xprime zprime: short form, destroyed
* qmqp: short form, preserved
*/
void fmonty(felem *x2, felem *z2, /* output 2Q */
felem *x3, felem *z3, /* output Q + Q' */
felem *x, felem *z, /* input Q */
felem *xprime, felem *zprime, /* input Q' */
const felem *qmqp /* input Q - Q' */) {
felem origx[10], origxprime[10], zzz[19], xx[19], zz[19], xxprime[19],
zzprime[19], zzzprime[19], xxxprime[19];
memcpy(origx, x, 10 * sizeof(felem));
fsum(x, z);
fdifference(z, origx); /* does x - z */
memcpy(origxprime, xprime, sizeof(felem) * 10);
fsum(xprime, zprime);
fdifference(zprime, origxprime);
fproduct(xxprime, xprime, z);
fproduct(zzprime, x, zprime);
freduce_degree(xxprime);
freduce_coefficients(xxprime);
freduce_degree(zzprime);
freduce_coefficients(zzprime);
memcpy(origxprime, xxprime, sizeof(felem) * 10);
fsum(xxprime, zzprime);
fdifference(zzprime, origxprime);
fsquare(xxxprime, xxprime);
fsquare(zzzprime, zzprime);
fproduct(zzprime, zzzprime, qmqp);
freduce_degree(zzprime);
freduce_coefficients(zzprime);
memcpy(x3, xxxprime, sizeof(felem) * 10);
memcpy(z3, zzprime, sizeof(felem) * 10);
fsquare(xx, x);
fsquare(zz, z);
fproduct(x2, xx, zz);
freduce_degree(x2);
freduce_coefficients(x2);
fdifference(zz, xx); /* does zz = xx - zz */
memset(zzz + 10, 0, sizeof(felem) * 9);
fscalar_product(zzz, zz, 121665);
freduce_degree(zzz);
freduce_coefficients(zzz);
fsum(zzz, xx);
fproduct(z2, zz, zzz);
freduce_degree(z2);
freduce_coefficients(z2);
}
// 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;
}
fmonty(felem *x2, /* output 2Q */
felem *x3, /* output Q + Q' */
felem *x, /* input Q */
felem *xprime, /* input Q' */
const felem *qmqp /* input Q - Q' */) {
felem *const z2 = &x2[5];
felem *const z3 = &x3[5];
felem *const z = &x[5];
felem *const zprime = &xprime[5];
felem origx[5], origxprime[5], zzz[5], xx[5], zz[5], xxprime[5],
zzprime[5], zzzprime[5];
memcpy(origx, x, 5 * sizeof(felem));
fsum(x, z);
fdifference_backwards(z, origx); // does x - z
memcpy(origxprime, xprime, sizeof(felem) * 5);
fsum(xprime, zprime);
fdifference_backwards(zprime, origxprime);
fmul(xxprime, xprime, z);
fmul(zzprime, x, zprime);
memcpy(origxprime, xxprime, sizeof(felem) * 5);
fsum(xxprime, zzprime);
fdifference_backwards(zzprime, origxprime);
fsquare(x3, xxprime);
fsquare(zzzprime, zzprime);
fmul(z3, zzzprime, qmqp);
fsquare(xx, x);
fsquare(zz, z);
fmul(x2, xx, zz);
fdifference_backwards(zz, xx); // does zz = xx - zz
fscalar(zzz, zz); // * 121665
freduce_coefficients(zzz);
fsum(zzz, xx);
fmul(z2, zz, zzz);
}
int main(int argc, char ** argv) {
/* BEGIN DECLARATIONS */
WINFO wi; /* struct for command line input */
/* workspace */
float * v; /* velocity field */
float * p1; /* pressure field, current time step */
float * p0; /* pressure field, last time step */
float * tr; /* storage for traces */
float * tmp; /* used to swap p1 and p0 */
int ix, it; /* counters */
int isrc; /* source counter */
int imf; /* movie frame counter */
int isx; /* source location, in units of dx */
int nxz; /* number of spatial grid points */
/* int nz; local number of gridpoints */
int ntr; /* number of traces */
int nsam; /* number of trace samples */
int nsrc; /* number of shots */
float rz,rx,s; /* precomputed coefficients */
float vmax,vmin; /* max, min velocity values */
/* float two; two */
/* END DECLARATIONS */
sf_init(argc,argv);
/* read inputs from command line */
getinputs(true,&wi);
/* compute number of shots */
nsrc = (wi.isxend-wi.isxbeg)/(wi.iskip); nsrc++;
/* compute number of spatial grid points */
nxz=wi.nx * wi.nz;
/* compute number of traces, samples in each record */
ntr=wi.igxend-wi.igxbeg+1;
nsam=ntr*wi.nt;
/* allocate, initialize p0, p1, v, traces */
p0=sf_floatalloc(nxz);
p1=sf_floatalloc(nxz);
v =sf_floatalloc(nxz);
tr=sf_floatalloc(nsam);
/* read velocity */
sf_floatread(v,nxz,wi.vfile);
/* CFL, sanity checks */
vmax=fgetmax(v,nxz);
vmin=fgetmin(v,nxz);
if (vmax*wi.dt>CFL*fmaxf(wi.dx,wi.dz)) {
sf_warning("CFL criterion violated");
sf_warning("vmax=%e dx=%e dz=%e dt=%e\n",vmax,wi.dx,wi.dz,wi.dt);
sf_error("max permitted dt=%e\n",CFL*fmaxf(wi.dx,wi.dz)/vmax);
}
if (vmin<=0.0)
sf_error("min velocity nonpositive");
/* only square of velocity array needed from here on */
fsquare(v,nxz);
/* precalculate some coefficients */
rz=wi.dt*wi.dt/(wi.dz*wi.dz);
rx=wi.dt*wi.dt/(wi.dx*wi.dx);
s =2.0*(rz+rx);
/* two=2.0;
nz=wi.nz; */
/* shot loop */
isrc=0;
isx=wi.isxbeg;
while (isx <= wi.isxend) {
/* initialize pressure fields, traces */
fzeros(p0,nxz);
fzeros(p1,nxz);
fzeros(tr,nsam);
/* initialize movie frame counter */
imf=0;
/* time loop */
for (it=0;it<wi.nt;it++) {
/* construct next time step, overwrite on p0 */
step_forward(p0,p1,v,wi.nz,wi.nx,rz,rx,s);
/* tack on source */
p0[wi.isz+isx*wi.nz]+=fgetrick(it*wi.dt,wi.freq);
/* swap pointers */
tmp=p0;
p0=p1;
p1=tmp;
/* store trace samples if necessary */
if (NULL != wi.tfile)
for (ix=0;ix<ntr;ix++)
tr[ix*wi.nt+it]=p1[(wi.igxbeg+ix)*wi.nz+wi.igz];
/* write movie snap to file if necessary */
if (NULL != wi.mfile && wi.nm && !(it%wi.nm)) {
sf_floatwrite(p1,nxz,wi.mfile);
imf++;
}
/* next t */
}
/* write traces to file if necessary */
if (NULL != wi.tfile)
sf_floatwrite(tr,nsam,wi.tfile);
isx += wi.iskip;
isrc++;
}
exit(0);
}
static void lowest(double *x, double *y, int n, double *xs, double *ys,
int nleft, int nright, double *w,
int userw, double *rw, int *ok)
{
int nrt, j;
double a, b, c, h, h1, h9, r, range;
x--;
y--;
w--;
rw--;
range = x[n]-x[1];
h = fmax2(*xs-x[nleft], x[nright]-*xs);
h9 = 0.999*h;
h1 = 0.001*h;
/* sum of weights */
a = 0.;
j = nleft;
while (j <= n) {
/* compute weights */
/* (pick up all ties on right) */
w[j] = 0.;
r = fabs(x[j] - *xs);
if (r <= h9) {
if (r <= h1)
w[j] = 1.;
else
w[j] = fcube(1.-fcube(r/h));
if (userw)
w[j] *= rw[j];
a += w[j];
}
else if (x[j] > *xs)
break;
j = j+1;
}
/* rightmost pt (may be greater */
/* than nright because of ties) */
nrt = j-1;
if (a <= 0.)
*ok = 0;
else {
*ok = 1;
/* weighted least squares */
/* make sum of w[j] == 1 */
for(j=nleft ; j<=nrt ; j++)
w[j] /= a;
if (h > 0.) {
a = 0.;
/* use linear fit */
/* weighted center of x values */
for(j=nleft ; j<=nrt ; j++)
a += w[j] * x[j];
b = *xs - a;
c = 0.;
for(j=nleft ; j<=nrt ; j++)
c += w[j]*fsquare(x[j]-a);
if (sqrt(c) > 0.001*range) {
b /= c;
/* points are spread out */
/* enough to compute slope */
for(j=nleft; j <= nrt; j++)
w[j] *= (b*(x[j]-a) + 1.);
}
}
*ys = 0.;
for(j=nleft; j <= nrt; j++)
*ys += w[j] * y[j];
}
}
static
void clowess(double *x, double *y, int n,
double f, int nsteps, double delta,
double *ys, double *rw, double *res)
{
int i, iter, j, last, m1, m2, nleft, nright, ns;
int ok;
double alpha, c1, c9, cmad, cut, d1, d2, denom, r, sc;
if (n < 2) {
ys[0] = y[0]; return;
}
/* nleft, nright, last, etc. must all be shifted to get rid of these: */
x--;
y--;
ys--;
/* at least two, at most n points */
ns = imax2(2, imin2(n, (int)(f*n + 1e-7)));
/* robustness iterations */
iter = 1;
while (iter <= nsteps+1) {
nleft = 1;
nright = ns;
last = 0; /* index of prev estimated point */
i = 1; /* index of current point */
for(;;) {
if (nright < n) {
/* move nleft, nright to right */
/* if radius decreases */
d1 = x[i] - x[nleft];
d2 = x[nright+1] - x[i];
/* if d1 <= d2 with */
/* x[nright+1] == x[nright], */
/* lowest fixes */
if (d1 > d2) {
/* radius will not */
/* decrease by */
/* move right */
nleft++;
nright++;
continue;
}
}
/* fitted value at x[i] */
lowest(&x[1], &y[1], n, &x[i], &ys[i],
nleft, nright, res, iter>1, rw, &ok);
if (!ok) ys[i] = y[i];
/* all weights zero */
/* copy over value (all rw==0) */
if (last < i-1) {
denom = x[i]-x[last];
/* skipped points -- interpolate */
/* non-zero - proof? */
for(j = last+1; j < i; j++) {
alpha = (x[j]-x[last])/denom;
ys[j] = alpha*ys[i] + (1.-alpha)*ys[last];
}
}
/* last point actually estimated */
last = i;
/* x coord of close points */
cut = x[last]+delta;
for (i = last+1; i <= n; i++) {
if (x[i] > cut)
break;
if (x[i] == x[last]) {
ys[i] = ys[last];
last = i;
}
}
i = imax2(last+1, i-1);
if (last >= n)
break;
}
/* residuals */
for(i = 0; i < n; i++)
res[i] = y[i+1] - ys[i+1];
/* overall scale estimate */
sc = 0.;
for(i = 0; i < n; i++) sc += fabs(res[i]);
sc /= n;
/* compute robustness weights */
/* except last time */
if (iter > nsteps)
break;
/* Note: The following code, biweight_{6 MAD|Ri|}
is also used in stl(), loess and several other places.
--> should provide API here (MM) */
for(i = 0 ; i < n ; i++)
rw[i] = fabs(res[i]);
/* Compute cmad := 6 * median(rw[], n) ---- */
/* FIXME: We need C API in R for Median ! */
m1 = n/2;
/* partial sort, for m1 & m2 */
rPsort((double *)rw, (int)n, (int)m1);
if(n % 2 == 0) {
m2 = n-m1-1;
rPsort((double *)rw, (int)n, (int)m2);
cmad = 3.*(rw[m1]+rw[m2]);
}
else { /* n odd */
cmad = 6.*rw[m1];
}
if(cmad < 1e-7 * sc) /* effectively zero */
break;
c9 = 0.999*cmad;
c1 = 0.001*cmad;
for(i = 0 ; i < n ; i++) {
r = fabs(res[i]);
if (r <= c1)
rw[i] = 1.;
else if (r <= c9)
rw[i] = fsquare(1.-fsquare(r/cmad));
else
rw[i] = 0.;
}
iter++;
}
}
// Prepare this DM for an extruder move
void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, size_t drive)
{
const float stepsPerMm = reprap.GetPlatform()->DriveStepsPerUnit(drive) * fabs(dda.directionVector[drive]);
mp.cart.twoCsquaredTimesMmPerStepDivA = (uint64_t)(((float)DDA::stepClockRate * (float)DDA::stepClockRate)/(stepsPerMm * dda.acceleration)) * 2;
// Calculate the elasticity compensation parameter (not needed for axis movements, but we do them anyway to keep the code simple)
const float compensationTime = reprap.GetPlatform()->GetElasticComp(drive);
uint32_t compensationClocks = (uint32_t)(compensationTime * DDA::stepClockRate);
const float accelCompensationDistance = compensationTime * (dda.topSpeed - dda.startSpeed);
const float accelCompensationSteps = accelCompensationDistance * stepsPerMm;
// Calculate the net total step count to allow for compensation (may be negative)
// Note that we add totalSteps in floating point mode, to round the number of steps down consistently
int32_t netSteps = (int32_t)(((dda.endSpeed - dda.startSpeed) * compensationTime * stepsPerMm) + totalSteps);
// Acceleration phase parameters
mp.cart.accelStopStep = (uint32_t)((dda.accelDistance * stepsPerMm) + accelCompensationSteps) + 1;
startSpeedTimesCdivA = params.startSpeedTimesCdivA + compensationClocks;
// Constant speed phase parameters
mp.cart.mmPerStepTimesCdivtopSpeed = (uint32_t)(((float)DDA::stepClockRate * K1)/(stepsPerMm * dda.topSpeed));
accelClocksMinusAccelDistanceTimesCdivTopSpeed = (int32_t)params.accelClocksMinusAccelDistanceTimesCdivTopSpeed - (int32_t)(compensationClocks * params.compFactor);
// Deceleration and reverse phase parameters
// First check whether there is any deceleration at all, otherwise we may get strange results because of rounding errors
if (dda.decelDistance * stepsPerMm < 0.5)
{
totalSteps = netSteps;
mp.cart.decelStartStep = mp.cart.reverseStartStep = netSteps + 1;
topSpeedTimesCdivAPlusDecelStartClocks = 0;
mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
twoDistanceToStopTimesCsquaredDivA = 0;
}
else
{
mp.cart.decelStartStep = (uint32_t)((params.decelStartDistance * stepsPerMm) + accelCompensationSteps) + 1;
const int32_t initialDecelSpeedTimesCdivA = (int32_t)params.topSpeedTimesCdivA - (int32_t)compensationClocks; // signed because it may be negative and we square it
const uint64_t initialDecelSpeedTimesCdivASquared = isquare64(initialDecelSpeedTimesCdivA);
topSpeedTimesCdivAPlusDecelStartClocks = params.topSpeedTimesCdivAPlusDecelStartClocks - compensationClocks;
twoDistanceToStopTimesCsquaredDivA =
initialDecelSpeedTimesCdivASquared + (uint64_t)(((params.decelStartDistance + accelCompensationDistance) * (DDA::stepClockRateSquared * 2))/dda.acceleration);
const float initialDecelSpeed = dda.topSpeed - dda.acceleration * compensationTime;
const float reverseStartDistance = (initialDecelSpeed > 0.0) ? fsquare(initialDecelSpeed)/(2 * dda.acceleration) + params.decelStartDistance : params.decelStartDistance;
// Reverse phase parameters
if (reverseStartDistance >= dda.totalDistance)
{
// No reverse phase
totalSteps = netSteps;
mp.cart.reverseStartStep = netSteps + 1;
mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
}
else
{
mp.cart.reverseStartStep = (initialDecelSpeed < 0.0)
? mp.cart.decelStartStep
: (twoDistanceToStopTimesCsquaredDivA/mp.cart.twoCsquaredTimesMmPerStepDivA) + 1;
// Because the step numbers are rounded down, we may sometimes get a situation in which netSteps = 1 and reverseStartStep = 1.
// This would lead to totalSteps = -1, which must be avoided.
int32_t overallSteps = (int32_t)(2 * (mp.cart.reverseStartStep - 1)) - netSteps;
if (overallSteps > 0)
{
totalSteps = overallSteps;
mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA =
(int64_t)((2 * (mp.cart.reverseStartStep - 1)) * mp.cart.twoCsquaredTimesMmPerStepDivA) - (int64_t)twoDistanceToStopTimesCsquaredDivA;
}
else
{
totalSteps = (uint)max<int32_t>(netSteps, 0);
mp.cart.reverseStartStep = totalSteps + 1;
mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
}
}
}
}
// Set up a real move. Return true if it represents real movement, else false.
bool DDA::Init(const GCodes::RawMove *nextMove, bool doMotorMapping)
{
// 1. Compute the new endpoints and the movement vector
const int32_t *positionNow = prev->DriveCoordinates();
const Move *move = reprap.GetMove();
if (doMotorMapping)
{
move->MotorTransform(nextMove->coords, endPoint); // transform the axis coordinates if on a delta or CoreXY printer
isDeltaMovement = move->IsDeltaMode()
&& (endPoint[X_AXIS] != positionNow[X_AXIS] || endPoint[Y_AXIS] != positionNow[Y_AXIS] || endPoint[Z_AXIS] != positionNow[Z_AXIS]);
}
else
{
isDeltaMovement = false;
}
isPrintingMove = false;
bool realMove = false, xyMoving = false;
const bool isSpecialDeltaMove = (move->IsDeltaMode() && !doMotorMapping);
float accelerations[DRIVES];
const float *normalAccelerations = reprap.GetPlatform()->Accelerations();
for (size_t drive = 0; drive < DRIVES; drive++)
{
accelerations[drive] = normalAccelerations[drive];
if (drive >= AXES || !doMotorMapping)
{
endPoint[drive] = Move::MotorEndPointToMachine(drive, nextMove->coords[drive]);
}
int32_t delta;
if (drive < AXES)
{
endCoordinates[drive] = nextMove->coords[drive];
delta = endPoint[drive] - positionNow[drive];
}
else
{
delta = endPoint[drive];
}
DriveMovement& dm = ddm[drive];
if (drive < AXES && !isSpecialDeltaMove)
{
directionVector[drive] = nextMove->coords[drive] - prev->GetEndCoordinate(drive, false);
dm.state = (isDeltaMovement || delta != 0)
? DMState::moving // on a delta printer, if one tower moves then we assume they all do
: DMState::idle;
}
else
{
directionVector[drive] = (float)delta/reprap.GetPlatform()->DriveStepsPerUnit(drive);
dm.state = (delta != 0) ? DMState::moving : DMState::idle;
}
if (dm.state == DMState::moving)
{
dm.totalSteps = labs(delta); // for now this is the number of net steps, but gets adjusted later if there is a reverse in direction
dm.direction = (delta >= 0); // for now this is the direction of net movement, but gets adjusted later if it is a delta movement
realMove = true;
if (drive < Z_AXIS)
{
xyMoving = true;
}
if (drive >= AXES && xyMoving)
{
if (delta > 0)
{
isPrintingMove = true; // we have both movement and extrusion
}
float compensationTime = reprap.GetPlatform()->GetElasticComp(drive);
if (compensationTime > 0.0)
{
// Compensation causes instant velocity changes equal to acceleration * k, so we may need to limit the acceleration
accelerations[drive] = min<float>(accelerations[drive], reprap.GetPlatform()->ConfiguredInstantDv(drive)/compensationTime);
}
}
}
}
// 2. Throw it away if there's no real movement.
if (!realMove)
{
return false;
}
// 3. Store some values
endStopsToCheck = nextMove->endStopsToCheck;
filePos = nextMove->filePos;
usePressureAdvance = nextMove->usePressureAdvance;
// The end coordinates will be valid at the end of this move if it does not involve endstop checks and is not a special move on a delta printer
endCoordinatesValid = (endStopsToCheck == 0) && (doMotorMapping || !move->IsDeltaMode());
// 4. Normalise the direction vector and compute the amount of motion.
// If there is any XYZ movement, then we normalise it so that the total XYZ movement has unit length.
// This means that the user gets the feed rate that he asked for. It also makes the delta calculations simpler.
if (xyMoving || ddm[Z_AXIS].state == DMState::moving)
{
totalDistance = Normalise(directionVector, DRIVES, AXES);
if (isDeltaMovement)
{
// The following are only needed when doing delta movements. We could defer computing them until Prepare(), which would make simulation faster.
a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]);
cKc = (int32_t)(directionVector[Z_AXIS] * DriveMovement::Kc);
const DeltaParameters& dparams = move->GetDeltaParams();
const float initialX = prev->GetEndCoordinate(X_AXIS, false);
const float initialY = prev->GetEndCoordinate(Y_AXIS, false);
const float diagonalSquared = fsquare(dparams.GetDiagonal());
const float a2b2D2 = a2plusb2 * diagonalSquared;
for (size_t drive = 0; drive < AXES; ++drive)
{
const float A = initialX - dparams.GetTowerX(drive);
const float B = initialY - dparams.GetTowerY(drive);
const float stepsPerMm = reprap.GetPlatform()->DriveStepsPerUnit(drive);
DriveMovement& dm = ddm[drive];
const float aAplusbB = A * directionVector[X_AXIS] + B * directionVector[Y_AXIS];
const float dSquaredMinusAsquaredMinusBsquared = diagonalSquared - fsquare(A) - fsquare(B);
float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared);
dm.mp.delta.hmz0sK = (int32_t)(h0MinusZ0 * stepsPerMm * DriveMovement::K2);
dm.mp.delta.minusAaPlusBbTimesKs = -(int32_t)(aAplusbB * stepsPerMm * DriveMovement::K2);
dm.mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared =
(int64_t)(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * DriveMovement::K2));
// Calculate the distance at which we need to reverse direction.
if (a2plusb2 <= 0.0)
{
// Pure Z movement. We can't use the main calculation because it divides by a2plusb2.
dm.direction = (directionVector[Z_AXIS] >= 0.0);
dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
}
else
{
// The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being above the bed,
// the other root corresponds to the carriages being above the bed.
const float drev = ((directionVector[Z_AXIS] * sqrt(a2b2D2 - fsquare(A * directionVector[Y_AXIS] - B * directionVector[X_AXIS])))
- aAplusbB)/a2plusb2;
if (drev > 0.0 && drev < totalDistance) // if the reversal point is within range
{
// Calculate how many steps we need to move up before reversing
float hrev = directionVector[Z_AXIS] * drev + sqrt(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - a2plusb2 * fsquare(drev));
int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm);
// We may be almost at the peak height already, in which case we don't really have a reversal.
// We must not set reverseStartStep to 1, because then we would set the direction when Prepare() calls CalcStepTime(), before the previous move finishes.
if (numStepsUp < 1 || (dm.direction && (uint32_t)numStepsUp <= dm.totalSteps))
{
dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
}
else
{
dm.mp.delta.reverseStartStep = (uint32_t)numStepsUp + 1;
// Correct the initial direction and the total number of steps
if (dm.direction)
{
// Net movement is up, so we will go up a bit and then down by a lesser amount
dm.totalSteps = (2 * numStepsUp) - dm.totalSteps;
}
else
{
// Net movement is down, so we will go up first and then down by a greater amount
dm.direction = true;
dm.totalSteps = (2 * numStepsUp) + dm.totalSteps;
}
}
}
else
{
dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
}
}
}
}
}
else
{
totalDistance = Normalise(directionVector, DRIVES, DRIVES);
}
// 5. Compute the maximum acceleration available and maximum top speed
float normalisedDirectionVector[DRIVES]; // Used to hold a unit-length vector in the direction of motion
memcpy(normalisedDirectionVector, directionVector, sizeof(normalisedDirectionVector));
Absolute(normalisedDirectionVector, DRIVES);
acceleration = VectorBoxIntersection(normalisedDirectionVector, accelerations, DRIVES);
// Set the speed to the smaller of the requested and maximum speed.
// Also enforce a minimum speed of 0.5mm/sec. We need a minimum speed to avoid overflow in the movement calculations.
float reqSpeed = nextMove->feedRate;
if (isSpecialDeltaMove)
{
// Special case of a raw or homing move on a delta printer
// We use the Cartesian motion system to implement these moves, so the feed rate will be interpreted in Cartesian coordinates.
// This is wrong, we want the feed rate to apply to the drive that is moving the farthest.
float maxDistance = 0.0;
for (size_t axis = 0; axis < AXES; ++axis)
{
if (normalisedDirectionVector[axis] > maxDistance)
{
maxDistance = normalisedDirectionVector[axis];
}
}
if (maxDistance != 0.0) // should always be true
{
reqSpeed /= maxDistance; // because normalisedDirectionVector is unit-normalised
}
}
requestedSpeed = max<float>(0.5, min<float>(reqSpeed, VectorBoxIntersection(normalisedDirectionVector, reprap.GetPlatform()->MaxFeedrates(), DRIVES)));
// On a Cartesian or CoreXY printer, it is OK to limit the X and Y speeds and accelerations independently, and in consequence to allow greater values
// for diagonal moves. On a delta, this is not OK and any movement in the XY plane should be limited to the X/Y axis values, which we assume to be equal.
if (isDeltaMovement)
{
const float xyFactor = sqrt(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[X_AXIS]));
const float maxSpeed = reprap.GetPlatform()->MaxFeedrates()[X_AXIS];
if (requestedSpeed * xyFactor > maxSpeed)
{
requestedSpeed = maxSpeed/xyFactor;
}
const float maxAcceleration = normalAccelerations[X_AXIS];
if (acceleration * xyFactor > maxAcceleration)
{
acceleration = maxAcceleration/xyFactor;
}
}
// 6. Calculate the provisional accelerate and decelerate distances and the top speed
endSpeed = 0.0; // until the next move asks us to adjust it
if (prev->state != provisional)
{
// There is no previous move that we can adjust, so this move must start at zero speed.
startSpeed = 0.0;
}
else
{
// Try to meld this move to the previous move to avoid stop/start
// Assuming that this move ends with zero speed, calculate the maximum possible starting speed: u^2 = v^2 - 2as
float maxStartSpeed = sqrtf(acceleration * totalDistance * 2.0);
prev->targetNextSpeed = min<float>(maxStartSpeed, requestedSpeed);
DoLookahead(prev);
startSpeed = prev->targetNextSpeed;
}
RecalculateMove();
state = provisional;
return true;
}
/* -----------------------------------------------------------------------------
** Shamelessly copied from djb's code
** ---------------------------------------------------------------------------*/
void crecip(felem *out, const felem *z) {
felem z2[10];
felem z9[10];
felem z11[10];
felem z2_5_0[10];
felem z2_10_0[10];
felem z2_20_0[10];
felem z2_50_0[10];
felem z2_100_0[10];
felem t0[10];
felem t1[10];
int i;
/* 2 */ fsquare(z2,z);
/* 4 */ fsquare(t1,z2);
/* 8 */ fsquare(t0,t1);
/* 9 */ fmul(z9,t0,z);
/* 11 */ fmul(z11,z9,z2);
/* 22 */ fsquare(t0,z11);
/* 2^5 - 2^0 = 31 */ fmul(z2_5_0,t0,z9);
/* 2^6 - 2^1 */ fsquare(t0,z2_5_0);
/* 2^7 - 2^2 */ fsquare(t1,t0);
/* 2^8 - 2^3 */ fsquare(t0,t1);
/* 2^9 - 2^4 */ fsquare(t1,t0);
/* 2^10 - 2^5 */ fsquare(t0,t1);
/* 2^10 - 2^0 */ fmul(z2_10_0,t0,z2_5_0);
/* 2^11 - 2^1 */ fsquare(t0,z2_10_0);
/* 2^12 - 2^2 */ fsquare(t1,t0);
/* 2^20 - 2^10 */ for (i = 2;i < 10;i += 2) { fsquare(t0,t1); fsquare(t1,t0); }
/* 2^20 - 2^0 */ fmul(z2_20_0,t1,z2_10_0);
/* 2^21 - 2^1 */ fsquare(t0,z2_20_0);
/* 2^22 - 2^2 */ fsquare(t1,t0);
/* 2^40 - 2^20 */ for (i = 2;i < 20;i += 2) { fsquare(t0,t1); fsquare(t1,t0); }
/* 2^40 - 2^0 */ fmul(t0,t1,z2_20_0);
/* 2^41 - 2^1 */ fsquare(t1,t0);
/* 2^42 - 2^2 */ fsquare(t0,t1);
/* 2^50 - 2^10 */ for (i = 2;i < 10;i += 2) { fsquare(t1,t0); fsquare(t0,t1); }
/* 2^50 - 2^0 */ fmul(z2_50_0,t0,z2_10_0);
/* 2^51 - 2^1 */ fsquare(t0,z2_50_0);
/* 2^52 - 2^2 */ fsquare(t1,t0);
/* 2^100 - 2^50 */ for (i = 2;i < 50;i += 2) { fsquare(t0,t1); fsquare(t1,t0); }
/* 2^100 - 2^0 */ fmul(z2_100_0,t1,z2_50_0);
/* 2^101 - 2^1 */ fsquare(t1,z2_100_0);
/* 2^102 - 2^2 */ fsquare(t0,t1);
/* 2^200 - 2^100 */ for (i = 2;i < 100;i += 2) { fsquare(t1,t0); fsquare(t0,t1); }
/* 2^200 - 2^0 */ fmul(t1,t0,z2_100_0);
/* 2^201 - 2^1 */ fsquare(t0,t1);
/* 2^202 - 2^2 */ fsquare(t1,t0);
/* 2^250 - 2^50 */ for (i = 2;i < 50;i += 2) { fsquare(t0,t1); fsquare(t1,t0); }
/* 2^250 - 2^0 */ fmul(t0,t1,z2_50_0);
/* 2^251 - 2^1 */ fsquare(t1,t0);
/* 2^252 - 2^2 */ fsquare(t0,t1);
/* 2^253 - 2^3 */ fsquare(t1,t0);
/* 2^254 - 2^4 */ fsquare(t0,t1);
/* 2^255 - 2^5 */ fsquare(t1,t0);
/* 2^255 - 21 */ fmul(out,t1,z11);
}
