test/pid.h

   1 #define MAIN(x) pid##x
   2 #include "test.h"
   3 #include "a/tf.h"
   4 #include "a/pid.h"
   5
   6 static A_INLINE a_float input(a_float const x)
   7 {
   8 #if defined(LIBA_MATH_H)
   9     return a_float_sin(4 * A_FLOAT_PI * x);
  10 #else
  11     return (void)x, 1;
  12 #endif
  13 }
  14
  15 int main(int argc, char *argv[]) /* NOLINT(misc-definitions-in-headers) */
  16 {
  17     unsigned int i;
  18     a_float num[] = {A_FLOAT_C(6.59492796e-05), A_FLOAT_C(6.54019884e-05)};
  19     a_float den[] = {A_FLOAT_C(-1.97530991), A_FLOAT_C(0.97530991)};
  20
  21     a_tf pos_tf;
  22     a_float pos_input[A_LEN(num)];
  23     a_float pos_output[A_LEN(den)];
  24     a_pid pos_pid;
  25
  26     a_tf inc_tf;
  27     a_float inc_input[A_LEN(num)];
  28     a_float inc_output[A_LEN(den)];
  29     a_pid inc_pid;
  30
  31     main_init(argc, argv, 1);
  32     a_tf_init(&pos_tf, A_LEN(num), num, pos_input, A_LEN(den), den, pos_output);
  33     a_tf_init(&inc_tf, A_LEN(num), num, inc_input, A_LEN(den), den, inc_output);
  34
  35     pos_pid.kp = A_FLOAT_C(100.0);
  36     pos_pid.ki = A_FLOAT_C(0.01);
  37     pos_pid.kd = A_FLOAT_C(1200.0);
  38     pos_pid.summax = +A_FLOAT_MAX;
  39     pos_pid.summin = -A_FLOAT_MAX;
  40     pos_pid.outmax = +A_FLOAT_MAX;
  41     pos_pid.outmin = -A_FLOAT_MAX;
  42     a_pid_init(&pos_pid);
  43
  44     inc_pid.kp = A_FLOAT_C(100.0);
  45     inc_pid.ki = A_FLOAT_C(0.01);
  46     inc_pid.kd = A_FLOAT_C(1200.0);
  47     inc_pid.outmax = +A_FLOAT_MAX;
  48     inc_pid.outmin = -A_FLOAT_MAX;
  49     a_pid_init(&inc_pid);
  50
  51     for (i = 0; i < 100; ++i)
  52     {
  53         a_float const ix = input(A_FLOAT_C(0.001) * a_float_c(i));
  54         a_tf_iter(&pos_tf, a_pid_pos(&pos_pid, ix, *pos_tf.output));
  55         a_tf_iter(&inc_tf, a_pid_inc(&inc_pid, ix, *inc_tf.output));
  56         debug("%+" A_FLOAT_PRI "f,%+" A_FLOAT_PRI "f,%+" A_FLOAT_PRI "f,%+" A_FLOAT_PRI "f\n",
  57               A_FLOAT_C(0.001) * a_float_c(i), ix, *pos_tf.output, *inc_tf.output);
  58     }
  59
  60 #if defined(__cplusplus) && (__cplusplus > 201100L)
  61     A_BUILD_ASSERT(std::is_pod<a_pid>::value);
  62 #endif /* __cplusplus */
  63
  64     return 0;
  65 }