mirror of
https://gitee.com/Lamdonn/varch.git
synced 2025-12-06 16:56:42 +08:00
56 lines
1.7 KiB
C
56 lines
1.7 KiB
C
/*********************************************************************************************************
|
|
* ------------------------------------------------------------------------------------------------------
|
|
* file description
|
|
* ------------------------------------------------------------------------------------------------------
|
|
* \file pid.c
|
|
* \unit pid
|
|
* \brief This is a general-purpose C language pid module
|
|
* \author Lamdonn
|
|
* \version v1.0.0
|
|
* \license GPL-2.0
|
|
* \copyright Copyright (C) 2023 Lamdonn.
|
|
********************************************************************************************************/
|
|
#include "pid.h"
|
|
|
|
|
|
void pid_init(PIDC *pid)
|
|
{
|
|
/* Initialize pid structure members */
|
|
pid->point = 0.0;
|
|
pid->process = 0.0;
|
|
pid->kp = 0.0;
|
|
pid->ki = 0.0;
|
|
pid->kd = 0.0;
|
|
pid->integral = 0.0;
|
|
pid->error = 0.0;
|
|
pid->output = 0.0;
|
|
}
|
|
|
|
double pid_compute(PIDC *pid)
|
|
{
|
|
double error; /* Record current error */
|
|
double delta; /* Record delta error */
|
|
double derivative; /* Record derivative item */
|
|
|
|
/* Calculate current error */
|
|
error = pid->point - pid->process;
|
|
|
|
/* Calculate the change in error */
|
|
delta = error - pid->error;
|
|
|
|
/* Accumulation of integral terms */
|
|
pid->integral += error * pid->ki;
|
|
|
|
/* Calculate differential terms */
|
|
derivative = delta * pid->kd;
|
|
|
|
/* Calculate PID output */
|
|
pid->output = pid->kp * error + pid->integral + derivative;
|
|
|
|
/* Update previous error */
|
|
pid->error = error;
|
|
|
|
return pid->output; /* Return controller output */
|
|
}
|
|
|