test/pid.h

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