src/pid.c

   1 #include "a/pid.h"
   2
   3 void a_pid_kpid(a_pid *ctx, a_float kp, a_float ki, a_float kd)
   4 {
   5     ctx->kp = kp;
   6     ctx->ki = ki;
   7     ctx->kd = kd;
   8 }
   9
  10 A_HIDDEN a_float a_pid_run_(a_pid *ctx, a_float set, a_float fdb, a_float err);
  11 a_float a_pid_run(a_pid *ctx, a_float set, a_float fdb)
  12 {
  13     return a_pid_run_(ctx, set, fdb, set - fdb);
  14 }
  15 a_float a_pid_run_(a_pid *ctx, a_float set, a_float fdb, a_float err)
  16 {
  17     a_float const var = ctx->fdb - fdb;
  18     ctx->out = A_SAT(set, ctx->outmin, ctx->outmax);
  19     ctx->var = var;
  20     ctx->fdb = fdb;
  21     ctx->err = err;
  22     return ctx->out;
  23 }
  24
  25 A_HIDDEN a_float a_pid_pos_(a_pid *ctx, a_float fdb, a_float err);
  26 a_float a_pid_pos(a_pid *ctx, a_float set, a_float fdb)
  27 {
  28     return a_pid_pos_(ctx, fdb, set - fdb);
  29 }
  30 a_float a_pid_pos_(a_pid *ctx, a_float fdb, a_float err)
  31 {
  32     a_float const var = ctx->fdb - fdb;
  33     // When the limit of integration is exceeded or the direction of integration is the same, the integration stops.
  34     if ((ctx->sum > ctx->summin && ctx->sum < ctx->summax) || ctx->sum * err < 0)
  35     {
  36 #if 1
  37         ctx->sum += ctx->ki * err; // sum = K_i[\sum^k_{i=0}err(i)]
  38 #else
  39         ctx->sum += ctx->ki * (err + ctx->err) * A_FLOAT_C(0.5);
  40 #endif
  41     }
  42     // Avoid derivative kick, fdb[k-1]-fdb[k]. out = K_p[err(k)]+sum+K_d[fdb(k-1)-fdb(k)].
  43     ctx->out = ctx->kp * err + ctx->sum + ctx->kd * var;
  44     ctx->out = A_SAT(ctx->out, ctx->outmin, ctx->outmax);
  45     ctx->var = var;
  46     ctx->fdb = fdb;
  47     ctx->err = err;
  48     return ctx->out;
  49 }
  50
  51 A_HIDDEN a_float a_pid_inc_(a_pid *ctx, a_float fdb, a_float err);
  52 a_float a_pid_inc(a_pid *ctx, a_float set, a_float fdb)
  53 {
  54     return a_pid_inc_(ctx, fdb, set - fdb);
  55 }
  56 a_float a_pid_inc_(a_pid *ctx, a_float fdb, a_float err)
  57 {
  58     a_float const var = ctx->fdb - fdb;
  59     ctx->out += ctx->kp * (err - ctx->err) + ctx->ki * err + ctx->kd * (var - ctx->var);
  60     ctx->out = A_SAT(ctx->out, ctx->outmin, ctx->outmax);
  61     ctx->var = var;
  62     ctx->fdb = fdb;
  63     ctx->err = err;
  64     return ctx->out;
  65 }
  66
  67 void a_pid_zero(a_pid *ctx)
  68 {
  69     ctx->sum = 0;
  70     ctx->out = 0;
  71     ctx->var = 0;
  72     ctx->fdb = 0;
  73     ctx->err = 0;
  74 }