// 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); }