cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(angle): 引入角度卡尔曼滤波器 EncoderAngleKalman

Browse Source 
新增 EncoderAngleKalman 类以处理编码器角度噪声，改善角度反馈质量，并配合速度滤波器提供更准确的速度估算。

perf(CAN): 优化CAN数据打包与状态标识字段
改进 pack_u16_to_bytes 等函数实现方式，使用 reinterpret_cast 提高效率；
扩展 Status_code 为多个独立状态位，增强诊断能力。

feat(FOC): 添加电机锁止和停止功能
新增 drv_stop 和 drv_hold 方法用于控制电机占空比，支持紧急制动与角度保持。优化了FOC控制器的接口设计，提升安全性和可控性。

fix(filter): 修正中值滤波器数组大小动态分配问题
将 medium_filter 函数中的排序数组改为使用传入参数 size 动态定义，避免固定大小带来的资源浪费或越界风险，提高滤波精度和稳定性。

chore(project): 移除冗余文件及代码清理
移除了 thermal.cpp/h 文件及相关引用，调整项目结构，删除未使用的全局变量定义、注释和头文件包含，精简系统初始化流程。
This commit is contained in:
cbb
2025-11-22 23:59:43 +08:00
parent a7a3805839
commit 59f161bd73
19 changed files with 310 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+18 -2
View File
@@ -20,6 +20,7 @@ constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float zero_electric_angle = 0.0f;
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
@@ -93,6 +94,22 @@ void FOC_Controlroller::shut_down() {
is_shut_down = 1;
gpio_bit_reset(GPIOB, GPIO_PIN_12);
}
/**
* @brief 占空比置0
*
*/
void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
* @brief 电机锁止
*/
void FOC_Controlroller::drv_hold(float duty) {
if (duty > 0.95f) {
duty = 0.95f;
} else if (duty < 0.0f) {
duty = 0.0f;
}
Set_3Pwm(duty, duty, duty);
}
/**
* @brief 电机恢复
*
@@ -113,8 +130,7 @@ void FOC_Controlroller::update_power(void) {
voltage_error = false;
v_half = voltage_power_supply * 0.5f;
v_safe = voltage_power_supply * 0.95f;
}
else {
} else {
voltage_error = true;
}
}
Robot_V2/Algorithm/DengFOC.h
+4 -2
View File
@@ -28,6 +28,8 @@ class FOC_Controlroller {
void DFOC_alignSensor(int _PP);
void shut_down(void);
void resume(void);
void drv_stop(void);
void drv_hold(float duty = 0.5f);
protected:
std::u64 last_time_us = 0;
@@ -42,9 +44,9 @@ class FOC_Controlroller {
// voltage_power_supply};//8V
// PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000,
// voltage_power_supply}; // 8V
PIDController vel_loop = PIDController{1.0f, 0.85f, 0.0002f, 1000000,
PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
voltage_power_supply}; // 12V
PIDController angle_loop = PIDController{70.0f, 75.0f, 0.0001f, 10000000,
PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
voltage_power_supply}; // 12V
void setPhaseVoltage(float Uq, float Ud);
Robot_V2/Algorithm/Filter.cpp
+78 -45
View File
@@ -1,53 +1,14 @@
#include "Filter.h"
#include "Can.h"
#include "ang_sensor.h"
#include "gd32c10x_can.h"
#include <algorithm>
#include <array>
#include <cstdio>
#include <cstring>
#include <stdint.h>
extern angle_sensor KTH7823;
EncoderVelocityKalman Vel_kalman(KTH7823.getVelocity(), 1e-5f, 1e-6f, 0.05f);
// 队列实现
void CircularBuffer::enqueue(const float value) {
buffer[head] = value;
head = (head + 1) % HISTORY_SIZE;
if (count < HISTORY_SIZE)
count++;
}
/*!
\brief 获取操作 - O(1)
\param[out] float 最新的元素
\retval 最新的元素值
\note 注意：此操作不删除元素，只是返回最新的元素
*/
float CircularBuffer::get(size_t index) const {
if (index >= count)
return buffer[head]; // 返回最新元素
size_t pos = (head + HISTORY_SIZE - 1 - index) % HISTORY_SIZE;
return buffer[pos];
}
/*!
\brief 获取最新的5个数值
\param[out] latest_values: 指向存储最新5个数值的数组的指针
\retval none
\note 数组必须至少能容纳5个float元素
*/
void CircularBuffer::getLatestFive(float *latest_values) const {
for (size_t i = 0; i < 5 && i < count; ++i) {
latest_values[i] = get(i);
}
// 如果历史数据不足5个，用同样的数字填充剩余位置
for (size_t i = count; i < 5; ++i) {
latest_values[i] = latest_values[count - 1];
}
}
extern CAN_transmiter CAN0_transmiter;
constexpr u8 SIZE_FLOAT = sizeof(float);
/*!
\brief 中值滤波器函数
@@ -58,8 +19,8 @@ void CircularBuffer::getLatestFive(float *latest_values) const {
*/
float medium_filter(float values[], u8 size) {
// 使用局部数组避免动态内存分配
float sorted[HISTORY_SIZE];
memcpy(sorted, values, size * sizeof(float));
float sorted[size];
memcpy(sorted, values, size * SIZE_FLOAT);
// 这比完整排序更高效，时间复杂度从O(n log n)降低到O(n)
std::nth_element(sorted, sorted + size / 2, sorted + size);
@@ -169,3 +130,75 @@ void EncoderVelocityKalman::setProcessNoise(float q) {
void EncoderVelocityKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
}
// EncoderAngleKalman类的实现
/*!
\brief 构造函数，初始化编码器角度卡尔曼滤波器参数
\param[in] initial_angle 初始角度值 (degree)
\param[in] initial_error_covariance 初始误差协方差
\param[in] process_noise 过程噪声协方差
\param[in] measurement_noise 测量噪声协方差
\retval none
*/
EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
float initial_error_covariance,
float process_noise,
float measurement_noise)
: kalman(initial_angle, initial_error_covariance, process_noise,
measurement_noise),
last_angle(initial_angle), initialized(true) {}
/*!
\brief 更新卡尔曼滤波器状态
\param[in] measured_angle 测量的角度值 (degree)
\retval 估计的角度值 (degree)
\note 实现卡尔曼滤波的预测和更新步骤，处理角度环绕问题
*/
float EncoderAngleKalman::update(float measured_angle) {
// 更新last_angle以保持同步
last_angle = kalman.getState();
// 1. 预测步骤 - 假设角度符合随机游走模型（角速度为0）
// 由于角度变化相对缓慢，我们可以简单预测角度保持不变
kalman.predict(kalman.getState());
// 2. 更新步骤 - 使用处理后的角度值更新
float filtered_angle = kalman.update(measured_angle);
return filtered_angle;
}
/*!
\brief 获取当前角度估计值
\retval 当前角度估计值 (degree)
*/
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; }
/*!
\brief 设置过程噪声协方差
\param[in] q 过程噪声协方差值
\retval none
*/
void EncoderAngleKalman::setProcessNoise(float q) {
kalman.setProcessNoise(q);
}
/*!
\brief 设置测量噪声协方差
\param[in] r 测量噪声协方差值
\retval none
*/
void EncoderAngleKalman::setMeasurementNoise(float r) {
kalman.setMeasurementNoise(r);
}
Robot_V2/Algorithm/Filter.h
+27 -16
View File
@@ -18,21 +18,6 @@ extern "C" {
#include <cstring>
#include <stdint.h>
constexpr u8 HISTORY_SIZE = 10; // 历史数据大小
// 环形缓冲区类，用于存储历史数据
class CircularBuffer {
private:
std::array<float, HISTORY_SIZE> buffer;
size_t head = 0;
size_t count = 0;
public:
void enqueue(float value);
float get(size_t index) const;
void getLatestFive(float *latest_values) const;
};
float medium_filter(float values[], u8 size);
/*!
@@ -125,7 +110,7 @@ template <typename T> class KalmanFilter {
\note 基于电机模型的卡尔曼滤波器实现，用于电流状态估计
\note
// 创建卡尔曼滤波器实例
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.02f, 1e-6f, 0.02f);
// 更新滤波器状态并获取估计值
float estimatedCurrent = kalman.update(measuredCurrent, appliedVoltage);
*/
@@ -171,6 +156,32 @@ class EncoderVelocityKalman {
void setMeasurementNoise(float r);
};
/*!
\brief 编码器角度卡尔曼滤波器类
\note 用于滤除编码器角度测量噪声的卡尔曼滤波器实现
*/
class EncoderAngleKalman {
private:
KalmanFilter<float> kalman;
float last_angle;
bool initialized;
public:
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);
void setMeasurementNoise(float r);
};
#endif // __cplusplus
#endif // _FILTER_H
Robot_V2/Algorithm/thermal.cpp
-41
View File
@@ -1,41 +0,0 @@
#include "thermal.h"
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
#define SQUARE(x) ((x) * (x))
#define max(a, b, c) \
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
u32 ThermalController::update() {
thermal_value = thermal_value * thermalRate;
thermal_value +=
SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp());
if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热
if (thermal_value < thermal_capacity * recoveryRate) {
remainingTime10Ms = 1; // 可以恢复工作
}
if (remainingTime10Ms == 1) { // 过热恢复
FOC.resume();
return --remainingTime10Ms;
}
return --remainingTime10Ms;
} else { // 当前没有过热停机
current_max =
1000 * max(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W')); // 获取电流最大值
thermal_value +=
SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值
}
if (thermal_value > thermal_capacity) {
FOC.shut_down();
remainingTime10Ms = PROTECT_TIME; // 过热停机10秒
return remainingTime10Ms;
}
return thermal_value;
}
Robot_V2/Algorithm/thermal.h
-20
View File
@@ -1,27 +1,7 @@
#ifndef THERMAL_H
#define THERMAL_H
#include "stdint.h"
class ThermalController {
private:
u8 temperature;
u16 current_max;
float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例）
float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作）
u32 thermal_value = 0;
u32 thermal_capacity = 2250000; // 热容量
u16 gapTimeMs = 10; // 单次计算间隔ms
u16 remainingTime10Ms = 0; // 过热冷却剩余ms
public:
u32 update();
void reset() {
thermal_value = 0;
remainingTime10Ms = 0;
}
};
#endif
Robot_V2/Robot_V2.cproject.yml
+2 -2
View File
@@ -10,6 +10,8 @@ project:
- file: sys/systick.h
- file: sys/initial.cpp
- file: sys/initial.h
- file: sys/health.h
- file: sys/health.cpp
- group: device
files:
- file: device/Can.cpp
@@ -29,8 +31,6 @@ project:
- file: Algorithm/DengFOC.h
- file: Algorithm/pid.cpp
- file: pid.h
- file: Algorithm/thermal.cpp
- file: Algorithm/thermal.h
components:
- component: ARM::CMSIS:CORE
- component: Device:Startup
Robot_V2/device/ADC.cpp
+1 -1
View File
@@ -10,7 +10,6 @@ constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
constexpr float VCC_FACTOR = 11.0f;
// constexpr float CURRENT_OFFSET = 0 - 4096.0f / 2.0f;
constexpr float CURRENT_OFFSET = -1.60f;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
@@ -21,6 +20,7 @@ constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
Robot_V2/device/Can.cpp
+9 -6
View File
@@ -20,7 +20,7 @@ constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
extern CAN_transmiter CAN0_transmiter;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controlroller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
@@ -432,10 +432,10 @@ void CAN_transmiter::update(void) {
mcu_temp = sampler.get_mcu_temp() * 100;
motor_temp = sampler.get_drv_temp() * 100;
Power_Voltage = sampler.get_vbus_voltage() * 1000;
rt_angle_union.f = KTH7823.getAngle();
rt_angle_union.f = FOC.rt_angle;
// rt_velocity_union.f = KTH7823.getVelocity();
rt_velocity_union.f = FOC.rt_velocity;
Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
Target_Control_Mode = FOC.control_mode;
controller_Mode = FOC.control_mode;
@@ -591,7 +591,7 @@ u8 CAN_transmiter::CAN_all_Send() {
success_flag += CAN_Send(Current_FEED, 6, transmit_data);
// 打包Status_FEED
transmit_data[0] = Status_code;
transmit_data[0] = MotorStatus;
transmit_data[1] = controller_Mode;
pack_u16_to_bytes(transmit_data, 2, motor_temp);
pack_u16_to_bytes(transmit_data, 4, mcu_temp);
@@ -621,7 +621,11 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f));
transmit_data[10] = Status_code;
transmit_data[10] = (transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) |
((MagStatus & 0x01) << 1) |
((ThermalStatus & 0x01) << 2));
pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
@@ -645,7 +649,6 @@ u8 CAN_transmiter::CANFD_all_Send() {
* 1: 有新消息
*/
void CAN_transmiter::communication_check() {
uint8_t i = 0;
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
Robot_V2/device/Can.h
+15 -22
View File
@@ -17,39 +17,30 @@ typedef union {
} float_union_t;
// 添加辅助函数用于将u16类型数据打包到u8数组中
static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
buffer[offset] = (u8)(value & 0xFF);
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
static inline void pack_u16_to_bytes(u8 * const buffer, const u16 offset, const u16 value) {
*(reinterpret_cast<u16*>(buffer + offset)) = value;
}
// 添加辅助函数用于将float类型数据打包到u8数组中
static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
float_union_t &float_union) {
buffer[offset] = float_union.b[0];
buffer[offset + 1] = float_union.b[1];
buffer[offset + 2] = float_union.b[2];
buffer[offset + 3] = float_union.b[3];
static inline void pack_float_to_bytes(u8 * const buffer, const u16 offset,
const float_union_t &float_union) {
*(reinterpret_cast<u32*>(buffer + offset)) = *(reinterpret_cast<const u32*>(float_union.b));
}
// 添加辅助函数用于从u8数组中解包short类型数据
static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
static inline short unpack_bytes_to_short(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const short*>(buffer + offset));
}
// 添加辅助函数用于从u8数组中解包u16类型数据
static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
static inline u16 unpack_bytes_to_u16(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const u16*>(buffer + offset));
}
// 添加辅助函数用于从u8数组中解包float类型数据
static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
float_union_t float_union;
float_union.b[0] = buffer[offset];
float_union.b[1] = buffer[offset + 1];
float_union.b[2] = buffer[offset + 2];
float_union.b[3] = buffer[offset + 3];
static inline float_union_t unpack_bytes_to_float(const u8 * const buffer, const u16 offset) {
static float_union_t float_union;
*(reinterpret_cast<u32*>(float_union.b)) = *(reinterpret_cast<const u32*>(buffer + offset));
return float_union;
}
@@ -79,7 +70,7 @@ class CAN_transmiter {
u16 Power_Voltage = 0; // 电源电压
float_union_t rt_angle_union = {0}; // 实时角度
float_union_t rt_velocity_union = {0}; // 实时速度
u8 Status_code = 0; // 状态码
bool MotorStatus = 0; // 电机状态码
float_union_t target_angle_union = {0}; // 目标角度
u8 Target_Control_Mode = 0; // 目标控制模式
float_union_t target_velocity_union = {0}; // 目标速度
@@ -108,6 +99,8 @@ class CAN_transmiter {
FlagStatus can0_send_flag; // CAN0发送标志
float_union_t testfloat_union = {0}; // 测试用
bool MagStatus = 0; // 磁状态码
bool ThermalStatus = 0; // 热能状态码
void communication_check(); // 通信检查
};
Robot_V2/device/Timer.cpp
+9 -8
View File
@@ -1,4 +1,5 @@
#include "Timer.h"
#include "Can.h"
#include "DengFOC.h"
#include "Timer.h"
#include "gd32c10x.h"
@@ -28,6 +29,7 @@ static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern FOC_Controlroller FOC;
extern TaskScheduler scheduler;
extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器
static volatile std::u64 system_runtime_us = 0;
@@ -179,6 +181,7 @@ void TIMER2_IRQHandler(void) {
break;
}
case 4: {
CAN0_transmiter.can0_send_flag = SET;
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
@@ -198,6 +201,7 @@ void TIMER2_IRQHandler(void) {
break;
}
case 2: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
break;
}
}
@@ -275,7 +279,7 @@ void timer1_gpio_config(void) {
gpio_bit_set(GPIOB, GPIO_PIN_0);
gpio_bit_set(GPIOB, GPIO_PIN_12);
/* 配置TIMER1完全重映射 - 应该使用DISABLE，因为我们使用的是默认引脚 */
/* 配置TIMER1完全重映射*/
gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
}
@@ -358,7 +362,6 @@ extern "C" void TIMER1_IRQHandler(void) {
timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE);
// 在这里添加需要在PWM周期90%位置执行的代码
// 例如触发ADC采样或其他操作
}
}
@@ -391,10 +394,8 @@ void Set_Pwm(u16 channel, float duty_cycle) {
\note The channel parameter must be between 0 and 3 inclusive.
*/
void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
duty_cycle0 * TIMER1_PERIOD);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1,
duty_cycle1 * TIMER1_PERIOD);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
duty_cycle2 * TIMER1_PERIOD);
// 直接写寄存器
TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
}
Robot_V2/device/ang_sensor.cpp
+8 -3
View File
@@ -1,11 +1,13 @@
#include "ang_sensor.h"
#include "Filter.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "initial.h"
#include <cstdio>
#include <cstring>
#include <stdint.h>
#include "initial.h"
#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
@@ -17,12 +19,16 @@ constexpr u16 FAULT_RECEIVE = 0xFFFF;
constexpr float VELOCITY_SCALE = 1e4f;
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
constexpr float ANGLE_WRAP_VALUE = 360.0f;
constexpr int SIZE_FLOAT = sizeof(float);
constexpr u8 SIZE_FLOAT = sizeof(float);
// DMA传输完成标志
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
angle_sensor KTH7823; // 角度传感器初始化
EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
/**
* @brief SPI DMA initialization
*/
@@ -297,7 +303,6 @@ float angle_sensor::getcurrent_sensorangle(void) const {
return current_angle_deg;
}
/**
* @brief 重置累积角度值
*/
Robot_V2/main.cpp
+21 -19
View File
@@ -3,36 +3,35 @@
#include "DengFOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "core_cm4.h"
#include "health.h"
#include "initial.h"
#include "sys/health.h"
#include "sys/systick.h"
#include "systick.h"
#include "thermal.h"
#include "ang_sensor.h"
#include "adc.h"
TaskScheduler scheduler = {0};
// 全局变量定义
CAN_transmiter CAN0_transmiter; // CAN初始化
FOC_Controlroller FOC; // FOC控制初始化
sampler sampler; // 采样器初始化
angle_sensor KTH7823; // 角度传感器初始化
ThermalController thermalController; // 热量控制器初始化
extern EncoderVelocityKalman Vel_kalman;
int main(void) {
systick_config(); // 系统时钟初始化
USART0_init(); // 调试串口初始化
User_timer_init(); // Timer初始化
driver_init(); // 电机驱动初始化
extern FOC_Controlroller FOC; // FOC控制
extern sampler sampler; // 采样器
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern angle_sensor KTH7823; // 角度传感器
extern CAN_transmiter CAN0_transmiter; // CAN
extern ThermalController thermalController; // 热量控制器
delay_1ms(10);
angsens_MG_test(); // 角度传感器磁感应强度测试
delay_1ms(5);
angsens_MG_check(); // 角度传感器磁感应强度测试
watchdog_init(5000); // 看门狗初始化
while (1) {
// delay_1ms(100);
fwdgt_counter_reload(); // 看门狗复位
// std::printf("1\n");
// 保存当前任务标志快照以提高效率并确保一致性
u32 currentTasks = scheduler.taskFlags;
@@ -46,15 +45,16 @@ int main(void) {
}
if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值
KTH7823.update();
FOC.rt_angle = KTH7823.getAngle();
FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
}
if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
}
if (currentTasks &
(1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
switch (FOC.control_mode) {
case 0: {
FOC.drv_hold(0.95); // 占空比置0
break;
}
case 1: {
@@ -83,14 +83,16 @@ int main(void) {
sampler.update_drv_temp();
}
if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制
CAN0_transmiter.testfloat_union.f = thermalController.update();
thermalController.update();
}
if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测
angsens_MG_check();
}
if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
sampler.update_vbus_voltage();
FOC.update_power();
}
if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
CAN0_transmiter.can0_send_flag = SET;
CAN0_transmiter.communication_check();
}
}
Robot_V2/sys/health.cpp
+58
View File
@@ -0,0 +1,58 @@
#include "health.h"
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"
extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
ThermalController thermalController; // 热量控制器初始化
#define SQUARE(x) ((x) * (x))
#define max(a, b, c) \
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
constexpr u16 PROTECT_TIME = 3000; // 过热保护时间，单位*10ms
/**
** @brief 热能计算
** @param None
** @retval 热量值
*/
u32 ThermalController::update() {
thermal_value = thermal_value * thermalRate;
thermal_value +=
SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp());
if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热
if (thermal_value < thermal_capacity * recoveryRate) {
remainingTime10Ms = 1; // 可以恢复工作
}
if (remainingTime10Ms == 1) { // 过热恢复
FOC.resume();
CAN0_transmiter.ThermalStatus = 0;
return --remainingTime10Ms;
}
return --remainingTime10Ms;
} else { // 当前没有过热停机
current_max =
1000 * max(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W')); // 获取电流最大值
thermal_value +=
SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值
}
if (thermal_value > thermal_capacity) {
FOC.shut_down();
remainingTime10Ms = PROTECT_TIME; // 过热停机
CAN0_transmiter.ThermalStatus = 1;
return remainingTime10Ms;
}
return thermal_value;
}
/**
** @brief 温度检测
** @param None
*/
void angsens_MG_check(void) {
bool MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
CAN0_transmiter.MagStatus = MGH or MGL;
}
Robot_V2/sys/health.h
+29
View File
@@ -0,0 +1,29 @@
#ifndef _HEALTH_H
#define _HEALTH_H
#include "stdint.h"
class ThermalController {
private:
u8 temperature;
u16 current_max;
float thermalRate = 0.995f; // 热耗散系数（每单位时间散热比例）
float recoveryRate = 0.6f; // 自然恢复率（散热到多少以下开始工作）
u32 thermal_value = 0;
u32 thermal_capacity = 2250000; // 热容量
u16 gapTimeMs = 10; // 单次计算间隔ms
u16 remainingTime10Ms = 0; // 过热冷却剩余ms
public:
u32 update();
void reset() {
thermal_value = 0;
remainingTime10Ms = 0;
}
};
void angsens_MG_check(void);
#endif
Robot_V2/sys/initial.cpp
+2 -5
View File
@@ -3,6 +3,7 @@
#include "ang_sensor.h"
#include <stdint.h>
void GPIO_config(void) {
// 使能GPIOB时钟
rcu_periph_clock_enable(RCU_GPIOB);
@@ -31,11 +32,7 @@ u8 watchdog_init(u32 time) {
return status;
}
void angsens_MG_test(void) {
u8 MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
u8 MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
std::printf("MGH:%d MGL:%d\r\n", MGH, MGL);
}
/**
* @brief driver输出初始化
*/
Robot_V2/sys/initial.h
+1 -1
View File
@@ -21,7 +21,6 @@ u8 tle5210b_init(void);
u8 USART0_init(void);
u8 watchdog_init(u32 time);
void driver_init(void);
void angsens_MG_test(void);
// 任务标志定义
typedef enum {
TASK_MOTOR_CONTROL = 0,
@@ -33,6 +32,7 @@ typedef enum {
TASK_VBUS_UPDATE,
TASK_THERMAL_CONTROL,
TASK_CAN_TRANSMIT,
TASK_MAGNETIC_CHECK,
TASK_COUNT
} SystemTask;
Robot_V2/sys/systick.cpp
+20
View File
@@ -95,3 +95,23 @@ void delay_decrement(void)
delay--;
}
}
/*!
\brief system soft reset
\param[in] none
\param[out] none
\retval none
*/
void system_soft_reset(void)
{
/* Set the software reset bit */
SCB->AIRCR = (0x05FA0000U | (SCB->AIRCR & (0x700U)) | (1U << 2));
/* Data Synchronization Barrier */
__DSB();
/* Wait for reset */
while (1){
__NOP();
}
}
Robot_V2/sys/systick.h
+2
View File
@@ -52,6 +52,8 @@ void delay_decrement(void);
void idle_task_count(void);
/* 获取CPU使用率 */
uint8_t get_cpu_usage(void);
/* 系统软复位 */
void system_soft_reset(void);
#ifdef __cplusplus
}