BeRTOS
|
Compute, save and load ramps for stepper motors (implementation) More...
Go to the source code of this file.
Functions | |
void | ramp_setup (struct Ramp *ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq) |
Setup an acceleration ramp for a stepper motor. | |
void | ramp_default (struct Ramp *ramp) |
Initialize a new ramp with default values. | |
uint16_t FAST_FUNC | ramp_evaluate (const struct Ramp *ramp, uint32_t curClock) |
Evaluate the ramp at the given point. |
Compute, save and load ramps for stepper motors (implementation)
The formula used by the ramp is the following:
a * b f(t) = ------------- lerp(a,b,t)
Where a
and b
are the maximum and minimum speed respectively (minimum and maximum wavelength respectively), and lerp
is a linear interpolation with a factor:
lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t)
t
must be in the [0,1] interval. It is easy to see that the following holds true:
f(0) = b, f(1) = a
And that the function is monotonic. So, the function effectively interpolates between the maximum and minimum speed through its domain ([0,1] -> [b,a]).
The curve drawn by this function is similar to 1 / (sqrt(n)), so it is slower than a linear acceleration (which would be 1/n).
The floating point version uses a slightly modified function which accepts the parameter in the domain [0, MT] (where MT is maxTime, the length of the ramp, which is a setup parameter for the ramp). This is done to reduce the number of operations per step. The formula looks like this:
a * b * MT g(t) = ---------------------------- (a * MT) + t * (b - a)
It can be shown that this g(t) = f(t * MT)
. The denominator is a linear interpolation in the range [b*MT, a*MT], as t moves in the interval [0, MT]. So the interpolation interval of the function is again [b, a]. The implementation caches the value of the numerator and parts of the denominator, so that the formula becomes:
alpha = a * b * MT beta = a * MT gamma = b - a
alpha g(t) = ---------------------- beta + t * gamma
and t
is exactly the parameter that ramp_evaluate() gets, that is the current time (in range [0, MT]). The operations performed for each step are just an addition, a multiplication and a division.
The fixed point version of the formula instead transforms the original function as follows:
a * b a f(t) = ------------------------- = -------------------- a a b * ( - * (1 - t) + t ) - * (1 - t) + t b b
t
must be computed by dividing the current time (24 bit integer) by the maximum time (24 bit integer). This is done by precomputing the reciprocal of the maximum time as a 0.32 fixed point number, and multiplying it to the current time. Multiplication is performed 8-bits a time by FIX_MULT32(), so that we end up with a 0.16 fixed point number for t
(and 1-t
is just its twos-complement negation). a/b
is in the range [0,1] (because a is always less than b, being the minimum wavelength), so it is precomputed as a 0.16 fixed point. The final step is then computing the denominator and executing the division (32 cycles using the 1-step division instruction in the DSP).
The assembly implementation is needed for efficiency, but a C version of it can be easily written, in case it is needed in the future.
Definition in file ramp.c.
uint16_t FAST_FUNC ramp_evaluate | ( | const struct Ramp * | ramp, |
uint32_t | curClock | ||
) |
Evaluate the ramp at the given point.
Given a ramp, and the current clock since the start of the acceleration, compute the next step, that is the interval at which send the signal to the motor.
void ramp_setup | ( | struct Ramp * | ramp, |
uint32_t | length, | ||
uint32_t | minFreq, | ||
uint32_t | maxFreq | ||
) |