From b71fba2016b67efa9fd72ae4765bc6b80197c7a0 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sun, 24 May 2026 04:56:33 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E5=90=8C=E6=AD=A5Uq=E5=89=8D=E9=A6=88=E8=A1=A5?= =?UTF-8?q?=E5=81=BF=E5=92=8C=E9=80=9F=E5=BA=A6=E7=8E=AF=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 实现update_angle_sync_uq_feedforward函数，支持mode2和mode3的角度同步 Uq前馈补偿，按物理360°查表学习负载/阻力差异 - 实现get_mode3_velocity_p函数，mode3低速时降低速度环P参数减少抖动 - 在velocity_control和angle_control_with_velocity_limit中集成Uq前馈补偿 - 优化normalizeAngle360函数，大角度时先扣整圈数避免while循环过多 - 调整配置参数，降低DEFAULT_vel_loop_p并添加详细的Uq前馈补偿配置 - 更新电源电压参数使用本周期采样值，避免电压滞后影响 --- Robot_V2/Algorithm/FOC.cpp | 154 +++++++++-- Robot_V2/Algorithm/FOC.h | 5 + Robot_V2/device/ang_sensor.h | 8 +- Robot_V2/sys/config.h | 31 ++- Robot_V2/sys/health.cpp | 4 +- .../Robot_V2.csolution.yml | 52 ---- .../Robot_V2/initial.h | 182 ------------- .../Robot_V2/main.cpp | 252 ------------------ .../Robot_V2/main.h | 52 ---- 9 files changed, 172 insertions(+), 568 deletions(-) delete mode 100644 backup_pre_tasks_split_20260524_011314/Robot_V2.csolution.yml delete mode 100644 backup_pre_tasks_split_20260524_011314/Robot_V2/initial.h delete mode 100644 backup_pre_tasks_split_20260524_011314/Robot_V2/main.cpp delete mode 100644 backup_pre_tasks_split_20260524_011314/Robot_V2/main.h diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 52817f4..cab1965 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -334,6 +334,107 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, // return I_q * 1000 - KE_FACTOR * rt_velocity; } + +/** + * @brief 角度同步 Uq 前馈更新/输出。 + * @note 只在稳定同向巡航时输出和学习；停止、低速、反向、明显刹车/超速时立即退出。 + */ +float FOC_Controller::update_angle_sync_uq_feedforward( + u8 mode, float table[], u16 table_size, float mech_angle, + float target_vel, float actual_vel, float velocity_error) { + if (table_size == 0U) return 0.0f; + + bool enable = false; + bool learn_enable = false; + float min_target = 0.0f; + float stop_speed = 0.0f; + float brake_err = 0.0f; + float learn_alpha = 0.0f; + float gain = 0.0f; + float max_ff = 0.0f; + float dir_sign = 1.0f; + + switch (mode) { + case 2: + enable = MODE2_UQ_FF_ENABLE; + learn_enable = MODE2_UQ_FF_LEARN_ENABLE; + min_target = MODE2_UQ_FF_MIN_TARGET; + stop_speed = MODE2_UQ_FF_STOP_SPEED; + brake_err = MODE2_UQ_FF_BRAKE_ERR; + learn_alpha = MODE2_UQ_FF_LEARN_ALPHA; + gain = MODE2_UQ_FF_GAIN; + max_ff = MODE2_UQ_FF_MAX; + dir_sign = 1.0f; + break; + case 3: + enable = MODE3_UQ_FF_ENABLE; + learn_enable = MODE3_UQ_FF_LEARN_ENABLE; + min_target = MODE3_UQ_FF_MIN_TARGET; + stop_speed = MODE3_UQ_FF_STOP_SPEED; + brake_err = MODE3_UQ_FF_BRAKE_ERR; + learn_alpha = MODE3_UQ_FF_LEARN_ALPHA; + gain = MODE3_UQ_FF_GAIN; + max_ff = MODE3_UQ_FF_MAX; + dir_sign = DIR; + break; + default: + return 0.0f; + } + + const bool active = (fabsf(target_vel) > stop_speed) && + (fabsf(actual_vel) > stop_speed) && + ((target_vel * actual_vel) > 0.0f) && + ((velocity_error * dir_sign) > -brake_err); + + float angle = normalizeAngle360(mech_angle); + float index_f = angle / 360.0f * (float)table_size; + u16 idx0 = (u16)index_f; + if (idx0 >= table_size) idx0 = table_size - 1U; + u16 idx1 = (idx0 + 1U) % table_size; + float frac = index_f - (float)idx0; + + float ff = table[idx0] * (1.0f - frac) + table[idx1] * frac; + if (ff > max_ff) ff = max_ff; + else if (ff < -max_ff) ff = -max_ff; + + if (learn_enable && active && fabsf(target_vel) > min_target) { + float ff_target = velocity_error * dir_sign * gain; + if (ff_target > max_ff) ff_target = max_ff; + else if (ff_target < -max_ff) ff_target = -max_ff; + table[idx0] += learn_alpha * (ff_target - table[idx0]); + } + + return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f; +} +/** + * @brief mode3 低速/静止附近降低速度内环 P，运行速度升高后平滑恢复。 + */ +float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) { + float vel_p = vel_angloop_p; + if (target_vel < -120.0f) { + vel_p = vel_angloop_p * 2.0f; + } else if (target_vel < -30.0f) { + vel_p = vel_angloop_p * 1.5f; + } + + if (MODE3_LOW_SPEED_P_SCALE_ENABLE) { + float abs_target = fabsf(target_vel); + float abs_speed = fabsf(actual_vel); + float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed; + float p_scale = 1.0f; + if (speed_ref <= MODE3_LOW_SPEED_P_LOW) { + p_scale = MODE3_LOW_SPEED_P_MIN_SCALE; + } else if (speed_ref < MODE3_LOW_SPEED_P_FULL) { + p_scale = MODE3_LOW_SPEED_P_MIN_SCALE + + (1.0f - MODE3_LOW_SPEED_P_MIN_SCALE) * + (speed_ref - MODE3_LOW_SPEED_P_LOW) / + (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW); + } + vel_p *= p_scale; + } + return vel_p; +} + /** * @brief 电机控制类初始化函数 * @@ -711,23 +812,27 @@ float FOC_Controller::angle_control() { * @return float: 当前速度 */ float FOC_Controller::velocity_control() { - // 速度误差计算 + // 获取电机轴角度：mode2 Uq 前馈补偿需要用物理 360° 角度查表。 + shaft_angle = KTH7823.getcurrent_sensorangle(); + + // mode2 速度环：真实速度波动保留在反馈里，PID 继续看真实 rt_velocity。 float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO; - // 目标速度突变检测：target 大幅变化时清积分 - static float last_target_vel = 0.0f; - const float target_delta = target_velocity - last_target_vel; - if (fabsf(target_delta) > 5.0f) { - vel_loop.reset_integral(); - } - last_target_vel = target_velocity; + // mode2 使用独立速度环参数。 + vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd); + vel_loop.set_limit(limit_voltage); + float Uq_pid = vel_loop(velocity_error); - // PID 控制计算 - float Uq = vel_loop(velocity_error); + static float uq_ff_table[MODE2_UQ_FF_TABLE_SIZE] = {0.0f}; + float Uq_ff = update_angle_sync_uq_feedforward( + 2, uq_ff_table, MODE2_UQ_FF_TABLE_SIZE, shaft_angle, + target_velocity, rt_velocity, velocity_error); + + float Uq = Uq_pid + Uq_ff; + if (Uq > limit_voltage) Uq = limit_voltage; + else if (Uq < -limit_voltage) Uq = -limit_voltage; CAN0_transmiter.testfloat_union.f = Uq; - // 获取电机轴角度 - shaft_angle = KTH7823.getcurrent_sensorangle(); // 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用） float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle); // 设置相电压 @@ -779,14 +884,7 @@ float FOC_Controller::angle_control_with_velocity_limit() { float target_velocity = ang_inVelAngloop(angle_error); // 使用速度PID控制器计算最终输出电压 - // 根据目标速度大小，选择不同的PID参数 - if (target_velocity < -120.0f) { - vel_inVelAngloop.set_P(vel_angloop_p * 2.0f); - } else if (target_velocity < -30.0f) { - vel_inVelAngloop.set_P(vel_angloop_p * 1.5f); - } else { - vel_inVelAngloop.set_P(vel_angloop_p); - } + vel_inVelAngloop.set_P(get_mode3_velocity_p(target_velocity, rt_velocity)); float velocity_error = target_velocity - rt_velocity; // 目标速度突变检测：target 大幅变化时清积分 @@ -796,14 +894,20 @@ float FOC_Controller::angle_control_with_velocity_limit() { } last_vang_target_vel = target_velocity; - static float last_velocity_error = 0.0f; - last_velocity_error = velocity_error; + float Uq_pid = vel_inVelAngloop(velocity_error) * DIR; - float Uq = vel_inVelAngloop(velocity_error)* DIR; - - // 获取电机轴角度 + // 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。 shaft_angle = KTH7823.getcurrent_sensorangle(); + static float mode3_uq_ff_table[MODE3_UQ_FF_TABLE_SIZE] = {0.0f}; + float Uq_ff = update_angle_sync_uq_feedforward( + 3, mode3_uq_ff_table, MODE3_UQ_FF_TABLE_SIZE, shaft_angle, + target_velocity, rt_velocity, velocity_error); + + float Uq = Uq_pid + Uq_ff; + if (Uq > limit_voltage) Uq = limit_voltage; + else if (Uq < -limit_voltage) Uq = -limit_voltage; CAN0_transmiter.testfloat_union.f = shaft_angle; + // 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用） float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle); // 获取当前实际电流值 diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 80857d6..376af6b 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -310,6 +310,11 @@ class FOC_Controller { * @return float 返回值 */ float cal_Iq_Id(float current_a, float current_b, float current_c, float angle_el); + float update_angle_sync_uq_feedforward(u8 mode, float table[], + u16 table_size, float mech_angle, + float target_vel, float actual_vel, + float velocity_error); + float get_mode3_velocity_p(float target_vel, float actual_vel); }; // 定义函数指针类型 diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index 6746769..5844342 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -96,10 +96,14 @@ class angle_sensor { }; /** * @brief 快速角度归一化到 [0, 360) - * 用 while 减法替代 fmodf，在角度变化量很小时（电机控制场景） - * 通常只需 0~1 次减法，远快于 fmodf 的 ~30-50 周期开销。 + * 小角度保留 while 快路径；大角度先一次性扣掉整圈数，避免累计角很大时 while 循环过多。 + * 不依赖 fmodf，减少嵌入式数学库开销和链接风险。 */ inline float normalizeAngle360(float angle) { + if (angle >= 720.0f || angle <= -360.0f) { + int turns = (int)(angle / 360.0f); + angle -= (float)turns * 360.0f; + } while (angle >= 360.0f) angle -= 360.0f; while (angle < 0.0f) angle += 360.0f; return angle; diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index ddcb335..d48346b 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -126,16 +126,45 @@ constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P constexpr float current_loop_Ki = 0.55f; // 电流 P constexpr float current_loop_Kd = 3e-4f; // 电流 P -constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度 P +constexpr float DEFAULT_vel_loop_p = 0.005f / 100 * 12; // 速度 P constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12; constexpr float vel_loop_Tu = 17.0f * 1e-3f; constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu; constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2; +// mode2 角度同步 Uq 前馈补偿：固定 Uq 下的真实速度波动不应从反馈中滤掉， +// 而应按物理 360° 提前补偿负载/阻力差异，降低速度 PID 的周期性纠偏量。 +constexpr bool MODE2_UQ_FF_ENABLE = true; +constexpr bool MODE2_UQ_FF_LEARN_ENABLE = true; +constexpr u16 MODE2_UQ_FF_TABLE_SIZE = 180; +constexpr float MODE2_UQ_FF_LEARN_ALPHA = 0.01f; // 越小越慢，避免误学瞬态 +constexpr float MODE2_UQ_FF_MIN_TARGET = 1.5f; // 目标速度过低时不学习 +constexpr float MODE2_UQ_FF_GAIN = 0.003f; // 速度误差(deg/s)到前馈电压(V)的学习比例 +constexpr float MODE2_UQ_FF_MAX = 3.5f; // 前馈补偿电压限幅(V) +constexpr float MODE2_UQ_FF_STOP_SPEED = 2.0f; // 目标/实际速度低于该值时不输出前馈 +constexpr float MODE2_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error + +// mode3 角度同步 Uq 前馈补偿：单独建表，避免和 mode2 互相污染。 +constexpr bool MODE3_UQ_FF_ENABLE = true; +constexpr bool MODE3_UQ_FF_LEARN_ENABLE = true; +constexpr u16 MODE3_UQ_FF_TABLE_SIZE = 180; +constexpr float MODE3_UQ_FF_LEARN_ALPHA = 0.01f; +constexpr float MODE3_UQ_FF_MIN_TARGET = 2.5f; // mode3 内部目标速度过低时不学习 +constexpr float MODE3_UQ_FF_GAIN = 0.3f; // mode3 速度误差未乘减速比，故增益独立于 mode2 +constexpr float MODE3_UQ_FF_MAX = 3.5f; +constexpr float MODE3_UQ_FF_STOP_SPEED = 2.0f; // mode3 内部目标/实际速度低于该值时不输出前馈 +constexpr float MODE3_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error + constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P constexpr float vel_angloop_Ki = 5.0f; // 速度 速度 - 角度 I constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D +// mode3 低速/静止附近速度 P 降低，减少速度反馈噪声、齿槽和摩擦导致的来回拉扯。 +constexpr bool MODE3_LOW_SPEED_P_SCALE_ENABLE = true; +constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.2f; +constexpr float MODE3_LOW_SPEED_P_LOW = 1.0f; +constexpr float MODE3_LOW_SPEED_P_FULL = 5.0f; + constexpr float DEFAULT_angle_angloop_p = 150.0f / 100 * 12; // 角度 速度 - 角度 P constexpr float ang_angloop_Ki = 0.00f; // 角度 速度 - 角度 I diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 75d50cd..42a13e3 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -115,8 +115,8 @@ void power_manage(void) { sampler.update_vbus_voltage(); float vbus = sampler.get_vbus_voltage(); - // 更新FOC的电源电压参数 - FOC.update_power(last_vbus_slow); + // 更新FOC的电源电压参数：使用本周期刚采到的 vbus，避免上一拍电压滞后放大速度/电流耦合。 + FOC.update_power(vbus); last_vbus_slow = vbus; // 如果快速检测已触发紧急关机，执行保存 diff --git a/backup_pre_tasks_split_20260524_011314/Robot_V2.csolution.yml b/backup_pre_tasks_split_20260524_011314/Robot_V2.csolution.yml deleted file mode 100644 index 54608ea..0000000 --- a/backup_pre_tasks_split_20260524_011314/Robot_V2.csolution.yml +++ /dev/null @@ -1,52 +0,0 @@ -# A solution is a collection of related projects that share same base configuration. -solution: - created-for: CMSIS-Toolbox@2.12.0 - cdefault: # use CMSIS-Toolbox default Compiler options - - # Compilers that can be selected - select-compiler: - - compiler: AC6 - - compiler: GCC - - compiler: IAR - - compiler: CLANG - - # List of miscellaneous tool-specific controls - misc: - - for-compiler: AC6 # GDB requires DWARF 5, remove when using uVision Debugger - C-CPP: - - -gdwarf-5 - ASM: - - -gdwarf-5 - - # List the packs that define the device and/or board. - packs: - - pack: GigaDevice::GD32C10x_DFP - - pack: ARM::CMSIS - - # List different hardware targets that are used to deploy the solution. - target-types: - - type: GD32C103CB - device: GD32C103CB - target-set: - - set: - images: - - project-context: Robot_V2.Release - debugger: - name: ST-Link@pyOCD - clock: 10000000 - protocol: swd - - # List of different build configurations. - build-types: - - type: Debug - debug: on - optimize: none - - - type: Release - debug: off - optimize: balanced - - # List related projects. - projects: - - project: Robot_V2/Robot_V2.cproject.yml - compiler: AC6 diff --git a/backup_pre_tasks_split_20260524_011314/Robot_V2/initial.h b/backup_pre_tasks_split_20260524_011314/Robot_V2/initial.h deleted file mode 100644 index bd597e0..0000000 --- a/backup_pre_tasks_split_20260524_011314/Robot_V2/initial.h +++ /dev/null @@ -1,182 +0,0 @@ -/** - * @file sys/initial.h - * @brief 系统服务与平台支持源文件 - * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。 - */ - -#ifndef _INITIAL_H -#define _INITIAL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "config.h" -#include "gd32c10x_eval.h" -#include -#include - -// 任务标志定义 -/** - * @brief 匿名枚举 枚举定义 - */ -typedef enum { - TASK_MOTOR_CONTROL = 0, - TASK_CURRENT_UPDATE, - TASK_VELOCITY_UPDATE, - TASK_ANGLE_UPDATE, - TASK_MCU_TEMP_UPDATE, - TASK_DRV_TEMP_UPDATE, - TASK_VBUS_UPDATE, - TASK_THERMAL_CONTROL, - TASK_CAN_TRANSMIT, - TASK_MAGNETIC_CHECK, - TASK_COUNT -} SystemTask; - -/** - * @brief rcu_config 函数 - * @return void - */ -void rcu_config(void); -static volatile uint32_t timer_delay_count = 0; // volatile 很重要！ -/** - * @brief fputc 函数 - * @param[in] ch 参数 - * @param[in] f 参数 - * @return int 返回值 - */ -int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 - -/** - * @brief 初始化 USART0 调试串口 - * @return u8 返回值 - */ -u8 USART0_init(void); -/** - * @brief 初始化独立看门狗 - * @param[in] time 参数 - * @return u8 返回值 - */ -u8 watchdog_init(u32 time); -/** - * @brief 获取复位原因 - * @return u16 返回值 - */ -u16 get_reset_cause(void); -/** - * @brief 初始化未使用引脚状态 - * @return void - */ -void unused_pin_init(); -/** - * @brief 初始化 OTA 与 A/B 分区状态 - * @return void - */ -void OTA_initial(void); - -// 任务管理结构 -typedef struct { - volatile u32 taskFlags; // 任务标志位(每个位代表一个任务) -} TaskScheduler; - -// 版本管理类 -/** - * @brief VersionManager 类或结构体 - */ -class VersionManager { - public: - // 版本信息结构体 - typedef struct { - u16 major; - u16 minor; - u16 patch; - u16 build; - } VersionInfo; - - // 构造函数 - VersionManager(); - - // 获取软件版本 - /** - * @brief 获取 getSoftwareVersion 对应的数据或状态 - * @return VersionInfo 返回值 - */ - VersionInfo getSoftwareVersion() const; - - // 获取硬件版本 - /** - * @brief 获取 getHardwareVersion 对应的数据或状态 - * @return VersionInfo 返回值 - */ - VersionInfo getHardwareVersion() const; - - // 获取BOOT版本 - /** - * @brief 获取 getBootVersion 对应的数据或状态 - * @return VersionInfo 返回值 - */ - VersionInfo getBootVersion() const; - - // 设置软件版本 - /** - * @brief 设置 setSoftwareVersion 对应的参数或状态 - * @param[in] major 参数 - * @param[in] minor 参数 - * @param[in] patch 参数 - * @param[in] build 参数 - * @return void - */ - void setSoftwareVersion(u16 major, u16 minor, u16 patch, u16 build); - - // 设置硬件版本 - /** - * @brief 设置 setHardwareVersion 对应的参数或状态 - * @param[in] major 参数 - * @param[in] minor 参数 - * @param[in] patch 参数 - * @param[in] build 参数 - * @return void - */ - void setHardwareVersion(u16 major, u16 minor, u16 patch, u16 build); - - // 设置BOOT版本 - /** - * @brief 设置 setBootVersion 对应的参数或状态 - * @param[in] major 参数 - * @param[in] minor 参数 - * @param[in] patch 参数 - * @param[in] build 参数 - * @return void - */ - void setBootVersion(u16 major, u16 minor, u16 patch, u16 build); - - // 将版本信息转换为字符串 - char *versionToString(const VersionInfo &version, char *buffer, - size_t bufSize) const; - - // 打印所有版本信息 - /** - * @brief printVersions 函数 - * @return void - */ - void printVersions() const; - - // 通过CAN发送版本信息 - /** - * @brief sendVersionInfoViaCan 函数 - * @return void - */ - void sendVersionInfoViaCan() const; - - private: - VersionInfo softwareVersion; - VersionInfo hardwareVersion; - VersionInfo bootVersion; -}; - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/backup_pre_tasks_split_20260524_011314/Robot_V2/main.cpp b/backup_pre_tasks_split_20260524_011314/Robot_V2/main.cpp deleted file mode 100644 index e364fe9..0000000 --- a/backup_pre_tasks_split_20260524_011314/Robot_V2/main.cpp +++ /dev/null @@ -1,252 +0,0 @@ -/** - * @file main.cpp - * @brief 应用程序主流程源文件 - * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。 - */ - -#include "main.h" -#include "Can.h" -#include "FOC.h" -#include "Filter.h" -#include "Timer.h" -#include "adc.h" -#include "ang_sensor.h" -#include "flash.h" -#include "health.h" -#include "initial.h" -#include "systick.h" - -// MAIN 调试宏定义 -#if MAIN_DEBUG -/** @brief MAIN_DEBUG_PRINT 宏定义 */ -#define MAIN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__) -#else -/** @brief MAIN_DEBUG_PRINT 宏定义 */ -#define MAIN_DEBUG_PRINT(fmt, ...) ((void)0) -#endif - -extern CAN_transmiter CAN0_transmiter; // CAN 初始化 -extern sampler sampler; // 采样器 -extern angle_sensor KTH7823; // 角度传感器 -extern EncoderVelocityKalman Vel_kalman; // 速度滤波 -extern EncoderAngleKalman Angle_kalman; // 角度滤波 -extern ThermalController thermalController; // 热量控制器 -extern FOC_Controller FOC; // FOC 控制 -extern FlashEEPROM flashEEP; // EEPROM 类 -extern VersionManager Version; - -// 主任务调度共享位图：TIMER2 ISR 置位任务，main 循环短关中断后取快照并清位。 -TaskScheduler scheduler = {0}; -// 电机 50us 控制周期计数：主循环电机任务递增，观测器/计时逻辑读取。 -u32 motor_runtime_50us = 0; -// 负载检测/通信心跳计数：主循环递增，CAN 周期发送任务清零。 -u32 cnt_test = 0; - -// 函数指针数组，包含各种控制模式对应的函数 -static FOCFunctionPtr control_functions[] = { - &FOC_Controller::drv_hold, // 0: 占空比置 - &FOC_Controller::angle_control, // 1: 位置闭环控制 - &FOC_Controller::velocity_control, // 2: 速度闭环控制 - &FOC_Controller:: - angle_control_with_velocity_limit, // 3: - // 角度闭环控制，带速度限制 - &FOC_Controller::MIT_Mode, // 4: MIT 模式 - &FOC_Controller::current_control, // 5: 电流闭环控制 - &FOC_Controller::velocity_open_loop, // 6: 速度开环控制 - &FOC_Controller::PWM_out, // 0: 占空比置 -}; - -/** - * @brief 判断任务标志中是否包含指定任务 - * @param[in] currentTasks 当前任务位图快照 - * @param[in] task 待检查任务枚举 - * @return bool true 表示该任务需要执行 - */ -static bool has_task(u32 currentTasks, SystemTask task) { - return (currentTasks & (1u << task)) != 0U; -} - -/** - * @brief 控制模式变化时复位所有控制环，避免积分项跨模式继承 - * @return void - */ -static void reset_control_loops_on_mode_change(void) { - static int last_control_mode = -1; - if (FOC.control_mode == last_control_mode) return; - - FOC.vel_loop.reset(); - FOC.angle_loop.reset(); - FOC.current_loop.reset(); - FOC.vel_inVelAngloop.reset(); - FOC.ang_inVelAngloop.reset(); - FOC.ang_inMITloop.reset(); - FOC.vel_inMITloop.reset(); - FOC.current_inMITloop.reset(); - last_control_mode = FOC.control_mode; -} - -/** - * @brief 执行 50us 电机控制任务，包含快速电压检测和控制函数派发 - * @return void - */ -static void run_motor_control_task(void) { - motor_runtime_50us++; - - static u8 vcheck_div = 0; - // 快速电压检测每 100us 执行一次，避免每 50us 都读取处理。 - if (++vcheck_div >= 2u) { - vcheck_div = 0; - FOC.fast_voltage_check(); - } - - if (FOC.control_mode >= 8) return; - - // 模式合法时再派发控制函数，防止数组越界。 - reset_control_loops_on_mode_change(); - if (!FOC.is_shut_down) { - (FOC.*control_functions[FOC.control_mode])(); - } else { - FOC.drv_stop(); - } -} - -/** - * @brief 执行高频采样和电机控制相关任务 - * @param[in] currentTasks 当前任务位图快照 - * @return void - */ -static void run_fast_sample_tasks(u32 currentTasks) { - if (has_task(currentTasks, TASK_CURRENT_UPDATE)) { - sampler.update_current(); - } - if (has_task(currentTasks, TASK_ANGLE_UPDATE)) { - KTH7823.update(); - FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()); - } - if (has_task(currentTasks, TASK_VELOCITY_UPDATE)) { - FOC.rt_velocity = Angle_kalman.getVelocity(); - } - if (has_task(currentTasks, TASK_MOTOR_CONTROL)) { - run_motor_control_task(); - } -} - -/** - * @brief 执行温度、热管理、磁检测和母线电压等低频维护任务 - * @param[in] currentTasks 当前任务位图快照 - * @return void - */ -static void run_housekeeping_tasks(u32 currentTasks) { - if (has_task(currentTasks, TASK_MCU_TEMP_UPDATE)) { - sampler.update_mcu_temp(); - } - if (has_task(currentTasks, TASK_DRV_TEMP_UPDATE)) { - sampler.update_drv_temp(); - } - if (has_task(currentTasks, TASK_THERMAL_CONTROL)) { - thermalController.update(); - } - if (has_task(currentTasks, TASK_MAGNETIC_CHECK)) { - angsens_MG_check(); - } - if (has_task(currentTasks, TASK_VBUS_UPDATE)) { - power_manage(); - } -} - -/** - * @brief 执行 CAN 周期发送任务并喂狗 - * @return void - */ -static void run_can_task(void) { - CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id(); - cnt_test = 0; - CAN0_transmiter.can0_send_flag = SET; - CAN0_transmiter.communication_check(); - fwdgt_counter_reload(); -} - -/** - * @brief 应用程序入口函数 - * @return int 返回值 - */ -int main(void) { - u8 init_status = hardware_init(); - - while (init_status) { // 初始化检测 - MAIN_DEBUG_PRINT("初始化失败\r\n"); - } - OTA_initial(); // OTA 初始化，A/B 分区处理 - - angsens_MG_check(); // 角度传感器磁感应强度测试 - watchdog_init(50); // 看门狗初始化 - - while (1) { - __disable_irq(); - u32 currentTasks = - scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性 - scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志 - __enable_irq(); - - if (currentTasks == 0U) { - idle_task_count(); - continue; - } - - do_task(currentTasks); - } - return 0; -} - -/** - * @brief 初始化应用层使用的硬件与软件模块 - * @return u8 返回值 - */ -u8 hardware_init(void) { - __enable_irq(); - systick_config(); // 系统时钟初始化 - unused_pin_init(); // 未使用引脚初始化 - USART0_init(); // 调试串口初始化 - User_timer_init(); // 任务 Timer 初始化 - - u8 init_status = 0; - init_status += KTH7823.init(); // 角度传感器初始化 - init_status += sampler.init(); // 采样器初始化 - - // EEPROM 初始化 (必须在 CAN 初始化之前，因为 CAN 会从 Flash 读取节点 ID) - flashEEP.init(); // EEPROM 初始化 - - // CAN 初始化 (会先设置默认节点 ID，然后从 Flash 加载已保存的节点 ID) - init_status += CAN0_transmiter.init(); // CAN 初始化 - - init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 - init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 - init_status += FOC.init(); // FOC 初始化 - return init_status; -} - -/** - * @brief 根据任务标志调度周期性业务任务 - * @param[in] currentTasks 参数 - * @return void - */ -void do_task(u32 currentTasks) { - busy_task_enter(); // DWT: 记录忙碌起始周期 - cnt_test++; - - // 高频任务先执行，保持电机控制优先级。 - run_fast_sample_tasks(currentTasks); - - // 齿槽转矩校准 tick（DEBUG，平时不启用） - #if FOC_DEBUG - FOC.calibrate_cogging_tick(); - #endif - - run_housekeeping_tasks(currentTasks); - - // CAN 发送放在最后，避免通信处理影响前面的实时控制任务。 - if (has_task(currentTasks, TASK_CAN_TRANSMIT)) { - run_can_task(); - } - busy_task_exit(); // DWT: 累加忙碌周期 -} \ No newline at end of file diff --git a/backup_pre_tasks_split_20260524_011314/Robot_V2/main.h b/backup_pre_tasks_split_20260524_011314/Robot_V2/main.h deleted file mode 100644 index 3992cf0..0000000 --- a/backup_pre_tasks_split_20260524_011314/Robot_V2/main.h +++ /dev/null @@ -1,52 +0,0 @@ -/*! - \file main.h - \brief the header file of main - - \version 2025-06-20, V1.5.1, firmware for GD32C10x -*/ - -/* - Copyright (c) 2024, GigaDevice Semiconductor Inc. - - Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, -this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - 3. Neither the name of the copyright holder nor the names of its -contributors may be used to endorse or promote products derived from this -software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON -ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef MAIN_H -#define MAIN_H -#include - - -/** - * @brief 根据任务标志调度周期性业务任务 - * @param[in] currentTasks 参数 - * @return void - */ -void do_task(u32 currentTasks); -/** - * @brief 初始化应用层使用的硬件与软件模块 - * @return u8 返回值 - */ -u8 hardware_init(void); - -#endif /* MAIN_H */