try

C_PID/Doge-ai/follow/main.c

@@ -0,0 +1,62 @@
+//这是找了代码学习了下，稍微调了一下和做了些注释，因为自己试着做的好像很不对劲（
+
+#include <stdio.h>
+#include "pid.h"
+#include "pid.c"
+#include <stdlib.h>
+
+int main() 
+{
+    PID pid1, pid2, pid3;
+    float kp, ki, kd, pos_output, inc_output, ff_output, ff_mid_output;
+
+    // 初始化PID控制器
+    PID_Init(&pid1);  
+    PID_Init(&pid2);
+    PID_Init(&pid3);
+
+
+    printf("输入本次使用的PID参数:\n");
+    printf("KP: ");
+    scanf("%f", &kp);
+    printf("KI: ");
+    scanf("%f", &ki);
+    printf("KD: ");
+    scanf("%f", &kd);
+    printf("普通位置式pid\t普通增量式pid\t前馈增量式pid\n误差表:\n");
+
+    // 设置PID参数
+    PID_SetParameter(&pid1, kp, ki, kd);  
+    PID_SetParameter(&pid2, kp, ki, kd);
+    PID_SetParameter(&pid3, kp, ki, kd);
+    pid1.target = 100;
+    pid1.max_integral = 100;
+    pid1.max_output = 200;
+    pid2.target = 100;
+    pid2.max_integral = 100;
+    pid2.max_output = 200;
+    pid3.target = 100;
+    pid3.max_integral = 100;
+    pid3.max_output = 200;
+    ff_mid_output = pid3.output;
+
+    // 模拟PID控制过程
+    for (int i = 0; i < 100; i++) {
+
+        // 普通位置式PID
+        PID_PostionalPID(&pid1);
+        pid1.current = pid1.output;
+
+        // 普通增量式PID
+        PID_IncrementalPID(&pid2);
+        pid2.current += pid2.output;
+
+        // 前馈增量式PID
+        PID_IncrementalPID(&pid3);
+        ff_mid_output += pid3.output;
+        pid3.current = FeedForward(pid3.target) + ff_mid_output;
+        printf("%.2f\t\t%.2f\t\t%.2f\n", pid1.error, pid2.error, pid3.error);
+    }
+    system("pause");
+    return 0;
+}

C_PID/Doge-ai/follow/pid.c

