参考 :
https://blog.csdn.net/yin_bu_feng/article/details/85871595
大林控制算法:
#ifndef _DALIN_CONTROLLER_H_
#define _DALIN_CONTROLLER_H_
#ifdef __cplusplus
extern "C" {
#endif
#ifndef __API__
#define __API__
#endif
#ifndef __STATIC_INLINE__
#define __STATIC_INLINE__ static inline
#endif
struct DahlinController {
float last_error;
float u;
float last_u;
float previous_u;
float a0;
float a1;
float b0;
float b1;
};
__API__ __STATIC_INLINE__ int DahlinController_Init(struct DahlinController *dc)
{
if (!dc) return -1;
dc->last_error = 0;
dc->u = 0;
dc->last_u = 0;
dc->previous_u = 0;
dc->a0 = 0;
dc->a1 = 0;
dc->b0 = 0;
dc->b1 = 0;
return 0;
}
__API__ __STATIC_INLINE__ int DahlinController_a0_Set(struct DahlinController *dc, float a0)
{
if (!dc) return -1;
dc->a0 = a0;
return 0;
}
__API__ __STATIC_INLINE__ int DahlinController_a1_Set(struct DahlinController *dc, float a1)
{
if (!dc) return -1;
dc->a1 = a1;
return 0;
}
__API__ __STATIC_INLINE__ int DahlinController_b0_Set(struct DahlinController *dc, float b0)
{
if (!dc) return -1;
dc->b0 = b0;
return 0;
}
__API__ __STATIC_INLINE__ int DahlinController_b1_Set(struct DahlinController *dc, float b1)
{
if (!dc) return -1;
dc->b1 = b1;
return 0;
}
__API__ __STATIC_INLINE__ float DahlinControl(struct DahlinController *dc, float target, float actual)
{
int result;
float error;
error = target - actual;
dc->u = dc->b0*dc->last_u + dc->b1*dc->previous_u + dc->a0*error + dc->a1*dc->last_error; /* Dahlin equation */
dc->last_error = error;
dc->previous_u = dc->last_u;
dc->last_u = dc->u;
return dc->u;
}
#ifdef __cplusplus
}
#endif
#endif /* _DALIN_CONTROLLER_H_ */