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

修复LK电机id计算错误，构建平衡底盘框架，增加通用通信模块，增加平衡底盘条件编译兼容，删除lqr

Browse Source
This commit is contained in:
NeoZng
2023-02-16 15:46:04 +08:00
parent b9a7d87dfd
commit a2a83f9fbf
35 changed files with 554 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
modules/algorithm/LQR.c
View File

@@ -1 +0,0 @@
#include "LQR.h"

12
modules/algorithm/LQR.h
View File

@@ -1,12 +0,0 @@
/**
* @file LQR.h
* @author your name (you@domain.com)
* @brief 利用arm math库实现矩阵运算功能
* @version 0.1
* @date 2023-02-14
*
* @copyright Copyright (c) 2023
*
*/
#pragma once

38
modules/algorithm/controller.c
View File

@@ -9,7 +9,7 @@
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h"
#include <memory.h>
#include "memory.h"
/* ----------------------------下面是pid优化环节的实现---------------------------- */
@@ -40,7 +40,7 @@ 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 (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
@@ -117,7 +117,6 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
@@ -126,8 +125,8 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
* @param pid PID实例
* @param config PID初始化设置
*/
void PID_Init(PIDInstance *pid, PID_Init_Config_s *config)
{
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
// 后续修改为逐个赋值
@@ -135,7 +134,6 @@ void PID_Init(PIDInstance *pid, PID_Init_Config_s *config)
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_Config_s));
// set rest of memory to 0
}
/**
@@ -145,13 +143,13 @@ void PID_Init(PIDInstance *pid, PID_Init_Config_s *config)
* @param[in] 期望值
* @retval 返回空
*/
float PID_Calculate(PIDInstance *pid, float measure, float ref)
float PIDCalculate(PIDInstance *pid, float measure, float ref)
{
// 堵转检测
if (pid->Improve & ErrorHandle)
if (pid->Improve & PID_ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); //获取两次pid计算的时间间隔,用于积分和微分
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;
@@ -160,33 +158,33 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref)
// 如果在死区外,则计算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 & Trapezoid_Intergral)
if (pid->Improve & PID_Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & ChangingIntegrationRate)
if (pid->Improve & PID_ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & Derivative_On_Measurement)
if (pid->Improve & PID_Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & DerivativeFilter)
if (pid->Improve & PID_DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & Integral_Limit)
if (pid->Improve & PID_Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm; // 累加积分
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 输出滤波
if (pid->Improve & OutputFilter)
if (pid->Improve & PID_OutputFilter)
f_Output_Filter(pid);
// 输出限幅
@@ -194,10 +192,10 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref)
}
else // 进入死区, 则清空积分和输出
{
pid->Output=0;
pid->ITerm=0;
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;

46
modules/algorithm/controller.h
View File

@@ -15,7 +15,7 @@
#include "main.h"
#include "stdint.h"
#include "string.h"
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "arm_math.h"
@@ -25,18 +25,18 @@
#define abs(x) ((x > 0) ? x : -x)
#endif
// PID 优化环节使能标志位
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
typedef enum
{
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
Integral_Limit = 0b00000001, // 0000 0001
Derivative_On_Measurement = 0b00000010, // 0000 0010
Trapezoid_Intergral = 0b00000100, // 0000 0100
Proportional_On_Measurement = 0b00001000, // 0000 1000
OutputFilter = 0b00010000, // 0001 0000
ChangingIntegrationRate = 0b00100000, // 0010 0000
DerivativeFilter = 0b01000000, // 0100 0000
ErrorHandle = 0b10000000, // 1000 0000
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 报错类型枚举*/
@@ -60,16 +60,16 @@ typedef struct
float Kp;
float Ki;
float Kd;
float MaxOut;
float IntegralLimit;
float DeadBand;
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;
PID_Improvement_e Improve;
//-----------------------------------
// for calculating
float Measure;
@@ -96,31 +96,31 @@ typedef struct
} PIDInstance;
/* 用于PID初始化的结构体*/
typedef struct
typedef struct // config parameter
{
// config parameter
// basic parameter
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
float DeadBand; // 死区
float MaxOut; // 输出限幅
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float DeadBand; // 死区
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;
PID_Improvement_e Improve;
} PID_Init_Config_s;
/**
* @brief 初始化PID实例
*
* @todo 待修改为统一的PIDRegister风格
* @param pid PID实例指针
* @param config PID初始化配置
*/
void PID_Init(PIDInstance *pid, PID_Init_Config_s *config);
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
/**
* @brief 计算PID输出
@@ -130,6 +130,6 @@ void PID_Init(PIDInstance *pid, PID_Init_Config_s *config);
* @param ref 设定值
* @return float PID计算输出
*/
float PID_Calculate(PIDInstance *pid, float measure, float ref);
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif

1
modules/algorithm/kalman_filter.h
View File

@@ -37,6 +37,7 @@
#endif
#endif
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define mat arm_matrix_instance_f32
#define Matrix_Init arm_mat_init_f32
#define Matrix_Add arm_mat_add_f32

9
modules/algorithm/user_lib.c
View File

@@ -12,7 +12,7 @@
******************************************************************************
*/
#include "stdlib.h"
#include "string.h"
#include "memory.h"
#include "user_lib.h"
#include "math.h"
#include "main.h"
@@ -25,10 +25,10 @@
uint8_t GlobalDebugMode = 7;
void* zero_malloc(size_t size)
void *zero_malloc(size_t size)
{
void* ptr=malloc(size);
memset(ptr,0,size);
void *ptr = malloc(size);
memset(ptr, 0, size);
return ptr;
}
@@ -96,7 +96,6 @@ float float_deadband(float Value, float minValue, float maxValue)
return Value;
}
// 限幅函数
float float_constrain(float Value, float minValue, float maxValue)
{