src/pid_neuro.c

   1 #include "a/pid_neuro.h"
   2
   3 void a_pid_neuro_kpid(a_pid_neuro *ctx, a_float k, a_float kp, a_float ki, a_float kd)
   4 {
   5     a_pid_kpid(&ctx->pid, kp, ki, kd);
   6     ctx->k = k;
   7 }
   8
   9 void a_pid_neuro_wpid(a_pid_neuro *ctx, a_float wp, a_float wi, a_float wd)
  10 {
  11     ctx->wp = wp;
  12     ctx->wi = wi;
  13     ctx->wd = wd;
  14 }
  15
  16 A_HIDDEN a_float a_pid_neuro_run_(a_pid_neuro *ctx, a_float set, a_float fdb, a_float err, a_float ec);
  17 a_float a_pid_neuro_run(a_pid_neuro *ctx, a_float set, a_float fdb)
  18 {
  19     a_float const err = set - fdb;
  20     return a_pid_neuro_run_(ctx, set, fdb, err, err - ctx->pid.err);
  21 }
  22 A_HIDDEN a_float a_pid_run_(a_pid *ctx, a_float set, a_float fdb, a_float err);
  23 a_float a_pid_neuro_run_(a_pid_neuro *ctx, a_float set, a_float fdb, a_float err, a_float ec)
  24 {
  25     a_pid_run_(&ctx->pid, set, fdb, err);
  26     ctx->ec = ec;
  27     return ctx->pid.out;
  28 }
  29
  30 A_HIDDEN a_float a_pid_neuro_inc_(a_pid_neuro *ctx, a_float fdb, a_float err, a_float ec);
  31 a_float a_pid_neuro_inc(a_pid_neuro *ctx, a_float set, a_float fdb)
  32 {
  33     a_float const err = set - fdb;
  34     return a_pid_neuro_inc_(ctx, fdb, err, err - ctx->pid.err);
  35 }
  36 a_float a_pid_neuro_inc_(a_pid_neuro *ctx, a_float fdb, a_float err, a_float ec)
  37 {
  38     a_float const var = ec - ctx->ec;
  39     a_float out = err * ctx->pid.out;
  40     ctx->wp += ctx->pid.kp * out * ctx->ec;
  41     ctx->wi += ctx->pid.ki * out * ctx->pid.err;
  42     ctx->wd += ctx->pid.kd * out * ctx->pid.var;
  43     out = A_ABS(ctx->wp) + A_ABS(ctx->wi) + A_ABS(ctx->wd);
  44     out = ctx->k * (ctx->wp * ec + ctx->wi * err + ctx->wd * var) / out;
  45     ctx->pid.out = A_SAT(out, ctx->pid.outmin, ctx->pid.outmax);
  46     ctx->pid.var = var;
  47     ctx->pid.fdb = fdb;
  48     ctx->pid.err = err;
  49     ctx->ec = ec;
  50     return ctx->pid.out;
  51 }
  52
  53 void a_pid_neuro_zero(a_pid_neuro *ctx)
  54 {
  55     a_pid_zero(&ctx->pid);
  56     ctx->ec = 0;
  57 }