--- /dev/null
+/*!
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Compute, save and load ramps for stepper motors (implementation)
+ *
+ * \version $Id$
+ *
+ * \author Simone Zinanni <s.zinanni@develer.com>
+ * \author Bernardo Innocenti <bernie@develer.com>
+ * \author Giovanni Bajo <rasky@develer.com>
+ * \author Daniele Basile <asterix@develer.com>
+ *
+ *
+ * The formula used by the ramp is the following:
+ *
+ * <pre>
+ * a * b
+ * f(t) = -------------
+ * lerp(a,b,t)
+ * </pre>
+ *
+ * Where <code>a</code> and <code>b</code> are the maximum and minimum speed
+ * respectively (minimum and maximum wavelength respectively), and <code>lerp</code>
+ * is a linear interpolation with a factor:
+ *
+ * <pre>
+ * lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t)
+ * </pre>
+ *
+ * <code>t</code> must be in the [0,1] interval. It is easy to see that the
+ * following holds true:
+ *
+ * <pre>
+ * f(0) = b, f(1) = a
+ * </pre>
+ *
+ * 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:
+ *
+ * <pre>
+ * a * b * MT
+ * g(t) = ----------------------------
+ * (a * MT) + t * (b - a)
+ * </pre>
+ *
+ * It can be shown that this <code>g(t) = f(t * MT)</code>. 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:
+ *
+ * <pre>
+ * alpha = a * b * MT
+ * beta = a * MT
+ * gamma = b - a
+ *
+ * alpha
+ * g(t) = ----------------------
+ * beta + t * gamma
+ * </pre>
+ *
+ * and <code>t</code> 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:
+ *
+ * <pre>
+ * a * b a
+ * f(t) = ------------------------- = --------------------
+ * a a
+ * b * ( - * (1 - t) + t ) - * (1 - t) + t
+ * b b
+ * </pre>
+ *
+ * <code>t</code> 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
+ * <code>t</code> (and <code>1-t</code> is just its twos-complement negation).
+ * <code>a/b</code> 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 <cfg/debug.h>
+
+#include <string.h> // 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
+
+
--- /dev/null
+
+/**
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Compute, save and load ramps for stepper motors (interace)
+ *
+ * \version $Id$
+ *
+ * \author Simone Zinanni <s.zinanni@develer.com>
+ * \author Giovanni Bajo <rasky@develer.com>
+ * \author Daniele Basile <asterix@develer.com>
+ *
+ * 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:
+ *
+ * <pre>
+ * float time = 0;
+ * while (1)
+ * {
+ * float delta = ramp_evaluate(&my_ramp, time);
+ * sleep(delta);
+ * do_motor_step();
+ * time += delta;
+ * }
+ * </pre>
+ *
+ * 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 <cfg/compiler.h>
+#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 */
+
+++ /dev/null
-/*!
- * \file
- * <!--
- * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
- * All Rights Reserved.
- * -->
- *
- * \brief Compute, save and load ramps for stepper motors (implementation)
- *
- * \version $Id$
- *
- * \author Simone Zinanni <s.zinanni@develer.com>
- * \author Bernardo Innocenti <bernie@develer.com>
- * \author Giovanni Bajo <rasky@develer.com>
- * \author Daniele Basile <asterix@develer.com>
- *
- *
- * The formula used by the ramp is the following:
- *
- * <pre>
- * a * b
- * f(t) = -------------
- * lerp(a,b,t)
- * </pre>
- *
- * Where <code>a</code> and <code>b</code> are the maximum and minimum speed
- * respectively (minimum and maximum wavelength respectively), and <code>lerp</code>
- * is a linear interpolation with a factor:
- *
- * <pre>
- * lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t)
- * </pre>
- *
- * <code>t</code> must be in the [0,1] interval. It is easy to see that the
- * following holds true:
- *
- * <pre>
- * f(0) = b, f(1) = a
- * </pre>
- *
- * 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:
- *
- * <pre>
- * a * b * MT
- * g(t) = ----------------------------
- * (a * MT) + t * (b - a)
- * </pre>
- *
- * It can be shown that this <code>g(t) = f(t * MT)</code>. 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:
- *
- * <pre>
- * alpha = a * b * MT
- * beta = a * MT
- * gamma = b - a
- *
- * alpha
- * g(t) = ----------------------
- * beta + t * gamma
- * </pre>
- *
- * and <code>t</code> 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:
- *
- * <pre>
- * a * b a
- * f(t) = ------------------------- = --------------------
- * a a
- * b * ( - * (1 - t) + t ) - * (1 - t) + t
- * b b
- * </pre>
- *
- * <code>t</code> 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
- * <code>t</code> (and <code>1-t</code> is just its twos-complement negation).
- * <code>a/b</code> 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 <cfg/debug.h>
-
-#include <string.h> // 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
-
-
projects / bertos.git / commitdiff