create functions a_float_sum{,sq,abs}
[liba.git] / src / pid_neuro.c
blobbcff1fcf6dfd9c9444a97413476d5d7e078e20a2
1 #include "a/pid_neuro.h"
3 void a_pid_neuro_set_kpid(a_pid_neuro *ctx, a_float k, a_float kp, a_float ki, a_float kd)
5 a_pid_set_kpid(&ctx->pid, kp, ki, kd);
6 ctx->k = k;
9 void a_pid_neuro_set_wpid(a_pid_neuro *ctx, a_float wp, a_float wi, a_float wd)
11 ctx->wp = wp;
12 ctx->wi = wi;
13 ctx->wd = wd;
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)
19 a_float const err = set - fdb;
20 return a_pid_neuro_run_(ctx, set, fdb, err, err - ctx->pid.err);
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)
25 a_pid_run_(&ctx->pid, set, fdb, err);
26 ctx->ec = ec;
27 return ctx->pid.out;
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)
33 a_float const err = set - fdb;
34 return a_pid_neuro_inc_(ctx, fdb, err, err - ctx->pid.err);
36 a_float a_pid_neuro_inc_(a_pid_neuro *ctx, a_float fdb, a_float err, a_float ec)
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;
53 void a_pid_neuro_zero(a_pid_neuro *ctx)
55 a_pid_zero(&ctx->pid);
56 ctx->ec = 0;