From bec7c11b8715fcce3f8b84f97ca1a7f3f1400879 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 19 May 2026 01:26:42 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E9=BD=BF?= =?UTF-8?q?=E6=A7=BD=E8=BD=AC=E7=9F=A9=E8=A1=A5=E5=81=BF=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 实现齿槽转矩补偿算法，通过查表+线性插值方式计算补偿电压值 - 添加齿槽转矩校准功能，支持正转3次+反转3次的非阻塞校准状态机 - 在所有控制模式中集成齿槽转矩补偿，包括开环、角度、速度、电流和MIT模式 - 硬编码预校准的正反转补偿表数据 - 通过CAN命令支持运行时触发校准功能 - 添加相关配置参数和调试开关 --- Robot_V2/Algorithm/FOC.cpp | 331 ++++++++++++++++++++++++++------- Robot_V2/Algorithm/FOC.h | 33 +++- Robot_V2/Robot_V2.cproject.yml | 1 - Robot_V2/device/Can.cpp | 6 + Robot_V2/device/test.cpp | 33 ---- Robot_V2/main.cpp | 7 + Robot_V2/sys/config.h | 20 +- 7 files changed, 322 insertions(+), 109 deletions(-) delete mode 100644 Robot_V2/device/test.cpp diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 8a7fb5e..72edc20 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -7,8 +7,11 @@ #include "config.h" #include "motor.h" #include "pid.h" +#include "systick.h" #include #include +#include +#include #include FOC_Controller FOC; // FOC控制初始化 @@ -19,6 +22,207 @@ extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern CurrentKalman Current_kalman; extern u16 adc_value[3]; // regular DMA缓冲区：MCU温度 / DRV温度 / VBUS +// 齿槽转矩补偿：查表 + 线性插值 +// angle_el: 电角度（度） +// 返回: 补偿电压值 (V)，叠加到 Uq 上 +inline float FOC_Controller::get_cogging_compensation(float angle_el) { + if (!cogging_comp_enabled) return 0.0f; + + // 归一化到 [0, 360) + float angle = fmodf(angle_el, 360.0f); + if (angle < 0.0f) angle += 360.0f; + + // 线性插值查表 + float index_f = angle / 360.0f * (float)COGGING_TABLE_SIZE; + u16 idx0 = (u16)index_f; + u16 idx1 = (idx0 + 1) % COGGING_TABLE_SIZE; + float frac = index_f - (float)idx0; + + // 根据转向选择补偿表（带滞回，避免零速附近跳变） + static float *last_table = cogging_table; + if (rt_velocity < -0.5f) last_table = cogging_table_rev; + else if (rt_velocity > 0.5f) last_table = cogging_table; + float *table = last_table; + float comp = table[idx0] * (1.0f - frac) + table[idx1] * frac; + + return -comp * cogging_comp_scale; // mA→V: 补偿电流(mA) × 换算系数(V/mA) +} + +// === 齿槽转矩校准（DEBUG，平时不启用）=== +// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4） +// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0 +#if FOC_DEBUG +// 齿槽转矩校准静态数据 +float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {}; +float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {}; +u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {}; + +/** + * @brief 齿槽转矩校准 - 非阻塞状态机 + * 正转3次 + 反转3次，分开存储补偿表。 + * 只打印最终平均数据和 C Array。 + */ +void FOC_Controller::calibrate_cogging_tick(void) { + constexpr u32 SETTLE_TICKS = 16000; // 800ms / 50us + constexpr u32 STOP_TICKS = 10000; // 500ms / 50us + constexpr u32 TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8; + constexpr float SLOW_VEL = 1.0f; // 开环速度 rad/s + constexpr u32 SAMPLE_DIV = 20; // 1ms / 50us + 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; + 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)); + } + 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 = fmodf(real_angle * PP + KTH7823.calibration_angle, 360.0f); + if (angle_el < 0.0f) angle_el += 360.0f; + + 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; + } + 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; + } + } + 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"); + cog_calib_state = CogCalibState::IDLE; + return; + } +} +#endif // FOC_DEBUG + // 初始变量及函数定义 #define _constrain(amt, low, high) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) @@ -92,6 +296,47 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, u8 FOC_Controller::init() { motor_init(); // 电机驱动初始化 + // 齿槽转矩补偿表（硬编码，3正+3反平均，用真实角度算Iq） + static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = { + 29.28f, 28.84f, 28.26f, 28.83f, 29.79f, 30.26f, 29.52f, 28.77f, + 28.23f, 27.68f, 26.65f, 25.19f, 23.92f, 22.59f, 19.61f, 16.77f, + 13.32f, 10.40f, 7.64f, 3.51f, -0.83f, -4.96f, -8.19f, -11.11f, + -13.40f, -14.39f, -15.69f, -15.99f, -16.39f, -15.50f, -14.76f, -14.21f, + -13.76f, -13.88f, -13.22f, -11.18f, -10.65f, -10.54f, -10.62f, -10.23f, + -9.52f, -8.19f, -6.31f, -4.14f, -1.94f, -0.25f, 2.06f, 4.08f, + 6.05f, 8.30f, 9.74f, 11.38f, 13.32f, 15.65f, 17.55f, 20.22f, + 21.72f, 22.28f, 23.30f, 24.78f, 23.53f, 23.35f, 23.00f, 21.42f, + 20.35f, 20.57f, 20.07f, 18.18f, 17.04f, 16.03f, 14.81f, 13.71f, + 12.47f, 10.93f, 9.38f, 6.53f, 4.63f, 3.37f, 1.29f, -1.28f, + -5.08f, -11.11f, -16.35f, -21.00f, -24.61f, -28.30f, -31.11f, -33.48f, + -35.25f, -36.54f, -38.01f, -38.69f, -39.27f, -39.52f, -38.18f, -36.55f, + -34.96f, -33.14f, -31.92f, -31.45f, -31.32f, -30.25f, -29.60f, -28.72f, + -27.51f, -25.30f, -22.66f, -20.87f, -18.19f, -16.10f, -13.25f, -10.81f, + -8.00f, -4.67f, -2.28f, 0.87f, 3.83f, 7.25f, 10.56f, 13.70f, + 17.70f, 19.90f, 22.14f, 23.62f, 24.53f, 26.03f, 26.96f, 27.95f, + }; + static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = { + 22.45f, 24.95f, 26.05f, 26.01f, 26.11f, 25.87f, 26.20f, 26.23f, + 26.19f, 25.87f, 25.97f, 26.09f, 24.77f, 24.22f, 23.71f, 22.10f, + 19.84f, 17.60f, 15.36f, 12.99f, 9.54f, 5.35f, 1.71f, -1.94f, + -4.15f, -6.16f, -7.93f, -9.10f, -10.01f, -10.25f, -10.85f, -11.74f, + -11.98f, -10.78f, -9.65f, -8.89f, -8.14f, -6.85f, -5.94f, -5.37f, + -4.28f, -3.38f, -3.36f, -3.40f, -2.81f, -2.36f, -1.04f, -0.18f, + 1.08f, 2.15f, 4.19f, 5.51f, 7.33f, 8.64f, 10.46f, 11.89f, + 13.42f, 14.83f, 17.11f, 19.12f, 20.76f, 21.88f, 22.35f, 22.86f, + 22.73f, 22.13f, 21.10f, 20.53f, 18.65f, 17.09f, 15.47f, 14.76f, + 14.02f, 12.94f, 11.98f, 10.29f, 8.11f, 5.83f, 4.23f, 2.42f, + 1.03f, -1.80f, -4.01f, -7.93f, -11.79f, -15.90f, -20.22f, -23.04f, + -25.66f, -28.80f, -30.45f, -32.11f, -33.17f, -33.97f, -34.68f, -34.26f, + -34.18f, -33.78f, -32.47f, -31.11f, -29.40f, -27.89f, -27.05f, -26.72f, + -25.59f, -24.86f, -23.61f, -22.97f, -22.89f, -21.81f, -20.78f, -19.23f, + -17.74f, -15.09f, -13.18f, -10.51f, -8.18f, -5.89f, -3.35f, -0.02f, + 2.02f, 3.87f, 6.37f, 9.11f, 12.33f, 14.63f, 17.12f, 19.12f, + }; + memcpy(cogging_table, COGGING_TABLE_FWD, sizeof(COGGING_TABLE_FWD)); + memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV)); + cogging_comp_enabled = true; + FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage; FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage; FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage; @@ -109,50 +354,6 @@ u8 FOC_Controller::init() { */ FOC_Controller::FOC_Controller() { ; } -// /** -// * @brief 设置电机输出 -// * -// * @param Uq: q轴电压 -// * @param Ud: d轴电压 -// * @param angle_el: 电机轴角度 -// */ -// inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) { -// // 计算电气角度 -// float angle_el = fmod(shaft_angle * PP, 360.0f); - -// // 帕克逆变换 (直接计算，避免函数调用开销) -// float sin_el, cos_el; -// arm_sin_cos_f32(angle_el, &sin_el, &cos_el); -// float Ualpha = Ud * cos_el - Uq * sin_el; -// float Ubeta = Ud * sin_el + Uq * cos_el; - -// // 克拉克逆变换 (使用预计算常量优化) -// Ua = Ualpha + v_half; -// Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half; -// Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half; - -// if (Ua > v_safe) -// Ua = v_safe; -// else if (Ua < -v_safe) -// Ua = -v_safe; -// if (Ub > v_safe) -// Ub = v_safe; -// else if (Ub < -v_safe) -// Ub = -v_safe; -// if (Uc > v_safe) -// Uc = v_safe; -// else if (Uc < -v_safe) -// Uc = -v_safe; - -// // 设置PWM输出 -// float voltage_exchange = limit_voltage / v_safe; -// duty_a = Ua / voltage_power_supply * voltage_exchange; -// duty_b = Ub / voltage_power_supply * voltage_exchange; -// duty_c = Uc / voltage_power_supply * voltage_exchange; - -// Set_3Pwm(duty_a, duty_b, duty_c); -// } - /** * @brief 使用SVPWM算法设置电机输出 * @@ -418,7 +619,9 @@ float FOC_Controller::velocity_open_loop() { // 因为在开环控制中，Ud 始终为 0，只控制 Uq const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f, 2.0f, voltage_power_supply * 0.5f); - setSVPWM_PhaseVoltage(Uq, 0.0f); + // 齿槽转矩补偿 + float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); + setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f); return limit_voltage; } @@ -436,8 +639,10 @@ float FOC_Controller::angle_control() { shaft_angle = KTH7823.getcurrent_sensorangle(); rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); + // 齿槽转矩补偿 + float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); // 设置相电压 - setSVPWM_PhaseVoltage(Uq, 0.0f); + setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f); return shaft_angle; } @@ -465,8 +670,10 @@ float FOC_Controller::velocity_control() { // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); + // 齿槽转矩补偿 + float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); // 设置相电压 - setSVPWM_PhaseVoltage(Uq, 0.0f); + setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f); rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); @@ -487,8 +694,10 @@ float FOC_Controller::current_control() { // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); CAN0_transmiter.testfloat_union.f = shaft_angle; + // 齿槽转矩补偿 + float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); // 设置相电压 - setSVPWM_PhaseVoltage(Uq, 0.0f); + setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f); return 1; } @@ -529,25 +738,17 @@ float FOC_Controller::angle_control_with_velocity_limit() { last_velocity_error = velocity_error; float Uq = vel_inVelAngloop(velocity_error)* DIR; - CAN0_transmiter.testfloat_union.f = Uq; - - // // 实现力度控制：如果电流超过目标电流，则限制输出 - // if (abs(rt_current) > abs(target_current)) { - // // 当实际电流超过目标电流时，减小输出以限制电流 - // // 可以采用比例限制或直接限制输出电压的方式 - // float current_ratio = abs(target_current) / abs(rt_current); - // if (current_ratio < 1.0f) { - // Uq *= current_ratio; // 按比例缩小输出电压 - // } - // } // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); + CAN0_transmiter.testfloat_union.f = shaft_angle; // 获取当前实际电流值 rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); + // 齿槽转矩补偿 + float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); // 设置相电压 - setSVPWM_PhaseVoltage(Uq, 0.0f); + setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f); return shaft_angle; } @@ -561,14 +762,12 @@ float FOC_Controller::angle_control_with_velocity_limit() { */ float FOC_Controller::MIT_Mode() { // 第一层：角度环（外环） - // 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度 float angle_error = target_angle - rt_angle; ang_inMITloop.set_P(ang_MIT_p); float target_velocity = - ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射 + ang_inMITloop(angle_error); // 第二层：速度环（中环） - // 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流 float velocity_error = (target_velocity - rt_velocity)* DIR; // 目标速度变化检测：target 大幅变化时清积分 @@ -582,11 +781,9 @@ float FOC_Controller::MIT_Mode() { float target_current = vel_inMITloop(velocity_error); // 第三层：电流环（内环） - // 计算当前实际电流 rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); - // 通过电流环PID控制器计算输出电压 float current_error = target_current - rt_current; current_inMITloop.set_P(cur_MIT_p); float Uq = current_inMITloop(current_error); @@ -594,8 +791,10 @@ float FOC_Controller::MIT_Mode() { // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); + // 齿槽转矩补偿 + float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); // 设置相电压 - setSVPWM_PhaseVoltage(Uq, 0.0f); + setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f); return shaft_angle; } diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index f87a46e..064b2d8 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -70,7 +70,38 @@ class FOC_Controller { void shut_down_thermal(void); // 过热关机 void resume_from_thermal(void); // 过热恢复 void fast_voltage_check(void); // 快速电压检测（100us周期调用） - + + // 齿槽转矩补偿 + bool cogging_comp_enabled = false; + float cogging_comp_scale = COGGING_COMP_SCALE; + float cogging_table[COGGING_TABLE_SIZE] = {0}; // 齿槽补偿表-正转 (mA) + float cogging_table_rev[COGGING_TABLE_SIZE] = {0}; // 齿槽补偿表-反转 (mA) + inline float get_cogging_compensation(float angle_el); + + // === 校准相关（DEBUG，平时不启用）=== + // 启用方法：config.h 中 FOC_DEBUG 改为 1 + #if FOC_DEBUG + volatile bool cogging_calib_request = false; + void calibrate_cogging_tick(void); + enum class CogCalibState : u8 { + IDLE = 0, + SETTLING, + SAMPLING, + STOPPING, + DONE + }; + CogCalibState cog_calib_state = CogCalibState::IDLE; + u8 cog_calib_run = 0; + u32 cog_calib_sample_count = 0; + u32 cog_calib_settle_tick = 0; + u32 cog_calib_stop_tick = 0; + u8 cog_calib_saved_mode = 0; + float cog_calib_saved_vel = 0; + static float cog_calib_run_data[6][COGGING_TABLE_SIZE]; // 3正+3反 + static float cog_calib_accum[COGGING_TABLE_SIZE]; + static u32 cog_calib_count[COGGING_TABLE_SIZE]; + #endif // FOC_DEBUG + // 此处默认值不作数 PIDController vel_loop = PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki, diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index c4c38eb..60c191f 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -25,7 +25,6 @@ project: - file: device/adc.h - file: device/Timer.cpp - file: device/Timer.h - - file: device/test.cpp - file: device/motor.cpp - file: device/motor.h - group: Algorithm diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 8e815a2..5a89287 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -1153,6 +1153,12 @@ void CAN_transmiter::communication_check() { Angle_kalman.init(0); // 角度滤波 Vel_kalman.init(0); // 角度滤波 } + // 齿槽转矩校准 (first_byte bit4) — DEBUG，平时不启用 + #if FOC_DEBUG + if (first_byte >> 4 & 0x01) { + FOC.cogging_calib_request = true; + } + #endif // 休眠 if (eigth_byte & 0x80 >> 7) { app_prepare_deep_sleep(true); diff --git a/Robot_V2/device/test.cpp b/Robot_V2/device/test.cpp deleted file mode 100644 index 7752e2f..0000000 --- a/Robot_V2/device/test.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "adc.h" -#include "config.h" -#include - -// TEST 调试宏定义 -#if TEST_DEBUG -#define TEST_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__) -#else -#define TEST_DEBUG_PRINT(fmt, ...) ((void)0) -#endif - -constexpr int status_print = 0; -extern sampler sampler; -void test_1111() { - // 初始化 CAN 回环测试 - // can_loopback_init(); - // can_config(); - if (status_print) { - /* value print */ - TEST_DEBUG_PRINT(" the temperature data is %2.2f degrees Celsius\r\n", - sampler.get_mcu_temp()); - TEST_DEBUG_PRINT(" the drv_temperature data is %2.2f degrees Celsius\r\n", - sampler.get_drv_temp()); - TEST_DEBUG_PRINT(" the U current data is %2.2f mA\r\n", sampler.get_current('U')); - TEST_DEBUG_PRINT(" the V current data is %2.2f mA\r\n", sampler.get_current('V')); - TEST_DEBUG_PRINT(" the W current data is %2.2f mA\r\n", sampler.get_current('W')); - TEST_DEBUG_PRINT(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage()); - // TEST_DEBUG_PRINT(" the angle is %2.2f\r\n", tle5012b_read()); - - TEST_DEBUG_PRINT("\n----------------------------------------------------\n"); - } - // can_loopback_test(); -} \ No newline at end of file diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 293e976..1edb100 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -99,6 +99,7 @@ u8 hardware_init(void) { void do_task(u32 currentTasks) { busy_task_enter(); // DWT: 记录忙碌起始周期 cnt_test++; + // 按照优先级和重要性处理任务 if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 sampler.update_current(); @@ -147,6 +148,12 @@ void do_task(u32 currentTasks) { } } } + + // 齿槽转矩校准 tick（DEBUG，平时不启用） + #if FOC_DEBUG + FOC.calibrate_cogging_tick(); + #endif + if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新 mcu 温度传感器值 sampler.update_mcu_temp(); } diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index fd261ea..614bb25 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -6,18 +6,18 @@ // 如果使用 CMSIS DSP 库，需要定义 ARM_MATH_CM4 #define ARM_MATH_CM4 -#define EEP_DEBUG 1 -#define OTA_DEBUG 1 -#define SLEEP_DEBUG 1 -#define ANG_SENS_DEBUG 1 -#define CAN_DEBUG 1 -#define MAIN_DEBUG 1 -#define TEST_DEBUG 1 +#define EEP_DEBUG 0 +#define OTA_DEBUG 0 +#define SLEEP_DEBUG 0 +#define ANG_SENS_DEBUG 0 +#define CAN_DEBUG 0 +#define MAIN_DEBUG 0 +#define FOC_DEBUG 0 // 版本号定义 - 可根据实际项目进行调整 constexpr u16 SOFTWARE_VERSION_MAJOR = 26; constexpr u16 SOFTWARE_VERSION_MINOR = 5; -constexpr u16 SOFTWARE_VERSION_PATCH = 17; +constexpr u16 SOFTWARE_VERSION_PATCH = 19; constexpr u16 SOFTWARE_VERSION_BUILD = 2; constexpr u16 HARDWARE_VERSION_MAJOR = 25; @@ -232,6 +232,10 @@ constexpr int TIMER1_DEFAULT_PULSE = 0; constexpr int TIMER1_CH3_COMPARE_VALUE = (TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 41% 位置触发ADC采样 +// 齿槽转矩补偿 +constexpr u16 COGGING_TABLE_SIZE = 128; // 每电周期采样点数 +constexpr float COGGING_COMP_SCALE = 0.005f; // 补偿换算系数 V/mA，40mA × 0.005 = 0.2V + // 电压检测参数（基于跌幅检测，不依赖固定额定电压） // 快速检测层（fast_voltage_check，100us周期）： // - 使用峰值检测算法跟踪当前额定电压（vbus_reference）