From 11de628646e9d6c28d245dc489a895c3dabcc922 Mon Sep 17 00:00:00 2001 From: FineYoung <3539492701@qq.com> Date: Mon, 29 Apr 2024 10:52:13 +0000 Subject: [PATCH] add application. Signed-off-by: FineYoung <3539492701@qq.com> --- application/pid.c | 201 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 201 insertions(+) create mode 100644 application/pid.c diff --git a/application/pid.c b/application/pid.c new file mode 100644 index 0000000..80fc2e2 --- /dev/null +++ b/application/pid.c @@ -0,0 +1,201 @@ +#include "debug.h" +#include "gpio.h" +#include "main.h" +#include "pid.h" + +extern unsigned int g_TimerInterruptCount; + +/* 增量式PID */ +PidEulerCar g_pidEulerCarRight; +PidEulerCar g_pidEulerCarLeft; +// 前后抖动 +// #define Kp 0.05 +// #define Ki 0.02 +// #define Kd 0.5 +// #define Kp 0.03 +// #define Ki 0.01 +// #define Kd 0.3 + +// v1 +// #define Kp 0.07 +// #define Ki 0.002 +// #define Kd 0.0 + +// v2 back +// #define Kp 0.48 +// #define Ki 0.95 +// #define Kd 0.007 + +// motor right +// #define KP_RIGHT 0.50 +// #define KI_RIGHT 1.01 +// #define KD_RIGHT 0.007 + +// motor left +// #define KP_LEFT 0.50 +// #define KI_LEFT 1.01 +// #define KD_LEFT 0.007 + +// motor right +//#define KP_RIGHT 0.2 +//#define KI_RIGHT 1.0 +//#define KD_RIGHT 0.007 + +// motor right +#define KP_RIGHT 0.3 +#define KI_RIGHT 0.01 +#define KD_RIGHT 0.0 + +// motor left +#define KP_LEFT 0.2 +#define KI_LEFT 0.01 +#define KD_LEFT 0.0 + +void Pid_Right_Init(void) +{ + g_pidEulerCarRight.SetSpeed = 0; + g_pidEulerCarRight.ActualSpeed = 0; + g_pidEulerCarRight.TargetSpeed = 0; + g_pidEulerCarRight.err = 0; + g_pidEulerCarRight.i_err = 0; + g_pidEulerCarRight.err_last = 0; + g_pidEulerCarRight.err_next = 0; + g_pidEulerCarRight.kp_param = KP_RIGHT; + g_pidEulerCarRight.ki_param = KI_RIGHT; + g_pidEulerCarRight.kd_param = KD_RIGHT; + DBG_PRINTF("pid right init success\r\n"); +} + +void Pid_Left_Init(void) +{ + g_pidEulerCarLeft.SetSpeed = 0; + g_pidEulerCarLeft.ActualSpeed = 0; + g_pidEulerCarLeft.TargetSpeed = 0; + g_pidEulerCarLeft.err = 0; + g_pidEulerCarLeft.i_err = 0; + g_pidEulerCarLeft.err_last = 0; + g_pidEulerCarLeft.err_next = 0; + g_pidEulerCarLeft.kp_param = KP_LEFT; + g_pidEulerCarLeft.ki_param = KI_LEFT; + g_pidEulerCarLeft.kd_param = KD_LEFT; + DBG_PRINTF("pid left init success\r\n"); +} + +void Set_Pid_TargertSpeed(float initSpeed) +{ + g_pidEulerCarRight.TargetSpeed = initSpeed; + g_pidEulerCarLeft.TargetSpeed = initSpeed; +} + + + +float Pid_Right_Ctrl(float setSpeed, float realSpeed) +{ + + g_pidEulerCarRight.SetSpeed = setSpeed; + g_pidEulerCarRight.ActualSpeed = realSpeed; + + //位置式PID算法，计算出目标轮速 + g_pidEulerCarRight.err = g_pidEulerCarRight.SetSpeed - g_pidEulerCarRight.ActualSpeed; + g_pidEulerCarRight.i_err = g_pidEulerCarRight.i_err + g_pidEulerCarRight.err; + g_pidEulerCarRight.TargetSpeed = KP_RIGHT * g_pidEulerCarRight.err + + KI_RIGHT * g_pidEulerCarRight.i_err + + KD_RIGHT * (g_pidEulerCarRight.err - g_pidEulerCarRight.err_next); + + g_pidEulerCarRight.err_last = g_pidEulerCarRight.err_next; + g_pidEulerCarRight.err_next = g_pidEulerCarRight.err; + + //限制幅度 + if(setSpeed > 0){ + if (g_pidEulerCarRight.TargetSpeed > 1280){ + g_pidEulerCarRight.TargetSpeed = 1280; + } + if (g_pidEulerCarRight.TargetSpeed < 0){ + g_pidEulerCarRight.TargetSpeed = 0; + } + } + + if(setSpeed < 0){ + if (g_pidEulerCarRight.TargetSpeed < -1280){ + g_pidEulerCarRight.TargetSpeed = -1280; + } + if (g_pidEulerCarRight.TargetSpeed > 0){ + g_pidEulerCarRight.TargetSpeed = 0; + } + } + + //如果电机停止需要设置启动PWM基础值 + if(g_pidEulerCarRight.ActualSpeed < 1){ + if(setSpeed > 0){ + g_pidEulerCarRight.TargetSpeed = g_pidEulerCarRight.TargetSpeed + 200; + } + else{ + g_pidEulerCarRight.TargetSpeed = g_pidEulerCarRight.TargetSpeed - 200; + } + } + + //每计数10次打印1次 + // if((g_TimerInterruptCount % 10) == 0) { + // DBG_PRINTF("MR:SS:%.0f, AS:%.0f, err:%.0f, TS:%.0f\r\n", + // g_pidEulerCarRight.SetSpeed, g_pidEulerCarRight.ActualSpeed,g_pidEulerCarRight.err,g_pidEulerCarRight.TargetSpeed); + // } + + return g_pidEulerCarRight.TargetSpeed; +} + + +float Pid_Left_Ctrl(float setSpeed, float realSpeed) +{ + + g_pidEulerCarLeft.SetSpeed = setSpeed; + g_pidEulerCarLeft.ActualSpeed = realSpeed; + + //位置式PID算法，计算出目标轮速 + g_pidEulerCarLeft.err = g_pidEulerCarLeft.SetSpeed - g_pidEulerCarLeft.ActualSpeed; + g_pidEulerCarLeft.i_err = g_pidEulerCarLeft.i_err + g_pidEulerCarLeft.err; + + g_pidEulerCarLeft.TargetSpeed = KP_RIGHT * g_pidEulerCarLeft.err + + KI_RIGHT * g_pidEulerCarLeft.i_err + + KD_RIGHT * (g_pidEulerCarLeft.err - g_pidEulerCarLeft.err_last); + + g_pidEulerCarLeft.err_last = g_pidEulerCarLeft.err_next; + g_pidEulerCarLeft.err_next = g_pidEulerCarLeft.err; + + //限制幅度 + if(setSpeed > 0){ + if (g_pidEulerCarLeft.TargetSpeed > 1280){ + g_pidEulerCarLeft.TargetSpeed = 1280; + } + if (g_pidEulerCarLeft.TargetSpeed < 0){ + g_pidEulerCarLeft.TargetSpeed = 0; + } + } + + if(setSpeed < 0){ + if (g_pidEulerCarLeft.TargetSpeed < -1280){ + g_pidEulerCarLeft.TargetSpeed = -1280; + } + if (g_pidEulerCarLeft.TargetSpeed > 0){ + g_pidEulerCarLeft.TargetSpeed = 0; + } + } + + //如果电机停止需要设置启动PWM基础值 + if(g_pidEulerCarLeft.ActualSpeed < 1){ + if(setSpeed > 0){ + g_pidEulerCarLeft.TargetSpeed = g_pidEulerCarLeft.TargetSpeed + 200; + } + else{ + g_pidEulerCarLeft.TargetSpeed = g_pidEulerCarLeft.TargetSpeed - 200; + } + } + + //每计数10次打印1次 + if((g_TimerInterruptCount % 10) == 0) { + DBG_PRINTF("ML:SS:%.0f, AS:%.0f, err:%.0f,TS:%.0f\r\n", + g_pidEulerCarLeft.SetSpeed, g_pidEulerCarLeft.ActualSpeed,g_pidEulerCarLeft.err,g_pidEulerCarLeft.TargetSpeed); + } + + return g_pidEulerCarLeft.TargetSpeed; + +} \ No newline at end of file -- Gitee