ELCT/hero
Archived
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加文件1和文件2

Browse Source
This commit is contained in:
echo
2025-11-27 20:08:38 +08:00
parent 5d4099938c
commit edc427c423
1675 changed files with 1068907 additions and 2830 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
Mecanum Wheel moving project 1/Core/Src/PID.c Normal file
View File

@@ -0,0 +1,119 @@
#include "PID.h"
#include "main.h"
#include "M3508.h"
#include <math.h> // <20><><EFBFBD><EFBFBD>isnan<61><6E>isinf<6E><66><EFBFBD><EFBFBD>
#include "Remote.h"
#include "CToC.h"
/**
* @brief PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
*/
void PID_Init(PID_Controller_t* pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->integral = 0.0f;
pid->prev_error = 0.0f;
pid->output = 0.0f;
pid->integral_limit = integral_limit;
pid->output_limit = output_limit;
}
/**
* @brief PID<49><44><EFBFBD>㺯<EFBFBD><E3BAAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
float PID_Calculate(PID_Controller_t* pid, float error, float dt)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
if (dt <= 0.0f || dt > 1.0f) {
dt = 0.01f; // ʹ<><CAB9>Ĭ<EFBFBD><C4AC>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
// <20><><EFBFBD><EFBFBD>error<6F>Ƿ<EFBFBD>ΪNaN<61><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (isnan(error) || isinf(error)) {
error = 0.0f;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float proportional = pid->kp * error;
// <20><><EFBFBD><EFBFBD>proportional<61>Ƿ<EFBFBD>ΪNaN
if (isnan(proportional) || isinf(proportional)) {
proportional = 0.0f;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEA3A8><EFBFBD>޷<EFBFBD><DEB7><EFBFBD>
pid->integral += error * dt;
if (pid->integral > pid->integral_limit) {
pid->integral = pid->integral_limit;
} else if (pid->integral < -pid->integral_limit) {
pid->integral = -pid->integral_limit;
}
float integral = pid->ki * pid->integral;
// <20><><EFBFBD><EFBFBD>integral<61>Ƿ<EFBFBD>ΪNaN
if (isnan(integral) || isinf(integral)) {
integral = 0.0f;
pid->integral = 0.0f;
}
// ΢<><CEA2><EFBFBD><EFBFBD><EEA3A8><EFBFBD><EFBFBD>dt<64><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float derivative = 0.0f;
if (dt > 0.001f) { // ȷ<><C8B7>dt<64>㹻<EFBFBD>󣬱<EFBFBD><F3A3ACB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ժ<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
derivative = pid->kd * (error - pid->prev_error) / dt;
}
// <20><><EFBFBD><EFBFBD>derivative<76>Ƿ<EFBFBD>ΪNaN
if (isnan(derivative) || isinf(derivative)) {
derivative = 0.0f;
}
pid->prev_error = error;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pid->output = proportional + integral + derivative;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ΪNaN
if (isnan(pid->output) || isinf(pid->output)) {
pid->output = 0.0f;
// <20><><EFBFBD><EFBFBD>PID״̬
pid->integral = 0.0f;
pid->prev_error = 0.0f;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
if (pid->output > pid->output_limit) {
pid->output = pid->output_limit;
} else if (pid->output < -pid->output_limit) {
pid->output = -pid->output_limit;
}
return pid->output;
}
/**
* @brief <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void Motor_PID_Init(void)
{
// PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>ʵ<EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԣ<EFBFBD>
float speed_kp = 400.0f; // <20>ٶȻ<D9B6><C8BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
float speed_ki = 0.05f; // <20>ٶȻ<D9B6><C8BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
float speed_kd = 300.0f; // <20>ٶȻ<D9B6>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>
for (int i = 0; i < 4; i++) {
PID_Init(&motor_controller[i].speed_pid, speed_kp, speed_ki, speed_kd, 3000.0f, 3000.0f);
}
}
/*
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:λ<><CEBB>ʽPID<49><44><EFBFBD><EFBFBD>
*<2A><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>:λ<><CEBB>ʽPID<49><44><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><>
*<2A><>ע:<3A><><EFBFBD><EFBFBD>PID<49>ڲ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
*/
void PID_PositionClean(PID_Controller_t* pid)
{
pid->integral = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEA3AC>ֹ<EFBFBD><D6B9><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>
pid->prev_error = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEA3AC>ֹ΢<D6B9>ֳ<EFBFBD><D6B3><EFBFBD>
pid->output = 0.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
}