From: asterix Date: Fri, 21 Mar 2008 10:31:59 +0000 (+0000) Subject: git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1211 38d2e660-2303-0410-9eaa... X-Git-Tag: 1.0.0~41 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=4211d6ccfc6d27de53d3087544c268b74d25a1d2;p=bertos.git git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1211 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/algo/ramp_test.c b/algo/ramp_test.c new file mode 100644 index 00000000..92de9405 --- /dev/null +++ b/algo/ramp_test.c @@ -0,0 +1,175 @@ +/*! + * \file + * + * + * \brief Test for 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 + +static bool ramp_test_single(uint32_t minFreq, uint32_t maxFreq, uint32_t length) +{ + struct Ramp r; + uint16_t cur, old; + uint32_t clock; + uint32_t oldclock; + + ramp_setup(&r, length, minFreq, maxFreq); + + cur = old = r.clocksMaxWL; + clock = 0; + oldclock = 0; + + kprintf("testing ramp: (length=%lu, min=%lu, max=%lu)\n", length, minFreq, maxFreq); + kprintf(" [length=%lu, max=%04x, min=%04x]\n", r.clocksRamp, r.clocksMaxWL, r.clocksMinWL); + + int i = 0; + int nonbyte = 0; + + while (clock + cur < r.clocksRamp) + { + oldclock = clock; + old = cur; + + clock += cur; + cur = ramp_evaluate(&r, clock); + + if (old < cur) + { + uint16_t t1 = FIX_MULT32(oldclock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time); + uint16_t t2 = FIX_MULT32(clock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time); + uint16_t denom1 = FIX_MULT32((uint16_t)((~t1) + 1), r.precalc.max_div_min) + t1; + uint16_t denom2 = FIX_MULT32((uint16_t)((~t2) + 1), r.precalc.max_div_min) + t2; + + kprintf(" Failed: %04x @ %lu --> %04x @ %lu\n", old, oldclock, cur, clock); + kprintf(" T: %04x -> %04x\n", t1, t2); + kprintf(" DENOM: %04x -> %04x\n", denom1, denom2); + + cur = ramp_evaluate(&r, clock); + return false; + } + i++; + if ((old-cur) >= 256) + nonbyte++; + } + + + + kprintf("Test finished: %04x @ %lu [min=%04x, totlen=%lu, numsteps:%d, nonbyte:%d]\n", cur, clock, r.clocksMinWL, r.clocksRamp, i, nonbyte); + + return true; +} + + +void ramp_test(void) +{ + bool ok = true; + + ok = ramp_test_single(200, 5000, 3000000) && ok; + ok = ramp_test_single(1000, 2000, 1000000) && ok; + + if (ok) + kputs("Ramp test succeeded!\n"); + else + kputs("Ramp test fail!\n"); +} + + diff --git a/drv/ramp_test.c b/drv/ramp_test.c deleted file mode 100644 index 92de9405..00000000 --- a/drv/ramp_test.c +++ /dev/null @@ -1,175 +0,0 @@ -/*! - * \file - * - * - * \brief Test for 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 - -static bool ramp_test_single(uint32_t minFreq, uint32_t maxFreq, uint32_t length) -{ - struct Ramp r; - uint16_t cur, old; - uint32_t clock; - uint32_t oldclock; - - ramp_setup(&r, length, minFreq, maxFreq); - - cur = old = r.clocksMaxWL; - clock = 0; - oldclock = 0; - - kprintf("testing ramp: (length=%lu, min=%lu, max=%lu)\n", length, minFreq, maxFreq); - kprintf(" [length=%lu, max=%04x, min=%04x]\n", r.clocksRamp, r.clocksMaxWL, r.clocksMinWL); - - int i = 0; - int nonbyte = 0; - - while (clock + cur < r.clocksRamp) - { - oldclock = clock; - old = cur; - - clock += cur; - cur = ramp_evaluate(&r, clock); - - if (old < cur) - { - uint16_t t1 = FIX_MULT32(oldclock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time); - uint16_t t2 = FIX_MULT32(clock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time); - uint16_t denom1 = FIX_MULT32((uint16_t)((~t1) + 1), r.precalc.max_div_min) + t1; - uint16_t denom2 = FIX_MULT32((uint16_t)((~t2) + 1), r.precalc.max_div_min) + t2; - - kprintf(" Failed: %04x @ %lu --> %04x @ %lu\n", old, oldclock, cur, clock); - kprintf(" T: %04x -> %04x\n", t1, t2); - kprintf(" DENOM: %04x -> %04x\n", denom1, denom2); - - cur = ramp_evaluate(&r, clock); - return false; - } - i++; - if ((old-cur) >= 256) - nonbyte++; - } - - - - kprintf("Test finished: %04x @ %lu [min=%04x, totlen=%lu, numsteps:%d, nonbyte:%d]\n", cur, clock, r.clocksMinWL, r.clocksRamp, i, nonbyte); - - return true; -} - - -void ramp_test(void) -{ - bool ok = true; - - ok = ramp_test_single(200, 5000, 3000000) && ok; - ok = ramp_test_single(1000, 2000, 1000000) && ok; - - if (ok) - kputs("Ramp test succeeded!\n"); - else - kputs("Ramp test fail!\n"); -} - -