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

```

Browse Source 
feat(DengFOC):
新增了基于三相电流采样的dq轴电流计算函数`cal_Iq_Id`
调整了开环控制中的电压限制逻辑
更新了默认电源电压值为24V。
KHT7823测试代码
```
This commit is contained in:
cbb
2025-10-30 20:20:14 +08:00
parent 5d14731af5
commit 180a46271c
10 changed files with 318 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+41 -27
View File
@@ -1,14 +1,16 @@
#include "adc.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
#include <cstdio>
#include <cstring>
#include <stdint.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_OFFSET = 0 - 4096.0f / 2.0f;
constexpr float CURRENT_OFFSET = 0;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f;
@@ -18,18 +20,23 @@ 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[6] = {0}; // 显式初始化数组元素为0
/**
* @brief 采样类初始化
*/
void sampler::init() {
for (int i = 0; i < 7; i++) {
for (int i = 0; i < FILTER_SIZE + 1; 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;
currU_samples[i] =
(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currV_samples[i] =
(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currW_samples[i] =
(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * 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;
@@ -43,13 +50,13 @@ void sampler::init() {
* 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
*/
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, 7);
tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
}
/**
* @brief 获取电机驱动温度
@@ -59,7 +66,7 @@ float sampler::get_mcu_temp() {
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
float sampler::get_drv_temp() {
tempora_sample = medium_filter(drv_temp_samples, 7);
tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
@@ -77,15 +84,15 @@ float sampler::get_drv_temp() {
float sampler::get_current(char type) {
switch (type) {
case 'U': {
tempora_sample = medium_filter(currU_samples, 7);
tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1);
break;
}
case 'V': {
tempora_sample = medium_filter(currV_samples, 7);
tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1);
break;
}
case 'W': {
tempora_sample = medium_filter(currW_samples, 7);
tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1);
break;
}
default: {
@@ -106,11 +113,13 @@ float sampler::get_current(char type) {
* 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
*/
float sampler::get_vbus_voltage() {
tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
// 检查电压读数是否在有效范围内
if (vbus_samples[4] > MAX_VCC || vbus_samples[4] < MIN_VCC) {
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC ||
vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
return ERROR_ADC_VAL;
}
return vbus_samples[4];
return tempora_sample;
}
/**
@@ -120,36 +129,41 @@ float sampler::get_vbus_voltage() {
* 如果计算结果超出有效范围(-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;
memmove(mcu_temp_samples, mcu_temp_samples + 1, FILTER_SIZE * sizeof(float));
mcu_temp_samples[FILTER_SIZE] =
(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));
memmove(drv_temp_samples, drv_temp_samples + 1, FILTER_SIZE * 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;
drv_temp_samples[FILTER_SIZE] =
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;
memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float));
memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float));
memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float));
currU_samples[FILTER_SIZE] =
(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currV_samples[FILTER_SIZE] =
(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currW_samples[FILTER_SIZE] =
(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
}
/**
* @brief 更新供电电压
*/
void sampler::update_vbus_voltage() {
memmove(vbus_samples, vbus_samples + 1, 6 * sizeof(float));
vbus_samples[6] = adc_value[5] * ADC_SCALE;
memmove(vbus_samples, vbus_samples + 1, FILTER_SIZE * sizeof(float));
vbus_samples[FILTER_SIZE] = adc_value[5] * ADC_SCALE;
}