@@ -0,0 +1,105 @@
+#include "pid.h"
+#include <math.h>
+
+/*************************************************************************
+ *  函数名称：PID_Init
+ *  功能说明：PID初始化
+ *  参数说明：pid: PID结构体指针
+ *  函数返回：0
+ *************************************************************************/
+int PID_Init(PID *pid) {
+    pid->kp = 1;
+    pid->ki = 0;
+    pid->kd = 0;
+    pid->p_out = 0;
+    pid->i_out = 0;
+    pid->d_out = 0;
+    pid->output = 0;
+    pid->target = 0;
+    pid->current = 0;
+    pid->error = 0;
+    pid->last_error = 0;
+    pid->pre_last_error = 0;
+    pid->i_band = 1000;
+    pid->max_integral = 1000;
+    pid->max_output = 1000;
+    return 0;
+}
+
+/*************************************************************************
+ *  函数名称：PID_SetParameter
+ *  功能说明：PID参数设置
+ *  参数说明：pid: PID结构体指针
+ *            kp: 比例系数
+ *            ki: 积分系数
+ *            kd: 微分系数
+ *  函数返回：0
+ *************************************************************************/
+int PID_SetParameter(PID *pid, float kp, float ki, float kd) {
+    pid->kp = kp;
+    pid->ki = ki;
+    pid->kd = kd;
+    return 0;
+}
+
+/*************************************************************************
+ *  函数名称：PID_PostionalPID
+ *  功能说明：位置式PID计算
+ *  参数说明：pid: PID结构体指针
+ *  函数返回：0
+ *************************************************************************/
+int PID_PostionalPID(PID *pid) {
+    pid->error = pid->target - pid->current;
+    pid->p_out = pid->kp * pid->error;
+    // 积分分离
+    if(fabs(pid->current) <= pid->i_band){
+        pid->i_out += pid->ki * pid->error;
+        // 积分限幅
+        LIMIT(pid->i_out, pid->max_integral);
+    }
+    else pid->i_out = 0;
+    pid->d_out = pid->kd * (pid->error - pid->last_error);
+    pid->output = pid->p_out + pid->i_out + pid->d_out;
+    // 输出限幅
+    LIMIT(pid->output, pid->max_output);
+    pid->last_error = pid->error;
+    return 0;
+}
+
+/*************************************************************************
+ *  函数名称：PID_IncrementalPID
+ *  功能说明：普通增量式PID计算
+ *  参数说明：pid: PID结构体指针
+ *  函数返回：0
+ *************************************************************************/
+int PID_IncrementalPID(PID *pid){
+    pid->error = pid->target - pid->current;
+    pid->p_out = pid->kp * (pid->error - pid->last_error);
+    // 积分分离
+    if(fabs(pid->current) <= pid->i_band){
+        pid->i_out = pid->ki * pid->error;
+        // 积分限幅
+        LIMIT(pid->i_out, pid->max_integral);
+    }
+    else pid->i_out = 0;
+    pid->d_out = pid->kd * (pid->error - 2 * pid->last_error + pid->pre_last_error);
+    pid->output = pid->p_out + pid->i_out + pid->d_out;
+    // 输出限幅
+    LIMIT(pid->output, pid->max_output);
+    pid->pre_last_error = pid->last_error;
+    pid->last_error = pid->error;
+    return 0;
+}
+
+/*************************************************************************
+ *  函数名称：FeedForward
+ *  功能说明：根据目标与预测模型前馈控制
+ *  参数说明：target: 目标值
+ *  函数返回：前馈输出值
+ *************************************************************************/
+float FeedForward(float target)
+{
+    //这里简单预测为线性模型
+    float kf = 0.5;
+    return kf * target;
+}

C_PID/Doge-ai/follow/pid.h

@@ -0,0 +1,37 @@
+#ifndef PID_H
+#define PID_H//多次用
+#define LIMIT(TargetValue, LimitValue) \
+if (TargetValue > LimitValue)\
+{\
+  TargetValue = LimitValue;\
+}\
+else if (TargetValue < -LimitValue)\
+{\
+  TargetValue = -LimitValue;\
+}//限幅
+
+typedef struct {
+    float kp;  // 比例系数
+    float ki;  // 积分系数
+    float kd;  // 微分系数
+    float p_out;  // 比例输出
+    float i_out;  // 积分输出
+    float d_out;  // 微分输出
+    float output;  // 输出值
+    float target;  // 设定值
+    float current;  // 当前值
+    float error;  // 误差
+    float last_error;  // 上一次的误差
+    float pre_last_error;  // 上上次的误差
+    float i_band;  // 积分分离值
+    float max_integral; // 最大积分值
+    float max_output; // 最大输出值
+} PID;//各项数据封装为一个结构体
+
+int PID_Init(PID *pid);//初始化
+int PID_SetParameter(PID *pid, float kp, float ki, float kd);//设置参数
+int PID_PostionalPID(PID *pid);//位置式PID计算
+int PID_IncrementalPID(PID *pid);//增量式PID计算
+float FeedForward(float target);//前馈式
+
+#endif /* PID_H */

C_PID/Doge-ai/try/main.c

