TuxMonkey/tronone-h7-scaffold
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/tronone-h7-scaffold synced 2026-07-24 11:37:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

imu test

Browse Source
This commit is contained in:
TuxMonkey
2025-11-16 17:19:14 +08:00
parent de8a0d5117
commit ab01747052
20 changed files with 10796 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
User_Code/module/algorithm/controller/controller.c
View File

@@ -1,5 +1,208 @@
//
// Created by ASUS on 2025/11/15.
//
/**
* @file controller.c
* @author wanghongxi
* @author modified by neozng
* @brief PID控制器定义
* @version beta
* @date 2022-11-01
*
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h"
#include "memory.h"
/* ----------------------------下面是pid优化环节的实现---------------------------- */
// 梯形积分
static void f_Trapezoid_Intergral(PIDInstance *pid)
{
// 计算梯形的面积,(上底+下底)*高/2
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
// 变速积分(误差小时积分作用更强)
static void f_Changing_Integration_Rate(PIDInstance *pid)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
else // 最大阈值,不使用积分
pid->ITerm = 0;
}
}
static void f_Integral_Limit(PIDInstance *pid)
{
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
pid->ITerm = 0; // 当前积分项置零
}
}
if (temp_Iout > pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = pid->IntegralLimit;
}
if (temp_Iout < -pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = -pid->IntegralLimit;
}
}
// 微分先行(仅使用反馈值而不计参考输入的微分)
static void f_Derivative_On_Measurement(PIDInstance *pid)
{
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
// 微分滤波(采集微分时,滤除高频噪声)
static void f_Derivative_Filter(PIDInstance *pid)
{
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}
// 输出滤波
static void f_Output_Filter(PIDInstance *pid)
{
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 输出限幅
static void f_Output_Limit(PIDInstance *pid)
{
if (pid->Output > pid->MaxOut)
{
pid->Output = pid->MaxOut;
}
if (pid->Output < -(pid->MaxOut))
{
pid->Output = -(pid->MaxOut);
}
}
// 电机堵转检测
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
{
// Motor blocked counting
pid->ERRORHandler.ERRORCount++;
}
else
{
pid->ERRORHandler.ERRORCount = 0;
}
if (pid->ERRORHandler.ERRORCount > 500)
{
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = PID_MOTOR_BLOCKED_ERROR;
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
*
* @param pid PID实例
* @param config PID初始化设置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
// 后续修改为逐个赋值
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_Config_s));
// set rest of memory to 0
DWT_GetDeltaT(&pid->DWT_CNT);
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref)
{
// 堵转检测
if (pid->Improve & PID_ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
// 如果在死区外,则计算PID
if (abs(pid->Err) > pid->DeadBand)
{
// 基本的pid计算,使用位置式
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & PID_Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & PID_ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & PID_Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & PID_DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & PID_Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 输出滤波
if (pid->Improve & PID_OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
}
else // 进入死区, 则清空积分和输出
{
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}

140
User_Code/module/algorithm/controller/controller.h
View File

@@ -1,8 +1,136 @@
//
// Created by ASUS on 2025/11/15.
//
/**
******************************************************************************
* @file controller.h
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _CONTROLLER_H
#define _CONTROLLER_H
#ifndef TRONONEH7_SCAFFOLD_CONTROLLER_H
#define TRONONEH7_SCAFFOLD_CONTROLLER_H
#include "main.h"
#include "stdint.h"
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include <math.h>
#endif //TRONONEH7_SCAFFOLD_CONTROLLER_H
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
typedef enum
{
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
PID_Integral_Limit = 0b00000001, // 0000 0001
PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
PID_Proportional_On_Measurement = 0b00001000, // 0000 1000
PID_OutputFilter = 0b00010000, // 0001 0000
PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
PID_DerivativeFilter = 0b01000000, // 0100 0000
PID_ErrorHandle = 0b10000000, // 1000 0000
} PID_Improvement_e;
/* PID 报错类型枚举*/
typedef enum errorType_e
{
PID_ERROR_NONE = 0x00U,
PID_MOTOR_BLOCKED_ERROR = 0x01U
} ErrorType_e;
typedef struct
{
uint64_t ERRORCount;
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
/* PID结构体 */
typedef struct
{
//---------------------------------- init config block
// config parameter
float Kp;
float Ki;
float Kd;
float MaxOut;
float DeadBand;
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // 变速积分 For Changing Integral
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
float Derivative_LPF_RC; // 微分滤波器系数
//-----------------------------------
// for calculating
float Measure;
float Last_Measure;
float Err;
float Last_Err;
float Last_ITerm;
float Pout;
float Iout;
float Dout;
float ITerm;
float Output;
float Last_Output;
float Last_Dout;
float Ref;
uint32_t DWT_CNT;
float dt;
PID_ErrorHandler_t ERRORHandler;
} PIDInstance;
/* 用于PID初始化的结构体*/
typedef struct // config parameter
{
// basic parameter
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
float DeadBand; // 死区
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
} PID_Init_Config_s;
/**
* @brief 初始化PID实例
* @todo 待修改为统一的PIDRegister风格
* @param pid PID实例指针
* @param config PID初始化配置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
/**
* @brief 计算PID输出
*
* @param pid PID实例指针
* @param measure 反馈值
* @param ref 设定值
* @return float PID计算输出
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif