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
);
9 void a_pid_neuro_set_wpid(a_pid_neuro
*ctx
, a_float wp
, a_float wi
, a_float 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
);
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
);
53 void a_pid_neuro_zero(a_pid_neuro
*ctx
)
55 a_pid_zero(&ctx
->pid
);