refactor(main): 统一外设初始化流程
集中所有外设初始化于main函数起始处，通过返回值确认各模块是否成功启动； refactor(adc): 修改采样器初始化返回值 将sampler构造函数替换为带返回值的init方法，在main函数中加入初始化结果判断， 确保系统启动时硬件配置正确加载。 feat(motor): 引入motor模块并重构电机控制逻辑 将GPIO操作封装进motor_sleep和motor_resume函数，提升代码可读性和维护性。 引入motor.h头文件，并在main.cpp中进行电机初始化统一管理。 修改FOC控制器中的shutdown与resume方法调用新接口。 refactor(filter): 优化卡尔曼滤波器实现 重命名状态变量xp为x，简化访问逻辑；移除冗余注释及未使用的getlastAngle方法； 调整getVelocity计算系数，提高速度估算精度；增加init方法支持初始状态设置。 refactor(angle_sensor): 移除冗余速度计算功能 删除传感器内部的速度估算相关数组和方法，交由外部滤波器处理以降低耦合度。 更新SPI片选宏定义以区分不同设备，并增强错误处理机制。 refactor(can): 调整CAN通信ID及初始化流程 更改状态与配置帧的标识符ID，添加对特定命令ID的响应处理如电机复位指令。 将CAN_transmiter构造函数改为init方法，配合主程序完成模块化初始化检查。
This commit is contained in:
@@ -8,6 +8,7 @@
|
||||
#include <cmath>
|
||||
#include <gd32c10x.h>
|
||||
#include <stdint.h>
|
||||
#include "motor.h"
|
||||
|
||||
constexpr double PI = 3.1415926535897932384626433832795f;
|
||||
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
|
||||
@@ -112,7 +113,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
|
||||
*/
|
||||
void FOC_Controlroller::shut_down() {
|
||||
is_shut_down = 1;
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
motor_sleep();
|
||||
}
|
||||
/**
|
||||
* @brief 占空比置0
|
||||
@@ -138,7 +139,7 @@ void FOC_Controlroller::resume() {
|
||||
over_current = false;
|
||||
voltage_error = false;
|
||||
is_shut_down = false;
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_12);
|
||||
motor_resume();
|
||||
}
|
||||
/**
|
||||
* @brief 更新电源电压
|
||||
@@ -235,13 +236,13 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
|
||||
*/
|
||||
float FOC_Controlroller::velocity_control(float target_velocity) {
|
||||
// PID 控制计算
|
||||
float Uq = vel_loop(target_velocity - rt_velocity);
|
||||
float Uq = vel_loop((target_velocity - rt_velocity)*25);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f);
|
||||
CAN0_transmiter.testfloat_union.f =
|
||||
// CAN0_transmiter.testfloat_union.f =
|
||||
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
|
||||
return rt_velocity;
|
||||
}
|
||||
|
||||
@@ -43,7 +43,7 @@ class FOC_Controlroller {
|
||||
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||||
|
||||
|
||||
PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
|
||||
PIDController vel_loop = PIDController{0.13f, 0.4f, 2.8e-4, 1000000,
|
||||
voltage_power_supply}; // 12V
|
||||
PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
|
||||
voltage_power_supply}; // 12V
|
||||
|
||||
@@ -105,18 +105,16 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
|
||||
\brief 更新卡尔曼滤波器状态
|
||||
\param[in] measured_angle 测量的角度值 (degree)
|
||||
\retval 估计的角度值 (degree)
|
||||
\note 实现卡尔曼滤波的预测和更新步骤，处理角度环绕问题
|
||||
\note 实现卡尔曼滤波的预测和更新步骤
|
||||
*/
|
||||
float EncoderAngleKalman::update(float measured_angle) {
|
||||
// 更新last_angle以保持同步
|
||||
// 更新last_angle以支持速度计算
|
||||
last_angle = kalman.getState();
|
||||
// 1. 预测步骤 - 速度估计
|
||||
kalman.predict(last_angle);
|
||||
|
||||
// 2. 更新步骤 - 使用处理后的角度值更新
|
||||
float filtered_angle = kalman.update(measured_angle);
|
||||
|
||||
|
||||
return filtered_angle;
|
||||
}
|
||||
|
||||
@@ -129,12 +127,7 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
|
||||
\brief 获取当前角度估计值
|
||||
\retval 当前角度估计值 (degree)
|
||||
*/
|
||||
float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*1e4; }
|
||||
/*!
|
||||
\brief 获取前一个角度估计值
|
||||
\retval 前一个角度估计值 (degree)
|
||||
*/
|
||||
float EncoderAngleKalman::getlastAngle() const { return last_angle; }
|
||||
float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*3.75e3; }
|
||||
|
||||
/*!
|
||||
\brief 设置过程噪声协方差
|
||||
|
||||
+21
-15
@@ -24,12 +24,12 @@ float medium_filter(float values[], u8 size);
|
||||
*/
|
||||
template <typename T> class KalmanFilter {
|
||||
private:
|
||||
T xp; // 状态估计
|
||||
T P; // 估计误差协方差
|
||||
T Q; // 过程噪声协方差
|
||||
T R; // 测量噪声协方差
|
||||
|
||||
public:
|
||||
T x; // 状态估计
|
||||
/*!
|
||||
\brief 构造函数
|
||||
\param[in] initial_state 初始状态值
|
||||
@@ -39,7 +39,7 @@ template <typename T> class KalmanFilter {
|
||||
*/
|
||||
KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
|
||||
T measurement_noise)
|
||||
: xp(initial_state), P(initial_error_covariance), Q(process_noise),
|
||||
: x(initial_state), P(initial_error_covariance), Q(process_noise),
|
||||
R(measurement_noise) {}
|
||||
|
||||
/*!
|
||||
@@ -48,7 +48,7 @@ template <typename T> class KalmanFilter {
|
||||
\retval none
|
||||
*/
|
||||
void predict(T state_prediction) {
|
||||
xp = state_prediction;
|
||||
x = state_prediction;
|
||||
P += Q;
|
||||
}
|
||||
|
||||
@@ -63,10 +63,10 @@ template <typename T> class KalmanFilter {
|
||||
T S = P + R; // 残差协方差
|
||||
T K = P / S; // 卡尔曼增益
|
||||
|
||||
xp += K * y; // 更新状态估计
|
||||
x += K * y; // 更新状态估计
|
||||
P = (1 - K) * P; // 更新误差协方差
|
||||
|
||||
return xp;
|
||||
return x;
|
||||
}
|
||||
|
||||
/*!
|
||||
@@ -77,16 +77,16 @@ template <typename T> class KalmanFilter {
|
||||
T update(T measurement) {
|
||||
T K = P / (P + R); // 卡尔曼增益
|
||||
|
||||
xp += K * (measurement - xp); // 更新状态估计
|
||||
x += K * (measurement - x); // 更新状态估计
|
||||
P = (1 - K) * P; // 更新误差协方差
|
||||
return xp;
|
||||
return x;
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 获取当前状态估计值
|
||||
\retval 当前状态估计值
|
||||
*/
|
||||
T getState() const { return xp; }
|
||||
T getState() const { return x; }
|
||||
|
||||
/*!
|
||||
\brief 设置过程噪声协方差
|
||||
@@ -103,7 +103,6 @@ template <typename T> class KalmanFilter {
|
||||
void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
|
||||
};
|
||||
|
||||
|
||||
/*!
|
||||
\brief 编码器速度卡尔曼滤波器类
|
||||
\note 用于滤除编码器速度测量噪声的卡尔曼滤波器实现
|
||||
@@ -124,6 +123,11 @@ class EncoderVelocityKalman {
|
||||
void setProcessNoise(float q);
|
||||
|
||||
void setMeasurementNoise(float r);
|
||||
|
||||
u8 init(float init_velocity) {
|
||||
kalman.x = init_velocity;
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
|
||||
/*!
|
||||
@@ -131,28 +135,30 @@ class EncoderVelocityKalman {
|
||||
\note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现
|
||||
*/
|
||||
class EncoderAngleKalman {
|
||||
private:
|
||||
private:
|
||||
KalmanFilter<float> kalman;
|
||||
float last_angle;
|
||||
bool initialized;
|
||||
|
||||
public:
|
||||
public:
|
||||
float last_angle;
|
||||
EncoderAngleKalman(float initial_angle, float initial_error_covariance,
|
||||
float process_noise, float measurement_noise);
|
||||
|
||||
float update(float measured_angle);
|
||||
|
||||
float getAngle() const;
|
||||
float getlastAngle() const;
|
||||
float getVelocity() const;
|
||||
|
||||
|
||||
void setProcessNoise(float q);
|
||||
float getstate() const { return kalman.getState(); };
|
||||
|
||||
void setMeasurementNoise(float r);
|
||||
u8 init(float init_angle) {
|
||||
kalman.x = init_angle;
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif // __cplusplus
|
||||
|
||||
#endif // _FILTER_H
|
||||
Reference in New Issue
Block a user