test/pid_fuzzy.h

   1 #define MAIN_(x) A_CAST_2(x, _pid_fuzzy)
   2 #include "test.h"
   3 #include "a/tf.h"
   4 #include "a/mf.h"
   5 // #include "a/math.h"
   6 #include "a/pid_fuzzy.h"
   7
   8 static A_INLINE a_float input(a_float const x)
   9 {
  10 #if defined(LIBA_MATH_H)
  11     return a_float_sin(4 * A_FLOAT_PI * x);
  12 #else
  13     return (void)x, 1;
  14 #endif
  15 }
  16
  17 #define N (-1 * X)
  18 #define Z (+0 * X)
  19 #define P (+1 * X)
  20 #undef X
  21 #define X 1
  22 static a_float const m3e[] = {
  23     A_MF_TRI, N, N, Z,
  24     A_MF_TRI, N, Z, P,
  25     A_MF_TRI, Z, P, P};
  26 #undef X
  27 #define X 2
  28 static a_float const m3ec[] = {
  29     A_MF_TRI, N, N, Z,
  30     A_MF_TRI, N, Z, P,
  31     A_MF_TRI, Z, P, P};
  32 #undef X
  33 #define X A_FLOAT_C(5.0)
  34 static a_float const m3kp[] = {
  35     N, N, N,
  36     N, P, P,
  37     P, P, P};
  38 #undef X
  39 #define X A_FLOAT_C(0.01)
  40 static a_float const m3ki[] = {
  41     Z, Z, Z,
  42     P, P, P,
  43     Z, Z, Z};
  44 #undef X
  45 #define X A_FLOAT_C(0.1)
  46 static a_float const m3kd[] = {
  47     N, N, Z,
  48     Z, Z, Z,
  49     Z, P, P};
  50 #undef X
  51
  52 #undef NL
  53 #undef NM
  54 #undef NS
  55 #undef ZO
  56 #undef PS
  57 #undef PM
  58 #undef PL
  59 #define NL (-3 * X)
  60 #define NM (-2 * X)
  61 #define NS (-1 * X)
  62 #define ZO (+0 * X)
  63 #define PS (+1 * X)
  64 #define PM (+2 * X)
  65 #define PL (+3 * X)
  66 #undef X
  67 #define X A_FLOAT_C(1.0) / 2
  68 static a_float const m7e[] = {
  69     A_MF_TRI, NL, NL, NM,
  70     A_MF_TRI, NL, NM, NS,
  71     A_MF_TRI, NM, NS, ZO,
  72     A_MF_TRI, NS, ZO, PS,
  73     A_MF_TRI, ZO, PS, PM,
  74     A_MF_TRI, PS, PM, PL,
  75     A_MF_TRI, PM, PL, PL};
  76 #undef X
  77 #define X A_FLOAT_C(1.0) / 2
  78 static a_float const m7ec[] = {
  79     A_MF_TRI, NL, NL, NM,
  80     A_MF_TRI, NL, NM, NS,
  81     A_MF_TRI, NM, NS, ZO,
  82     A_MF_TRI, NS, ZO, PS,
  83     A_MF_TRI, ZO, PS, PM,
  84     A_MF_TRI, PS, PM, PL,
  85     A_MF_TRI, PM, PL, PL};
  86 #undef X
  87 #define X A_FLOAT_C(500.0) / 6
  88 static a_float const m7kp[] = {
  89     NL, NL, NM, NM, NS, ZO, ZO,
  90     NL, NL, NM, NS, NS, ZO, PS,
  91     NM, NM, NM, NS, ZO, PS, PS,
  92     NM, NM, NS, ZO, PS, PM, PM,
  93     NS, NS, ZO, PS, PS, PM, PM,
  94     NS, ZO, PS, PM, PM, PM, PL,
  95     ZO, ZO, PM, PM, PM, PL, PL};
  96 #undef X
  97 #define X A_FLOAT_C(10.0) / 3
  98 static a_float const m7ki[] = {
  99     PL, PL, PM, PM, PS, ZO, ZO,
 100     PL, PL, PM, PS, PS, ZO, ZO,
 101     PL, PM, PS, PS, ZO, NS, NS,
 102     PM, PM, PS, ZO, NS, NM, NM,
 103     PM, PS, ZO, NS, NS, NM, NL,
 104     ZO, ZO, NS, NS, NM, NL, NL,
 105     ZO, ZO, NS, NM, NM, NL, NL};
 106 #undef X
 107 #define X A_FLOAT_C(5000.0) / 3
 108 static a_float const m7kd[] = {
 109     NS, PS, PL, PL, PL, PM, NS,
 110     NS, PS, PL, PM, PM, PS, ZO,
 111     ZO, PS, PM, PM, PS, PS, ZO,
 112     ZO, PS, PS, PS, PS, PS, ZO,
 113     ZO, ZO, ZO, ZO, ZO, ZO, ZO,
 114     NL, NS, NS, NS, NS, NS, NL,
 115     NL, NM, NM, NM, NS, NS, NL};
 116 #undef X
 117
 118 int MAIN(int argc, char *argv[]) // NOLINT(misc-definitions-in-headers)
 119 {
 120     main_init(argc, argv, 1);
 121
 122     a_float num[] = {A_FLOAT_C(6.59492796e-05), A_FLOAT_C(6.54019884e-05)};
 123     a_float den[] = {A_FLOAT_C(-1.97530991), A_FLOAT_C(0.97530991)};
 124
 125     a_tf pos_tf;
 126     a_float pos_input[A_LEN(num)];
 127     a_float pos_output[A_LEN(den)];
 128     a_tf_init(&pos_tf, A_LEN(num), num, pos_input, A_LEN(den), den, pos_output);
 129     a_tf inc_tf;
 130     a_float inc_input[A_LEN(num)];
 131     a_float inc_output[A_LEN(den)];
 132     a_tf_init(&inc_tf, A_LEN(num), num, inc_input, A_LEN(den), den, inc_output);
 133
 134     a_pid_fuzzy pos_pid;
 135     pos_pid.pid.summax = +10;
 136     pos_pid.pid.summin = -10;
 137     pos_pid.pid.outmax = +10;
 138     pos_pid.pid.outmin = -10;
 139     pos_pid.order = 3;
 140     pos_pid.me = m3e;
 141     pos_pid.mec = m3ec;
 142     pos_pid.mkp = m3kp;
 143     pos_pid.mki = m3ki;
 144     pos_pid.mkd = m3kd;
 145     pos_pid.order = 7;
 146     pos_pid.me = m7e;
 147     pos_pid.mec = m7ec;
 148     pos_pid.mkp = m7kp;
 149     pos_pid.mki = m7ki;
 150     pos_pid.mkd = m7kd;
 151     pos_pid.op = a_pid_fuzzy_op(A_PID_FUZZY_EQU);
 152     a_pid_fuzzy_init(&pos_pid);
 153     a_byte pos_block[A_PID_FUZZY_BLOCK(2)];
 154     a_pid_fuzzy_set_block(&pos_pid, pos_block, 2);
 155     a_pid_fuzzy inc_pid;
 156     inc_pid.pid.outmax = +10;
 157     inc_pid.pid.outmin = -10;
 158     inc_pid.order = 3;
 159     inc_pid.me = m3e;
 160     inc_pid.mec = m3ec;
 161     inc_pid.mkp = m3kp;
 162     inc_pid.mki = m3ki;
 163     inc_pid.mkd = m3kd;
 164     inc_pid.order = 7;
 165     inc_pid.me = m7e;
 166     inc_pid.mec = m7ec;
 167     inc_pid.mkp = m7kp;
 168     inc_pid.mki = m7ki;
 169     inc_pid.mkd = m7kd;
 170     inc_pid.op = a_pid_fuzzy_op(A_PID_FUZZY_EQU);
 171     a_pid_fuzzy_init(&inc_pid);
 172     a_byte inc_block[A_PID_FUZZY_BLOCK(2)];
 173     a_pid_fuzzy_set_block(&inc_pid, inc_block, 2);
 174
 175     a_pid_fuzzy_kpid(&pos_pid, 600, A_FLOAT_C(20.0), A_FLOAT_C(6000.0));
 176     a_pid_fuzzy_kpid(&inc_pid, 600, A_FLOAT_C(20.0), A_FLOAT_C(6000.0));
 177     for (unsigned int i = 0; i < 200; ++i)
 178     {
 179         a_float in = input(A_FLOAT_C(0.001) * a_float_c(i));
 180         a_tf_iter(&pos_tf, a_pid_fuzzy_pos(&pos_pid, in, *pos_tf.output));
 181         a_tf_iter(&inc_tf, a_pid_fuzzy_inc(&inc_pid, in, *inc_tf.output));
 182         debug(A_FLOAT_PRI("+", "f,") A_FLOAT_PRI("+", "f,") A_FLOAT_PRI("+", "f,") A_FLOAT_PRI("+", "f\n"),
 183               A_FLOAT_C(0.001) * a_float_c(i), in, *pos_tf.output, *inc_tf.output);
 184     }
 185     a_pid_fuzzy_zero(&pos_pid);
 186     a_pid_fuzzy_zero(&inc_pid);
 187     a_tf_zero(&pos_tf);
 188     a_tf_zero(&inc_tf);
 189
 190 #if defined(__cplusplus) && (__cplusplus > 201100L)
 191     A_ASSERT_BUILD(std::is_pod<a_pid_fuzzy>::value);
 192 #endif /* __cplusplus */
 193
 194     return 0;
 195 }