refactor(FOC): 重构齿槽转矩校准状态机为独立函数
- 将原本在 calibrate_cogging_tick 中的大函数拆分为多个职责单一的函数 - 新增 start_cogging_calibration、cogging_settling_tick、cogging_sampling_tick、 cogging_stopping_tick、cogging_done_tick 等状态处理函数 - 添加 process_cogging_run_data、start_next_cogging_run、build_cogging_tables、 print_cogging_table、print_cogging_c_array 等辅助函数 - 移动全局 FOC 控制器单例注释到更合适位置 - 添加齿槽转矩校准参数常量定义和详细注释 - 添加防御性编程处理默认状态分支，避免状态机卡死 - 提高代码可读性和维护性 feat(tasks): 添加任务头文件到项目配置 - 在 Robot_V2.cproject.yml 中添加 tasks.h 和 tasks.cpp 文件引用 docs(ADC): 添加ADC转换常量说明注释 - 在 ADC.cpp 中添加关于ADC换算常量的详细注释 - 说明全局采样器单例的初始化和使用方式
This commit is contained in:
+170
-150
@@ -20,6 +20,9 @@
|
||||
#include <cstring>
|
||||
#include <gd32c10x.h>
|
||||
|
||||
// 全局 FOC 控制器单例：系统只有一个电机控制对象。
|
||||
// 初始化顺序由 main::hardware_init() 明确调用 FOC.init() 完成；
|
||||
// 运行期主要由主循环控制任务访问，CAN 命令只更新目标/参数。
|
||||
FOC_Controller FOC; // FOC控制初始化
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
@@ -62,6 +65,14 @@ inline float FOC_Controller::get_cogging_compensation(float angle_el) {
|
||||
// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4）
|
||||
// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0
|
||||
#if FOC_DEBUG
|
||||
// 齿槽转矩校准参数
|
||||
constexpr u32 COG_CALIB_SETTLE_TICKS = 16000; // 800ms / 50us
|
||||
constexpr u32 COG_CALIB_STOP_TICKS = 10000; // 500ms / 50us
|
||||
constexpr u32 COG_CALIB_TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
|
||||
constexpr float COG_CALIB_SLOW_VEL = 1.0f; // 开环速度 rad/s
|
||||
constexpr u32 COG_CALIB_SAMPLE_DIV = 20; // 1ms / 50us
|
||||
constexpr u8 COG_CALIB_TOTAL_RUNS = 6; // 3正+3反
|
||||
|
||||
// 齿槽转矩校准静态数据
|
||||
float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {};
|
||||
float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {};
|
||||
@@ -70,172 +81,181 @@ u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};
|
||||
/**
|
||||
* @brief 齿槽转矩校准 - 非阻塞状态机
|
||||
* 正转3次 + 反转3次，分开存储补偿表。
|
||||
* 只打印最终平均数据和 C Array。
|
||||
*/
|
||||
void FOC_Controller::calibrate_cogging_tick(void) {
|
||||
/** @brief SETTLE_TICKS 常量定义 */
|
||||
constexpr u32 SETTLE_TICKS = 16000; // 800ms / 50us
|
||||
/** @brief STOP_TICKS 常量定义 */
|
||||
constexpr u32 STOP_TICKS = 10000; // 500ms / 50us
|
||||
/** @brief TARGET_SAMPLES 常量定义 */
|
||||
constexpr u32 TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
|
||||
/** @brief SLOW_VEL 常量定义 */
|
||||
constexpr float SLOW_VEL = 1.0f; // 开环速度 rad/s
|
||||
/** @brief SAMPLE_DIV 常量定义 */
|
||||
constexpr u32 SAMPLE_DIV = 20; // 1ms / 50us
|
||||
/** @brief TOTAL_RUNS 常量定义 */
|
||||
constexpr u8 TOTAL_RUNS = 6; // 3正+3反
|
||||
|
||||
// 入口只做状态派发，避免单函数堆叠所有校准分支。
|
||||
switch (cog_calib_state) {
|
||||
case CogCalibState::IDLE:
|
||||
if (!cogging_calib_request) return;
|
||||
cogging_calib_request = false;
|
||||
|
||||
cog_calib_run = 0;
|
||||
cog_calib_saved_mode = control_mode;
|
||||
cog_calib_saved_vel = target_velocity;
|
||||
cogging_comp_enabled = false;
|
||||
memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
|
||||
|
||||
printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
|
||||
|
||||
control_mode = 6;
|
||||
target_velocity = SLOW_VEL;
|
||||
cog_calib_settle_tick = 0;
|
||||
cog_calib_state = CogCalibState::SETTLING;
|
||||
start_cogging_calibration();
|
||||
return;
|
||||
|
||||
case CogCalibState::SETTLING:
|
||||
cog_calib_settle_tick++;
|
||||
if (cog_calib_settle_tick >= SETTLE_TICKS) {
|
||||
cog_calib_state = CogCalibState::SAMPLING;
|
||||
cog_calib_sample_count = 0;
|
||||
memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
|
||||
memset(cog_calib_count, 0, sizeof(cog_calib_count));
|
||||
}
|
||||
cogging_settling_tick();
|
||||
return;
|
||||
|
||||
case CogCalibState::SAMPLING:
|
||||
// 采样分频
|
||||
static u32 sample_div = 0;
|
||||
sample_div++;
|
||||
if (sample_div < SAMPLE_DIV) return;
|
||||
sample_div = 0;
|
||||
|
||||
{
|
||||
float real_angle = KTH7823.getcurrent_sensorangle();
|
||||
float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
|
||||
|
||||
float iq = rt_current;
|
||||
|
||||
if (fabsf(iq) < 5000.0f) {
|
||||
u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
|
||||
if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
|
||||
cog_calib_accum[idx] += iq;
|
||||
cog_calib_count[idx]++;
|
||||
}
|
||||
cog_calib_sample_count++;
|
||||
}
|
||||
if (cog_calib_sample_count >= TARGET_SAMPLES) {
|
||||
target_velocity = 0;
|
||||
cog_calib_state = CogCalibState::STOPPING;
|
||||
cog_calib_stop_tick = 0;
|
||||
}
|
||||
cogging_sampling_tick();
|
||||
return;
|
||||
|
||||
case CogCalibState::STOPPING:
|
||||
cog_calib_stop_tick++;
|
||||
if (cog_calib_stop_tick >= STOP_TICKS) {
|
||||
// 处理本轮数据：去直流
|
||||
float dc = 0;
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cog_calib_run_data[cog_calib_run][i] =
|
||||
(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
|
||||
dc += cog_calib_run_data[cog_calib_run][i];
|
||||
}
|
||||
dc /= (float)COGGING_TABLE_SIZE;
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cog_calib_run_data[cog_calib_run][i] -= dc;
|
||||
}
|
||||
|
||||
printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
|
||||
cog_calib_run + 1, TOTAL_RUNS, dc);
|
||||
|
||||
cog_calib_run++;
|
||||
if (cog_calib_run < TOTAL_RUNS) {
|
||||
// run 0~2 正转，run 3~5 反转
|
||||
control_mode = 6;
|
||||
target_velocity = (cog_calib_run < 3) ? SLOW_VEL : -SLOW_VEL;
|
||||
cog_calib_state = CogCalibState::SETTLING;
|
||||
cog_calib_settle_tick = 0;
|
||||
} else {
|
||||
cog_calib_state = CogCalibState::DONE;
|
||||
}
|
||||
}
|
||||
cogging_stopping_tick();
|
||||
return;
|
||||
|
||||
case CogCalibState::DONE:
|
||||
// 正转平均 (run 0~2)
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cogging_table[i] = (cog_calib_run_data[0][i] +
|
||||
cog_calib_run_data[1][i] +
|
||||
cog_calib_run_data[2][i]) / 3.0f;
|
||||
}
|
||||
// 反转平均 (run 3~5)
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cogging_table_rev[i] = (cog_calib_run_data[3][i] +
|
||||
cog_calib_run_data[4][i] +
|
||||
cog_calib_run_data[5][i]) / 3.0f;
|
||||
}
|
||||
cogging_comp_enabled = true;
|
||||
control_mode = cog_calib_saved_mode;
|
||||
target_velocity = cog_calib_saved_vel;
|
||||
|
||||
// 打印正转平均
|
||||
printf("\r\n[COG] === FWD Average ===\r\n");
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
|
||||
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table[i]);
|
||||
fwdgt_counter_reload();
|
||||
}
|
||||
|
||||
// 打印反转平均
|
||||
printf("\r\n[COG] === REV Average ===\r\n");
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
|
||||
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table_rev[i]);
|
||||
fwdgt_counter_reload();
|
||||
}
|
||||
|
||||
// 打印正转 C Array
|
||||
printf("\r\n[COG] --- FWD C Array ---\r\n");
|
||||
printf("static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {\r\n");
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
if (i % 8 == 0) printf(" ");
|
||||
printf("%.2ff", cogging_table[i]);
|
||||
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
|
||||
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
|
||||
fwdgt_counter_reload();
|
||||
}
|
||||
printf("};\r\n");
|
||||
|
||||
// 打印反转 C Array
|
||||
printf("\r\n[COG] --- REV C Array ---\r\n");
|
||||
printf("static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {\r\n");
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
if (i % 8 == 0) printf(" ");
|
||||
printf("%.2ff", cogging_table_rev[i]);
|
||||
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
|
||||
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
|
||||
fwdgt_counter_reload();
|
||||
}
|
||||
printf("};\r\n");
|
||||
|
||||
printf("\r\n[COG] === Done. ===\r\n");
|
||||
cogging_done_tick();
|
||||
return;
|
||||
default:
|
||||
// 防御性处理：枚举值异常时回到空闲态，避免状态机卡死。
|
||||
cog_calib_state = CogCalibState::IDLE;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void FOC_Controller::start_cogging_calibration(void) {
|
||||
// IDLE 状态下没有请求则保持空闲，不扰动正常控制路径。
|
||||
if (!cogging_calib_request) return;
|
||||
cogging_calib_request = false;
|
||||
|
||||
cog_calib_run = 0;
|
||||
// 保存原控制状态，校准完成后恢复现场。
|
||||
cog_calib_saved_mode = control_mode;
|
||||
cog_calib_saved_vel = target_velocity;
|
||||
cogging_comp_enabled = false;
|
||||
memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
|
||||
|
||||
printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
|
||||
|
||||
control_mode = 6;
|
||||
target_velocity = COG_CALIB_SLOW_VEL;
|
||||
cog_calib_settle_tick = 0;
|
||||
cog_calib_state = CogCalibState::SETTLING;
|
||||
}
|
||||
|
||||
void FOC_Controller::cogging_settling_tick(void) {
|
||||
// 给低速开环运行留出稳定时间，再开始采样。
|
||||
cog_calib_settle_tick++;
|
||||
if (cog_calib_settle_tick < COG_CALIB_SETTLE_TICKS) return;
|
||||
|
||||
cog_calib_state = CogCalibState::SAMPLING;
|
||||
cog_calib_sample_count = 0;
|
||||
memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
|
||||
memset(cog_calib_count, 0, sizeof(cog_calib_count));
|
||||
}
|
||||
|
||||
void FOC_Controller::cogging_sampling_tick(void) {
|
||||
static u32 sample_div = 0;
|
||||
// 每 1ms 采一次，降低校准数据相关处理对实时控制的影响。
|
||||
sample_div++;
|
||||
if (sample_div < COG_CALIB_SAMPLE_DIV) return;
|
||||
sample_div = 0;
|
||||
|
||||
float real_angle = KTH7823.getcurrent_sensorangle();
|
||||
float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
|
||||
float iq = rt_current;
|
||||
|
||||
if (fabsf(iq) < 5000.0f) {
|
||||
u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
|
||||
if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
|
||||
cog_calib_accum[idx] += iq;
|
||||
cog_calib_count[idx]++;
|
||||
}
|
||||
cog_calib_sample_count++;
|
||||
|
||||
if (cog_calib_sample_count >= COG_CALIB_TARGET_SAMPLES) {
|
||||
target_velocity = 0;
|
||||
cog_calib_state = CogCalibState::STOPPING;
|
||||
cog_calib_stop_tick = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void FOC_Controller::cogging_stopping_tick(void) {
|
||||
// 每轮采样结束后先停稳，再计算本轮去直流数据。
|
||||
cog_calib_stop_tick++;
|
||||
if (cog_calib_stop_tick < COG_CALIB_STOP_TICKS) return;
|
||||
|
||||
float dc = process_cogging_run_data();
|
||||
printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
|
||||
cog_calib_run + 1, COG_CALIB_TOTAL_RUNS, dc);
|
||||
|
||||
cog_calib_run++;
|
||||
if (cog_calib_run < COG_CALIB_TOTAL_RUNS) {
|
||||
start_next_cogging_run();
|
||||
} else {
|
||||
cog_calib_state = CogCalibState::DONE;
|
||||
}
|
||||
}
|
||||
|
||||
float FOC_Controller::process_cogging_run_data(void) {
|
||||
// 先求每个角度桶的平均电流，再统一去除直流偏置。
|
||||
float dc = 0;
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cog_calib_run_data[cog_calib_run][i] =
|
||||
(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
|
||||
dc += cog_calib_run_data[cog_calib_run][i];
|
||||
}
|
||||
dc /= (float)COGGING_TABLE_SIZE;
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cog_calib_run_data[cog_calib_run][i] -= dc;
|
||||
}
|
||||
return dc;
|
||||
}
|
||||
|
||||
void FOC_Controller::start_next_cogging_run(void) {
|
||||
control_mode = 6;
|
||||
target_velocity = (cog_calib_run < 3) ? COG_CALIB_SLOW_VEL : -COG_CALIB_SLOW_VEL;
|
||||
cog_calib_state = CogCalibState::SETTLING;
|
||||
cog_calib_settle_tick = 0;
|
||||
}
|
||||
|
||||
void FOC_Controller::cogging_done_tick(void) {
|
||||
// 生成最终表后恢复用户进入校准前的控制模式和目标速度。
|
||||
build_cogging_tables();
|
||||
cogging_comp_enabled = true;
|
||||
control_mode = cog_calib_saved_mode;
|
||||
target_velocity = cog_calib_saved_vel;
|
||||
|
||||
print_cogging_table("\r\n[COG] === FWD Average ===\r\n", cogging_table);
|
||||
print_cogging_table("\r\n[COG] === REV Average ===\r\n", cogging_table_rev);
|
||||
print_cogging_c_array("\r\n[COG] --- FWD C Array ---\r\n",
|
||||
"COGGING_TABLE_FWD", cogging_table);
|
||||
print_cogging_c_array("\r\n[COG] --- REV C Array ---\r\n",
|
||||
"COGGING_TABLE_REV", cogging_table_rev);
|
||||
|
||||
printf("\r\n[COG] === Done. ===\r\n");
|
||||
cog_calib_state = CogCalibState::IDLE;
|
||||
}
|
||||
|
||||
void FOC_Controller::build_cogging_tables(void) {
|
||||
// 前 3 轮为正转，后 3 轮为反转，分别求平均表。
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
cogging_table[i] = (cog_calib_run_data[0][i] +
|
||||
cog_calib_run_data[1][i] +
|
||||
cog_calib_run_data[2][i]) / 3.0f;
|
||||
cogging_table_rev[i] = (cog_calib_run_data[3][i] +
|
||||
cog_calib_run_data[4][i] +
|
||||
cog_calib_run_data[5][i]) / 3.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void FOC_Controller::print_cogging_table(const char *title, float *table) {
|
||||
printf("%s", title);
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
|
||||
printf("[COG] %.2f, %.2f\r\n", angle_deg, table[i]);
|
||||
fwdgt_counter_reload();
|
||||
}
|
||||
}
|
||||
|
||||
void FOC_Controller::print_cogging_c_array(const char *title,
|
||||
const char *arrayName,
|
||||
float *table) {
|
||||
printf("%s", title);
|
||||
printf("static const float %s[COGGING_TABLE_SIZE] = {\r\n", arrayName);
|
||||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||||
if (i % 8 == 0) printf(" ");
|
||||
printf("%.2ff", table[i]);
|
||||
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
|
||||
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
|
||||
fwdgt_counter_reload();
|
||||
}
|
||||
printf("};\r\n");
|
||||
}
|
||||
#endif // FOC_DEBUG
|
||||
|
||||
// 初始变量及函数定义
|
||||
|
||||
@@ -167,10 +167,66 @@ class FOC_Controller {
|
||||
#if FOC_DEBUG
|
||||
volatile bool cogging_calib_request = false;
|
||||
/**
|
||||
* @brief calibrate_cogging_tick 函数
|
||||
* @brief 齿槽校准状态机入口，每个周期按当前状态派发到对应处理函数
|
||||
* @return void
|
||||
*/
|
||||
void calibrate_cogging_tick(void);
|
||||
/**
|
||||
* @brief 处理齿槽校准空闲态，收到请求后保存现场并进入稳定等待态
|
||||
* @return void
|
||||
*/
|
||||
void start_cogging_calibration(void);
|
||||
/**
|
||||
* @brief 处理齿槽校准稳定等待态，等待电机进入稳定低速运行
|
||||
* @return void
|
||||
*/
|
||||
void cogging_settling_tick(void);
|
||||
/**
|
||||
* @brief 处理齿槽校准采样态，按分频周期累计每个电角度桶的电流
|
||||
* @return void
|
||||
*/
|
||||
void cogging_sampling_tick(void);
|
||||
/**
|
||||
* @brief 处理齿槽校准停止态，等待停稳后完成本轮数据归一化
|
||||
* @return void
|
||||
*/
|
||||
void cogging_stopping_tick(void);
|
||||
/**
|
||||
* @brief 处理齿槽校准完成态，生成补偿表并打印校准结果
|
||||
* @return void
|
||||
*/
|
||||
void cogging_done_tick(void);
|
||||
/**
|
||||
* @brief 计算当前校准轮次的平均电流并去除直流偏置
|
||||
* @return float 当前轮次直流偏置，单位 mA
|
||||
*/
|
||||
float process_cogging_run_data(void);
|
||||
/**
|
||||
* @brief 切换到下一轮齿槽校准运行方向并重新进入稳定等待态
|
||||
* @return void
|
||||
*/
|
||||
void start_next_cogging_run(void);
|
||||
/**
|
||||
* @brief 根据 3 次正转和 3 次反转采样结果生成最终补偿表
|
||||
* @return void
|
||||
*/
|
||||
void build_cogging_tables(void);
|
||||
/**
|
||||
* @brief 打印齿槽补偿表的人类可读角度/电流列表
|
||||
* @param[in] title 表格标题字符串
|
||||
* @param[in] table 待打印的补偿表
|
||||
* @return void
|
||||
*/
|
||||
void print_cogging_table(const char *title, float *table);
|
||||
/**
|
||||
* @brief 打印可直接复制回源码的齿槽补偿 C 数组
|
||||
* @param[in] title 输出标题字符串
|
||||
* @param[in] arrayName C 数组名
|
||||
* @param[in] table 待打印的补偿表
|
||||
* @return void
|
||||
*/
|
||||
void print_cogging_c_array(const char *title, const char *arrayName,
|
||||
float *table);
|
||||
/**
|
||||
* @brief CogCalibState 枚举定义
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user