feat(FOC): 添加角度同步Uq前馈补偿和速度环优化

- 实现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

@@ -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);
 	// 获取当前实际电流值

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);
 };
 
 // 定义函数指针类型

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;

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

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;
 
 	// 如果快速检测已触发紧急关机，执行保存

backup_pre_tasks_split_20260524_011314/Robot_V2.csolution.yml

@@ -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

backup_pre_tasks_split_20260524_011314/Robot_V2/initial.h

@@ -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 <cstdint>
-#include <cstdio>
-
-// 任务标志定义
-/**
- * @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

backup_pre_tasks_split_20260524_011314/Robot_V2/main.cpp

@@ -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: 累加忙碌周期
-}

backup_pre_tasks_split_20260524_011314/Robot_V2/main.h

@@ -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 <cstdint>
-
-
-/**
- * @brief 根据任务标志调度周期性业务任务
- * @param[in] currentTasks 参数
- * @return void
- */
-void do_task(u32 currentTasks);
-/**
- * @brief 初始化应用层使用的硬件与软件模块
- * @return u8 返回值
- */
-u8 hardware_init(void);
-
-#endif /* MAIN_H */