bertos/algo/ramp_test.c

   1 /*!
   2  * \file
   3  * <!--
   4  * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
   5  * All Rights Reserved.
   6  * -->
   7  *
   8  * \brief Test for compute, save and load ramps for stepper motors (implementation)
   9  *
  10  *
  11  * \author Simone Zinanni <s.zinanni@develer.com>
  12  * \author Bernie Innocenti <bernie@codewiz.org>
  13  * \author Giovanni Bajo <rasky@develer.com>
  14  * \author Daniele Basile <asterix@develer.com>
  15  *
  16  *
  17  * The formula used by the ramp is the following:
  18  *
  19  * <pre>
  20  *            a * b
  21  * f(t) = -------------
  22  *         lerp(a,b,t)
  23  * </pre>
  24  *
  25  * Where <code>a</code> and <code>b</code> are the maximum and minimum speed
  26  * respectively (minimum and maximum wavelength respectively), and <code>lerp</code>
  27  * is a linear interpolation with a factor:
  28  *
  29  * <pre>
  30  * lerp(a,b,t) =  a + t * (b - a)  =  (a * (1 - t)) + (b * t)
  31  * </pre>
  32  *
  33  * <code>t</code> must be in the [0,1] interval. It is easy to see that the
  34  * following holds true:
  35  *
  36  * <pre>
  37  * f(0) = b,   f(1) = a
  38  * </pre>
  39  *
  40  * And that the function is monotonic. So, the function effectively interpolates
  41  * between the maximum and minimum speed through its domain ([0,1] -> [b,a]).
  42  *
  43  * The curve drawn by this function is similar to 1 / (sqrt(n)), so it is slower
  44  * than a linear acceleration (which would be 1/n).
  45  *
  46  * The floating point version uses a slightly modified function which accepts
  47  * the parameter in the domain [0, MT] (where MT is maxTime, the length of the
  48  * ramp, which is a setup parameter for the ramp). This is done to reduce the
  49  * number of operations per step. The formula looks like this:
  50  *
  51  * <pre>
  52  *               a * b * MT
  53  * g(t) = ----------------------------
  54  *           (a * MT) + t * (b - a)
  55  * </pre>
  56  *
  57  * It can be shown that this <code>g(t) = f(t * MT)</code>. The denominator
  58  * is a linear interpolation in the range [b*MT, a*MT], as t moves in the
  59  * interval [0, MT]. So the interpolation interval of the function is again
  60  * [b, a]. The implementation caches the value of the numerator and parts
  61  * of the denominator, so that the formula becomes:
  62  *
  63  * <pre>
  64  * alpha = a * b * MT
  65  * beta = a * MT
  66  * gamma = b - a
  67  *
  68  *                alpha
  69  * g(t) = ----------------------
  70  *           beta + t * gamma
  71  * </pre>
  72  *
  73  * and <code>t</code> is exactly the parameter that ramp_evaluate() gets,
  74  * that is the current time (in range [0, MT]). The operations performed
  75  * for each step are just an addition, a multiplication and a division.
  76  *
  77  * The fixed point version of the formula instead transforms the original
  78  * function as follows:
  79  *
  80  * <pre>
  81  *                   a * b                         a
  82  *  f(t) =  -------------------------  =  --------------------
  83  *                 a                         a
  84  *           b * ( - * (1 - t) + t )         - * (1 - t) + t
  85  *                 b                         b
  86  * </pre>
  87  *
  88  * <code>t</code> must be computed by dividing the current time (24 bit integer)
  89  * by the maximum time (24 bit integer). This is done by precomputing the
  90  * reciprocal of the maximum time as a 0.32 fixed point number, and multiplying
  91  * it to the current time. Multiplication is performed 8-bits a time by
  92  * FIX_MULT32(), so that we end up with a 0.16 fixed point number for
  93  * <code>t</code> (and <code>1-t</code> is just its twos-complement negation).
  94  * <code>a/b</code> is in the range [0,1] (because a is always less than b,
  95  * being the minimum wavelength), so it is precomputed as a 0.16 fixed point.
  96  * The final step is then computing the denominator and executing the division
  97  * (32 cycles using the 1-step division instruction in the DSP).
  98  *
  99  * The assembly implementation is needed for efficiency, but a C version of it
 100  * can be easily written, in case it is needed in the future.
 101  *
 102  */
 103
 104 #include "ramp.h"
 105 #include <cfg/debug.h>
 106 #include <cfg/test.h>
 107
 108
 109 static bool ramp_test_single(uint32_t minFreq, uint32_t maxFreq, uint32_t length)
 110 {
 111         struct Ramp r;
 112         uint16_t cur, old;
 113         uint32_t clock;
 114         uint32_t oldclock;
 115
 116         ramp_setup(&r, length, minFreq, maxFreq);
 117
 118         cur = old = r.clocksMaxWL;
 119         clock = 0;
 120         oldclock = 0;
 121
 122         kprintf("testing ramp: (length=%lu, min=%lu, max=%lu)\n", (unsigned long)length, (unsigned long)minFreq, (unsigned long)maxFreq);
 123         kprintf("              [length=%lu, max=%04x, min=%04x]\n", (unsigned long)r.clocksRamp, r.clocksMaxWL, r.clocksMinWL);
 124
 125         int i = 0;
 126         int nonbyte = 0;
 127
 128         while (clock + cur < r.clocksRamp)
 129         {
 130                 oldclock = clock;
 131                 old = cur;
 132
 133                 clock += cur;
 134                 cur = ramp_evaluate(&r, clock);
 135
 136                 if (old < cur)
 137                 {
 138                         uint16_t t1 = FIX_MULT32(oldclock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time);
 139                         uint16_t t2 = FIX_MULT32(clock >> RAMP_CLOCK_SHIFT_PRECISION,    r.precalc.inv_total_time);
 140                         uint16_t denom1 = FIX_MULT32((uint16_t)((~t1) + 1), r.precalc.max_div_min) + t1;
 141                         uint16_t denom2 = FIX_MULT32((uint16_t)((~t2) + 1), r.precalc.max_div_min) + t2;
 142
 143                         kprintf("    Failed: %04x @ %lu   -->   %04x @ %lu\n", old, (unsigned long)oldclock, cur, (unsigned long)clock);
 144                         kprintf("    T:     %04x -> %04x\n", t1, t2);
 145                         kprintf("    DENOM: %04x -> %04x\n", denom1, denom2);
 146
 147                         cur = ramp_evaluate(&r, clock);
 148                         return false;
 149                 }
 150                 i++;
 151                 if ((old-cur) >= 256)
 152                         nonbyte++;
 153         }
 154
 155
 156
 157         kprintf("Test finished: %04x @ %lu [min=%04x, totlen=%lu, numsteps:%d, nonbyte:%d]\n", cur, (unsigned long)clock, r.clocksMinWL, (unsigned long)r.clocksRamp, i, nonbyte);
 158
 159         return true;
 160 }
 161
 162 int ramp_testSetup(void)
 163 {
 164         kdbg_init();
 165         return 0;
 166 }
 167
 168 int ramp_testTearDown(void)
 169 {
 170         return 0;
 171 }
 172
 173 int ramp_testRun(void)
 174 {
 175         #define TEST_RAMP(min, max, len) do { \
 176                 if (!ramp_test_single(min, max, len)) \
 177                         return -1; \
 178         } while(0)
 179
 180         TEST_RAMP(200,  5000, 3000000);
 181         TEST_RAMP(1000, 2000, 1000000);
 182
 183         return 0;
 184 }
 185
 186 TEST_MAIN(ramp);