test/pid_expert.h

   1 #define MAIN_(x) A_CAST_2(x, _pid_expert)
   2 #include "test.h"
   3 #include "a/tf.h"
   4 #undef A_EXTERN
   5 #define A_EXTERN A_EXPORT extern
   6
   7 /*!
   8  @file pid_expert.h
   9  @brief expert proportional integral derivative controller
  10 */
  11
  12 #ifndef LIBA_PID_EXPERT_H
  13 #define LIBA_PID_EXPERT_H
  14
  15 #include "a/pid.h"
  16
  17 /*!
  18  @ingroup A
  19  @addtogroup A_PID_EXPERT expert proportional integral derivative controller
  20  @{
  21 */
  22
  23 typedef struct a_pid_expert a_pid_expert;
  24
  25 #if defined(__cplusplus)
  26 extern "C" {
  27 #endif /* __cplusplus */
  28
  29 /*!
  30  @brief initialize for expert PID controller
  31  @param[in,out] ctx points to an instance of expert PID controller
  32 */
  33 #define a_pid_expert_init(ctx) a_pid_expert_zero(ctx)
  34
  35 /*!
  36  @brief set proportional integral derivative constant for expert PID controller
  37  @param[in,out] ctx points to an instance of expert PID controller
  38  @param[in] kp proportional constant
  39  @param[in] ki integral constant
  40  @param[in] kd derivative constant
  41 */
  42 A_EXTERN void a_pid_expert_kpid(a_pid_expert *ctx, a_float kp, a_float ki, a_float kd);
  43
  44 /*!
  45  @brief calculate for expert PID controller
  46  @param[in,out] ctx points to an instance of expert PID controller
  47  @param[in] set setpoint value
  48  @param[in] fdb feedback value
  49  @return output value
  50 */
  51 A_EXTERN a_float a_pid_expert_iter(a_pid_expert *ctx, a_float set, a_float fdb);
  52
  53 /*!
  54  @brief zeroing for expert PID controller
  55  @param[in,out] ctx points to an instance of expert PID controller
  56 */
  57 A_EXTERN void a_pid_expert_zero(a_pid_expert *ctx);
  58
  59 #if defined(__cplusplus)
  60 } /* extern "C" */
  61 namespace a
  62 {
  63 struct pid_expert;
  64 } /* namespace a */
  65 #endif /* __cplusplus */
  66
  67 /*!
  68  @brief instance structure for expert PID controller
  69 */
  70 struct a_pid_expert
  71 {
  72     a_pid pid; //!< instance structure for PID controller
  73     a_float ec; //!< error change
  74     a_float outmax; //!< maximum output
  75     a_float epsilon; //!< precision
  76     a_float max1; //!< first error bound
  77     a_float gain; //!< gain coefficient
  78     a_float max2; //!< second error bound
  79     a_float loss; //!< loss coefficient
  80 #if defined(__cplusplus)
  81     A_INLINE void init() { a_pid_expert_init(this); }
  82     A_INLINE void kpid(a_float kp, a_float ki, a_float kd)
  83     {
  84         a_pid_expert_kpid(this, kp, ki, kd);
  85     }
  86     A_INLINE a_float operator()(a_float set, a_float fdb)
  87     {
  88         return a_pid_expert_iter(this, set, fdb);
  89     }
  90     A_INLINE void zero() { a_pid_expert_zero(this); }
  91 #endif /* __cplusplus */
  92 };
  93
  94 /*! @} A_PID_EXPERT */
  95
  96 #endif /* a/pid_expert.h */
  97
  98 // #include "a/math.h"
  99
 100 static A_INLINE a_float input(a_float const x)
 101 {
 102 #if defined(LIBA_MATH_H)
 103     return a_float_sin(4 * A_FLOAT_PI * x);
 104 #else
 105     return (void)x, 1;
 106 #endif
 107 }
 108
 109 int MAIN(int argc, char *argv[]) // NOLINT(misc-definitions-in-headers)
 110 {
 111     main_init(argc, argv, 1);
 112
 113     a_float num[] = {A_FLOAT_C(6.59492796e-05), A_FLOAT_C(6.54019884e-05)};
 114     a_float den[] = {A_FLOAT_C(-1.97530991), A_FLOAT_C(0.97530991)};
 115
 116     a_tf tf;
 117     a_float tf_input[A_LEN(num)];
 118     a_float tf_output[A_LEN(den)];
 119     a_tf_init(&tf, A_LEN(num), num, tf_input, A_LEN(den), den, tf_output);
 120
 121     a_pid_expert ctx;
 122     ctx.pid.kp = A_FLOAT_C(10.0);
 123     ctx.pid.ki = A_FLOAT_C(0.5);
 124     ctx.pid.kd = A_FLOAT_C(10.0);
 125     ctx.pid.outmax = +A_FLOAT_MAX;
 126     ctx.pid.outmin = -A_FLOAT_MAX;
 127     ctx.max1 = A_FLOAT_C(0.4);
 128     ctx.gain = A_FLOAT_C(2.0);
 129     ctx.max2 = A_FLOAT_C(0.1);
 130     ctx.loss = A_FLOAT_C(0.5);
 131     ctx.outmax = A_FLOAT_C(10.0);
 132     ctx.epsilon = A_FLOAT_C(0.01);
 133     a_pid_expert_init(&ctx);
 134     for (unsigned int i = 0; i < 500; ++i)
 135     {
 136         a_float in = input(A_FLOAT_C(0.001) * a_float_c(i));
 137         a_tf_iter(&tf, a_pid_expert_iter(&ctx, in, *tf.output));
 138         debug(A_FLOAT_PRI("+", "f,") A_FLOAT_PRI("+", "f,") A_FLOAT_PRI("+", "f,") A_FLOAT_PRI("+", "f\n"),
 139               A_FLOAT_C(0.001) * a_float_c(i), in, *tf.output, ctx.pid.err);
 140     }
 141     a_pid_expert_zero(&ctx);
 142     a_tf_zero(&tf);
 143
 144 #if defined(__cplusplus) && (__cplusplus > 201100L)
 145     A_ASSERT_BUILD(std::is_pod<a_pid_expert>::value);
 146 #endif /* __cplusplus */
 147
 148     return 0;
 149 }