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

feat(FOC): 优化开环速度控制逻辑并提升性能

Browse Source 
refactor(ADC): 重构采样逻辑并封装为 sampler 类
refactor(CAN): 更新 CAN 发送器以使用新的sampler 接口
refactor(timer): 调整定时器中断结构并优化任务调度
This commit is contained in:
cbb
2025-10-19 23:20:28 +08:00
parent 34df732659
commit 7f0ca1857b
8 changed files with 309 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+88 -46
View File
@@ -1,39 +1,55 @@
#include "adc.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
#include <cstdio>
#include <stdint.h>
#include "math.h"
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_FACTOR = 1.0f / 50.0f * 100.0f * 1000.0f;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 50.0f;
constexpr float MAX_CURRENT = 65.0f;
constexpr float MIN_CURRENT = -0.1f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
u16 adc_value[7] = {0}; // 显式初始化数组元素为0
u16 adc_value[7] = {0}; // 显式初始化数组元素为0
/**
* @brief 采样类初始化
*/
void sampler::init() {
for (int i = 0; i < 7; i++) {
mcu_temp_samples[i] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
currU_samples[i] = adc_value[2] * ADC_SCALE * CURRENT_FACTOR;
currV_samples[i] = adc_value[3] * ADC_SCALE * CURRENT_FACTOR;
currW_samples[i] = adc_value[4] * ADC_SCALE * CURRENT_FACTOR;
vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[i] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}
}
/**
* @brief 获取MCU温度
* @return float
* 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
*/
float get_mcu_temp() {
float temp_latest = ERROR_ADC_VAL;
// 迭代采样5次，直到获得有效温度值
for (int i = 0; i < 5; i++) {
temp_latest = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
if (temp_latest < MIN_TEMP || temp_latest > MAX_TEMP)
return ERROR_ADC_VAL;
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, 7);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return temp_latest;
}
return tempora_sample;
}
/**
* @brief 获取电机驱动温度
@@ -42,19 +58,13 @@ float get_mcu_temp() {
* 通过热敏电阻分压电路计算温度值，使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
float get_drv_temp() {
float temp_latest = ERROR_ADC_VAL;
for (int i = 0; i < 5; i++) {
float ana_val = adc_value[1];
float R_now = ana_val / (4096 - ana_val) * 4700;
temp_latest =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
if (temp_latest < MIN_TEMP || temp_latest > MAX_TEMP) {
return ERROR_ADC_VAL;
}
float sampler::get_drv_temp() {
tempora_sample = medium_filter(drv_temp_samples, 7);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return temp_latest;
return tempora_sample;
}
/**
@@ -64,31 +74,28 @@ float get_drv_temp() {
*
* 通过ADC采样计算各相电流值
*/
float get_current(char type) {
float current_latest = ERROR_ADC_VAL;
float sampler::get_current(char type) {
switch (type) {
case 'U': {
current_latest = adc_value[2] * ADC_SCALE * CURRENT_FACTOR;
tempora_sample = medium_filter(currU_samples, 7);
break;
}
case 'V': {
current_latest = adc_value[3] * ADC_SCALE * CURRENT_FACTOR;
tempora_sample = medium_filter(currV_samples, 7);
break;
}
case 'W': {
current_latest = adc_value[4] * ADC_SCALE * CURRENT_FACTOR;
tempora_sample = medium_filter(currW_samples, 7);
break;
}
default: {
return ERROR_ADC_VAL;
}
}
// 检查电流读数是否在有效范围内
if (current_latest < MIN_CURRENT || current_latest > MAX_CURRENT) {
if (tempora_sample < MIN_CURRENT || tempora_sample > MAX_CURRENT) {
return ERROR_ADC_VAL;
}
return current_latest;
return tempora_sample;
}
/**
@@ -98,16 +105,51 @@ float get_current(char type) {
* 通过ADC采样计算供电电压值，使用迭代方式确保获取有效的电压读数。
* 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
*/
float get_vbus_voltage() {
float vcc_latest = ERROR_ADC_VAL;
for (int i = 0; i < 5; i++) {
// 获取电压值
vcc_latest = adc_value[5] * ADC_SCALE * VCC_FACTOR;
// 检查电压读数是否在有效范围内
if (vcc_latest > MAX_VCC || vcc_latest < MIN_VCC) {
return ERROR_ADC_VAL;
}
float sampler::get_vbus_voltage() {
// 检查电压读数是否在有效范围内
if (vbus_samples[4] > MAX_VCC || vbus_samples[4] < MIN_VCC) {
return ERROR_ADC_VAL;
}
return vcc_latest;
return vbus_samples[4];
}
/**
* @brief 更新MCU温度
*
* 通过ADC采样计算MCU温度值，使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
void sampler::update_mcu_temp() {
memmove(mcu_temp_samples, mcu_temp_samples + 1, 6 * sizeof(float));
mcu_temp_samples[6] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
}
/**
* @brief 更新MCU温度
*/
void sampler::update_drv_temp() {
memmove(drv_temp_samples, drv_temp_samples + 1, 6 * sizeof(float));
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[6] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}
/**
* @brief 更新相电流
*/
void sampler::update_current() {
memmove(currU_samples, currU_samples + 1, 6 * sizeof(float));
memmove(currV_samples, currV_samples + 1, 6 * sizeof(float));
memmove(currW_samples, currW_samples + 1, 6 * sizeof(float));
currU_samples[6] = adc_value[2] * ADC_SCALE;
currV_samples[6] = adc_value[3] * ADC_SCALE;
currW_samples[6] = adc_value[4] * ADC_SCALE;
}
/**
* @brief 更新供电电压
*/
void sampler::update_vbus_voltage() {
memmove(vbus_samples, vbus_samples + 1, 6 * sizeof(float));
vbus_samples[6] = adc_value[5] * ADC_SCALE;
}