From: asterix Date: Thu, 20 Mar 2008 14:39:12 +0000 (+0000) Subject: Move ramp module to appropriate directory. X-Git-Tag: 1.0.0~51 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;ds=sidebyside;h=839bb85440ae6b21ef990fd373008a7a021a9148;hp=f3ba158c1ebfcef6c85759056d0a3d5f421ea870;p=bertos.git Move ramp module to appropriate directory. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1201 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/algo/ramp.c b/algo/ramp.c new file mode 100644 index 00000000..9a2cc92e --- /dev/null +++ b/algo/ramp.c @@ -0,0 +1,206 @@ +/*! + * \file + * + * + * \brief Compute, save and load ramps for stepper motors (implementation) + * + * \version $Id$ + * + * \author Simone Zinanni + * \author Bernardo Innocenti + * \author Giovanni Bajo + * \author Daniele Basile + * + * + * 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. + * + */ + +#include "ramp.h" +#include + +#include // memcpy() + +/** + * Multiply \p a and \p b two integer at 32 bit and extract the high 16 bit word. + */ +#define FIX_MULT32(a,b) (((uint64_t)(a)*(uint32_t)(b)) >> 16) + +void ramp_compute(struct Ramp *ramp, uint32_t clocksRamp, uint16_t clocksMinWL, uint16_t clocksMaxWL) +{ + ASSERT(clocksMaxWL >= clocksMinWL); + + // Save values in ramp struct + ramp->clocksRamp = clocksRamp; + ramp->clocksMinWL = clocksMinWL; + ramp->clocksMaxWL = clocksMaxWL; + +#if RAMP_USE_FLOATING_POINT + ramp->precalc.gamma = ramp->clocksMaxWL - ramp->clocksMinWL; + ramp->precalc.beta = (float)ramp->clocksMinWL * (float)ramp->clocksRamp; + ramp->precalc.alpha = ramp->precalc.beta * (float)ramp->clocksMaxWL; + +#else + ramp->precalc.max_div_min = ((uint32_t)clocksMinWL << 16) / (uint32_t)clocksMaxWL; + + /* Calcola 1/total_time in fixed point .32. Assumiamo che la rampa possa al + * massimo avere 25 bit (cioé valore in tick fino a 2^25, che con il + * prescaler=3 sono circa 7 secondi). Inoltre, togliamo qualche bit di precisione + * da destra (secondo quanto specificato in RAMP_CLOCK_SHIFT_PRECISION). + */ + ASSERT(ramp->clocksRamp < (1UL << (24 + RAMP_CLOCK_SHIFT_PRECISION))); + ramp->precalc.inv_total_time = 0xFFFFFFFFUL / (ramp->clocksRamp >> RAMP_CLOCK_SHIFT_PRECISION); + ASSERT(ramp->precalc.inv_total_time < 0x1000000UL); + +#endif +} + + +void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq) +{ + uint32_t minWL, maxWL; + + minWL = TIME2CLOCKS(FREQ2MICROS(maxFreq)); + maxWL = TIME2CLOCKS(FREQ2MICROS(minFreq)); + + ASSERT2(minWL < 65536UL, "Maximum frequency too high"); + ASSERT2(maxWL < 65536UL, "Minimum frequency too high"); + ASSERT(maxFreq > minFreq); + + ramp_compute( + ramp, + TIME2CLOCKS(length), + TIME2CLOCKS(FREQ2MICROS(maxFreq)), + TIME2CLOCKS(FREQ2MICROS(minFreq)) + ); +} + +void ramp_default(struct Ramp *ramp) +{ + ramp_setup(ramp, RAMP_DEF_TIME, RAMP_DEF_MINFREQ, RAMP_DEF_MAXFREQ); +} + +#if RAMP_USE_FLOATING_POINT + +float ramp_evaluate(const struct Ramp* ramp, float curClock) +{ + return ramp->precalc.alpha / (curClock * ramp->precalc.gamma + ramp->precalc.beta); +} + +#else + +INLINE uint32_t fix_mult32(uint32_t m1, uint32_t m2) +{ + uint32_t accum = 0; + accum += m1 * ((m2 >> 0) & 0xFF); + accum >>= 8; + accum += m1 * ((m2 >> 8) & 0xFF); + accum >>= 8; + accum += m1 * ((m2 >> 16) & 0xFF); + return accum; +} + +// a*b >> 16 +INLINE uint16_t fix_mult16(uint16_t a, uint32_t b) +{ + return (b*(uint32_t)a) >> 16; +} + +uint16_t FAST_FUNC ramp_evaluate(const struct Ramp* ramp, uint32_t curClock) +{ + uint16_t t = FIX_MULT32(curClock >> RAMP_CLOCK_SHIFT_PRECISION, ramp->precalc.inv_total_time); + uint16_t denom = fix_mult16((uint16_t)~t + 1, ramp->precalc.max_div_min) + t; + uint16_t cur_delta = ((uint32_t)ramp->clocksMinWL << 16) / denom; + + return cur_delta; +} + +#endif + + diff --git a/algo/ramp.h b/algo/ramp.h new file mode 100644 index 00000000..f438dc5e --- /dev/null +++ b/algo/ramp.h @@ -0,0 +1,199 @@ + +/** + * \file + * + * + * \brief Compute, save and load ramps for stepper motors (interace) + * + * \version $Id$ + * + * \author Simone Zinanni + * \author Giovanni Bajo + * \author Daniele Basile + * + * The acceleration ramp is used to properly accelerate a stepper motor. The main + * entry point is the function ramp_evaluate(), which must be called at every step + * of the motor: it gets as input the time elapsed since the stepper started + * accelerating, and returns the time to wait before sending the next step. A pseudo + * usage pattern is as follows: + * + * 
+ *  float time = 0;
+ *  while (1)
+ *  {
+ *      float delta = ramp_evaluate(&my_ramp, time);
+ *      sleep(delta);
+ *      do_motor_step();
+ *      time += delta;
+ *  }
+ *
+ * + * A similar pattern can be used to decelerate (it is sufficient to move the total + * time backward, such as "time -= delta"). + * + * The ramp can be configured with ramp_setup(), providing it with the minimum and + * maximum operating frequency of the motor, and the total acceleration time in + * milliseconds (that is, the time that will be needed to accelerate from the + * minimum frequency to the maximum frequency). + * + * Both a very precise floating point and a very fast fixed point implementation + * of the ramp evaluation are provided. The fixed point is hand-optimized assembly + * for DSP56000 (but a portable C version of it can be easily written, see the + * comments in the code). + * + */ + +#ifndef ALGO_RAMP_H +#define ALGO_RAMP_H + +#include +#include "hw_stepper.h" + +/** + * Define whether the ramp will use floating point calculation within ramp_evaluate(). + * Otherwise, a less precise fixed point version will be used, which is faster on + * platforms which do no support floating point operations. + * + * \note Floating point operations will be always done within ramp_compute() to + * precalculate values, so there has to be at least a floating point emulation support. + */ +#define RAMP_USE_FLOATING_POINT 0 + + +#if !RAMP_USE_FLOATING_POINT + + /** + * Number of least-significant bits which are stripped away during ramp evaluation. + * This setting allows to specify larger ramps at the price of less precision. + * + * The maximum ramp size allowed is 2^(24 + RAMP_CLOCK_SHIFT_PRECISION), in clocks. + * For instance, using RAMP_CLOCK_SHIFT_PRECISION 1, and a 8x prescaler, the maximum + * length of a ramp is about 6.7 secs. Raising RAMP_CLOCK_SHIFT_PRECISION to 2 + * brings the maximum length to 13.4 secs, at the price of less precision. + * + * ramp_compute() will check that the length is below the maximum allowed through + * a runtime assertion. + * + * \note This macro is used only for the fixed-point version of the ramp. + */ + #define RAMP_CLOCK_SHIFT_PRECISION 2 +#endif + + +///< Negative pulse width for ramp +#define RAMP_PULSE_WIDTH 50 + +///< Default ramp +#define RAMP_DEF_TIME 6000000 ///< microsecs +#define RAMP_DEF_MAXFREQ 5000 ///< Hz +#define RAMP_DEF_MINFREQ 200 ///< Hz +#define RAMP_DEF_POWERRUN 10 ///< 10 deciampere (1 ampere) +#define RAMP_DEF_POWERIDLE 1 ///< 1 deciampere + + +/** + * Convert microseconds to timer clock ticks + */ +#define TIME2CLOCKS(micros) ((uint32_t)(micros) * (STEPPER_CLOCK / 1000000)) + +/** + * Convert timer clock ticks back to microseconds + */ +#define CLOCKS2TIME(clocks) ((uint32_t)(clocks) / (STEPPER_CLOCK / 1000000)) + +/** + * Convert microseconds to Hz + */ +#define MICROS2FREQ(micros) (1000000UL / ((uint32_t)(micros))) + +/** + * Convert frequency (in Hz) to time (in microseconds) + */ +#define FREQ2MICROS(hz) (1000000UL / ((uint32_t)(hz))) + + + +/** + * Structure holding pre-calculated data for speeding up real-time evaluation + * of the ramp. This structure is totally different between the fixed and the + * floating point version of the code. + * + * Consult the file-level documentation of ramp.c for more information about + * the values of this structure. + */ +struct RampPrecalc +{ +#if RAMP_USE_FLOATING_POINT + float beta; + float alpha; + float gamma; +#else + uint16_t max_div_min; + uint32_t inv_total_time; +#endif +}; + + +/** + * Ramp structure + */ +struct Ramp +{ + uint32_t clocksRamp; + uint16_t clocksMinWL; + uint16_t clocksMaxWL; + + struct RampPrecalc precalc; ///< pre-calculated values for speed +}; + + +/* + * Function prototypes + */ +void ramp_compute( + struct Ramp * ramp, + uint32_t clocksInRamp, + uint16_t clocksInMinWavelength, + uint16_t clocksInMaxWavelength); + + +/** Setup an acceleration ramp for a stepper motor + * + * \param ramp Ramp to fill + * \param length Length of the ramp (milliseconds) + * \param minFreq Minimum operating frequency of the motor (hertz) + * \param maxFreq Maximum operating frequency of the motor (hertz) + * + */ +void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq); + + +/** + * Initialize a new ramp with default values + */ +void ramp_default(struct Ramp *ramp); + + +/** + * Evaluate the ramp at the given point. Given a \a ramp, and the current \a clock since + * the start of the acceleration, compute the next step, that is the interval at which + * send the signal to the motor. + * + * \note The fixed point version does not work when curClock is zero. Anyway, + * the first step is always clocksMaxWL, as stored within the ramp structure. + */ +#if RAMP_USE_FLOATING_POINT + float ramp_evaluate(const struct Ramp* ramp, float curClock); +#else + uint16_t ramp_evaluate(const struct Ramp* ramp, uint32_t curClock); +#endif + + +/** Self test */ +void ramp_test(void); + +#endif /* ALGO_RAMP_H */ + diff --git a/drv/ramp.c b/drv/ramp.c deleted file mode 100644 index 9a2cc92e..00000000 --- a/drv/ramp.c +++ /dev/null @@ -1,206 +0,0 @@ -/*! - * \file - * - * - * \brief Compute, save and load ramps for stepper motors (implementation) - * - * \version $Id$ - * - * \author Simone Zinanni - * \author Bernardo Innocenti - * \author Giovanni Bajo - * \author Daniele Basile - * - * - * 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. - * - */ - -#include "ramp.h" -#include - -#include // memcpy() - -/** - * Multiply \p a and \p b two integer at 32 bit and extract the high 16 bit word. - */ -#define FIX_MULT32(a,b) (((uint64_t)(a)*(uint32_t)(b)) >> 16) - -void ramp_compute(struct Ramp *ramp, uint32_t clocksRamp, uint16_t clocksMinWL, uint16_t clocksMaxWL) -{ - ASSERT(clocksMaxWL >= clocksMinWL); - - // Save values in ramp struct - ramp->clocksRamp = clocksRamp; - ramp->clocksMinWL = clocksMinWL; - ramp->clocksMaxWL = clocksMaxWL; - -#if RAMP_USE_FLOATING_POINT - ramp->precalc.gamma = ramp->clocksMaxWL - ramp->clocksMinWL; - ramp->precalc.beta = (float)ramp->clocksMinWL * (float)ramp->clocksRamp; - ramp->precalc.alpha = ramp->precalc.beta * (float)ramp->clocksMaxWL; - -#else - ramp->precalc.max_div_min = ((uint32_t)clocksMinWL << 16) / (uint32_t)clocksMaxWL; - - /* Calcola 1/total_time in fixed point .32. Assumiamo che la rampa possa al - * massimo avere 25 bit (cioé valore in tick fino a 2^25, che con il - * prescaler=3 sono circa 7 secondi). Inoltre, togliamo qualche bit di precisione - * da destra (secondo quanto specificato in RAMP_CLOCK_SHIFT_PRECISION). - */ - ASSERT(ramp->clocksRamp < (1UL << (24 + RAMP_CLOCK_SHIFT_PRECISION))); - ramp->precalc.inv_total_time = 0xFFFFFFFFUL / (ramp->clocksRamp >> RAMP_CLOCK_SHIFT_PRECISION); - ASSERT(ramp->precalc.inv_total_time < 0x1000000UL); - -#endif -} - - -void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq) -{ - uint32_t minWL, maxWL; - - minWL = TIME2CLOCKS(FREQ2MICROS(maxFreq)); - maxWL = TIME2CLOCKS(FREQ2MICROS(minFreq)); - - ASSERT2(minWL < 65536UL, "Maximum frequency too high"); - ASSERT2(maxWL < 65536UL, "Minimum frequency too high"); - ASSERT(maxFreq > minFreq); - - ramp_compute( - ramp, - TIME2CLOCKS(length), - TIME2CLOCKS(FREQ2MICROS(maxFreq)), - TIME2CLOCKS(FREQ2MICROS(minFreq)) - ); -} - -void ramp_default(struct Ramp *ramp) -{ - ramp_setup(ramp, RAMP_DEF_TIME, RAMP_DEF_MINFREQ, RAMP_DEF_MAXFREQ); -} - -#if RAMP_USE_FLOATING_POINT - -float ramp_evaluate(const struct Ramp* ramp, float curClock) -{ - return ramp->precalc.alpha / (curClock * ramp->precalc.gamma + ramp->precalc.beta); -} - -#else - -INLINE uint32_t fix_mult32(uint32_t m1, uint32_t m2) -{ - uint32_t accum = 0; - accum += m1 * ((m2 >> 0) & 0xFF); - accum >>= 8; - accum += m1 * ((m2 >> 8) & 0xFF); - accum >>= 8; - accum += m1 * ((m2 >> 16) & 0xFF); - return accum; -} - -// a*b >> 16 -INLINE uint16_t fix_mult16(uint16_t a, uint32_t b) -{ - return (b*(uint32_t)a) >> 16; -} - -uint16_t FAST_FUNC ramp_evaluate(const struct Ramp* ramp, uint32_t curClock) -{ - uint16_t t = FIX_MULT32(curClock >> RAMP_CLOCK_SHIFT_PRECISION, ramp->precalc.inv_total_time); - uint16_t denom = fix_mult16((uint16_t)~t + 1, ramp->precalc.max_div_min) + t; - uint16_t cur_delta = ((uint32_t)ramp->clocksMinWL << 16) / denom; - - return cur_delta; -} - -#endif - -