@@ -0,0 +1,49 @@
+//自己从零尝试做了，但是具体公式的实现和模拟的过程没弄明白（甚至感觉因为看了蛮多不同的代码弄混了）
+//另外找了一个代码研究和注释，再研究研究先，后面再尝试弄自己的，怕时间到了就打pr了，也顺便把这个失败品发了（
+#include <stdio.h>
+#include"pid.h"
+#include"pid.c" 
+int main()
+{
+    PID pid1,pid2;
+    PID_Init(&pid1);
+    PID_Init(&pid2);
+
+    float kp,ki,kd;
+    printf("输入本次使用的PID参数\n");
+    printf("KP:");
+    scanf("%lf",&kp);
+    printf("KI:");
+    scanf("%lf",&ki);
+    printf("KD:");
+    scanf("%lf",&kd);
+
+    PID_SetParameter(kp,ki,kd,&pid1);
+    PID_SetParameter(kp,ki,kd,&pid2);
+
+    float target;
+    printf("请输入目标值:");
+    scanf("%lf",&target);
+    float actual;
+    printf("请输入当前值:");
+    scanf("%lf",&actual);
+
+    float actual1,actual2;
+    actual1=target;
+    actual2=target;
+
+    int times =0;
+    PID_Init(&pid1);
+    PID_Init(&pid1);
+    printf("普通位置式pid误差表:              普通增量式pid误差表:\n");
+    for(times = 0 ; times<10 ; times++)
+    {
+        PID_PostionalPID(target,actual1,&pid1);
+        PID_IncrementalPID(target,actual2,&pid2);
+        Limit_i(&pid1);
+        Limit_i(&pid2);
+        printf("E=%2f                        E=%2f\n",pid1.err,pid2.err);
+    }
+
+    return 0;
+}

C_PID/Doge-ai/try/pid.c

@@ -0,0 +1,63 @@
+#include<stdio.h>
+#include"pid.h"
+
+int PID_Init(PID*pid)
+{
+    pid->kp=0.0;
+    pid->ki=0.0;
+    pid->kd=0.0;
+    pid->err=0.0;
+    pid->target=100;
+    pid->errLast=0.0;
+    pid->errSum=0.0;
+    pid->out=0.0;  
+    pid->LastOut=0.0;
+    pid->inte=0.0; 
+    pid->actual=0.0;
+    return 0;
+}
+int Limit_i(PID *pid)
+{
+    if (pid->inte > 100);
+        pid->inte = 100 ;
+    if (pid->inte <= 100);
+        pid->inte = 100;
+    return 0;//对i设限
+}
+int PID_SetParameter(float kp, float ki, float kd, PID*pid)
+{
+    pid->kp=kp;
+    pid->ki=ki;
+    pid->kd=kd;
+    return 0;
+}
+int PID_PostionalPID(float target, float actual, PID*pid)
+{
+    pid->target=target;
+    pid->actual=actual;
+
+    pid->err=pid->target-pid->actual;
+    pid->inte+=pid->err;
+    pid->errSum+=pid->err;
+
+    pid->inte+=pid->err;
+    Limit_i(pid);
+    pid->out=pid->kp*pid->err+pid->ki*pid->inte+pid->kd*(pid->err-pid->errLast);
+    pid->errLast=pid->err;
+
+    return pid->out;
+}
+int PID_IncrementalPID(float target, float actual, PID*pid)
+{
+    pid->target=target;
+    pid->actual=actual;
+    pid->err=pid->target-pid->actual;
+
+    pid->out=pid->LastOut+pid->kp*(pid->err-pid->errLast)+pid->ki*pid->err+pid->kd*(pid->err-2*pid->errLast+pid->errSum);
+
+    pid->errLast=pid->err;
+    pid->errSum+=pid->err;
+    pid->LastOut=pid->out;
+
+    return pid->out;
+}

C_PID/Doge-ai/try/pid.h

@@ -0,0 +1,24 @@
+#pragma once
+#ifndef _PID_H_
+#define _PID_H_
+
+typedef struct 
+{
+    float LastOut,out;//输出
+    float inte;//积分
+    float kp,ki,kd;//比例积分微分系数
+    float target;//目标值
+    float actual;//实际值
+    float errSum;//误差和
+    float err;//误差s
+    float errLast;//上次误差
+
+
+} PID;
+
+    int PID_Init(PID*pid);       //初始化PID结构体
+    int PID_SetParameter(float kp, float ki, float kd, PID*pid);  //设置PID参数
+    int PID_PostionalPID(float target, float actual, PID*pid);      //位置式PID实现
+    int PID_IncrementalPID(float target, float actual, PID*pid);    //增量式PID实现
+
+    #endif