pid 算法代码

哈哈啊哈h

已于 2023-12-08 15:54:07 修改

阅读量510

点赞数 7

文章标签： java 算法 jvm

于 2023-12-08 12:14:29 首次发布

本文链接：https://blog.csdn.net/weixin_51593470/article/details/134875288

版权

// pid.c
#include "pid.h"
#include "main.h"




// pid结构体的初始化
void PIDStructInit(_PID *pid,float target,float p,float i,float d)
{
    pid->p = p;
    pid->i = i;
    pid->d = d;
    pid->target = target;
    pid->err_curr = 0;
    pid->err_last = 0;
    pid->err_last_last = 0;
    pid->err_sum = 0;
    pid->Out  = 0;
}

// pid 位置式计算
float PID_Postion(_PID *pid,float current)
{
    volatile float Pout;   // 计算的输出值
    pid->err_curr = pid->target - current;   // 计算当前误差
    pid->err_sum += pid->err_curr;   //积分
    Pout = (pid->p * pid->err_curr) + (pid->i * pid->err_sum) + (pid->d * (pid->err_curr - pid->err_last));   // 计算pid的值
    printf("P = %.3f   I = %.3f   D = %.3f\r\n",(pid->p * pid->err_curr) , (pid->i * pid->err_sum) , (pid->d * (pid->err_curr - pid->err_last)));
    pid->err_last = pid->err_curr;   // 记录上一次的误差为当前这一次
    
    return Pout;
}


// 增量式PID 计算
float PID_Incremental(_PID *pid,float current)
{
    volatile float Pout;   // 计算的输出值
    pid->err_curr = pid->target - current;   // 计算当前误差
    Pout = (pid->p * (pid->err_curr - pid->err_last)) + (pid->i * pid->err_last) + (pid->d * ((pid->err_curr - 2* pid->err_last + pid->err_last_last)));   // 计算pid的值
    printf("P = %.3f   I = %.3f   D = %.3f\r\n",(pid->p * (pid->err_curr - pid->err_last)) , (pid->i * pid->err_last) , (pid->d * ((pid->err_curr - 2* pid->err_last + pid->err_last_last))));
    pid->err_last_last = pid->err_last;   // 上上次更新为上次
    pid->err_last = pid->err_curr;     // 将上次的值更新为这次
    pid->Out += Pout;
    return pid->Out;
}

// pid.h

#ifndef _PID_H
#define _PID_H

typedef struct _PID
{
    volatile float p;
    volatile float i;
    volatile float d;
    float err_curr;   // 当前误差
    float err_last;  // 上次误差
    float err_sum;   // 误差和  积分
    float err_last_last;  // 上上次的误差
    float target;    // 目标值
    float Out;  // 增量 输出的值
}_PID;

void PIDStructInit(_PID *pid,float target,float p,float i,float d);  // 结构体初始化

float PID_Postion(_PID *pid,float current);   // 计算位置pid的值


float PID_Incremental(_PID *pid,float current);    // 增量式PID 计算
#endif