feat(PID): 调整速度与角度环PID参数

更新了FOC控制器中的速度环和角度环PID参数以改善控制响应，并微调电流环参数提升稳定性。

feat(Filter): 改进卡尔曼滤波器初始化逻辑及速度计算方式
引入系统运行时间计数器，修正编码器角度和速度卡尔曼滤波器的初始化流程，并基于实际时间间隔优化角速度估算精度。

refactor(Filter): 移动成员变量声明顺序
调整`EncoderAngleKalman`类中`last_angle`成员变量的访问权限和声明位置，增强封装性和可读性。

feat(Flash): 新增Flash EEPROM功能支持
添加FlashEEPROM类及相关头文件，支持电机角度等关键数据掉电保存，并在CAN通信复位时进行角度存储操作。

refactor(Timer): 优化定时器配置和中断优先级设置
删除冗余的TIMER3定义，修改TIMER1中断服务函数可见性，并调整NVIC子优先级分配，确保中断响应合理有序。

feat(AngleSensor): 完善角度传感器初始化与角度设定接口
重构角度传感器初始化方法并新增角度设置函数，支持从Flash读取初始角度值并与减速比结合应用。

feat(Health): 实现母线电压监测与保护策略
新增电源管理函数，在母线电压异常下降时自动关闭电机输出，防止因供电不足导致设备损坏。

Robot_V2.csolution.yml

@@ -30,7 +30,7 @@ solution:
       target-set:
         - set:
           images:
-            - project-context: Robot_V2.Debug
+            - project-context: Robot_V2.Release
           debugger:
             name: ST-Link@pyOCD
             port: 3333

Robot_V2/Algorithm/DengFOC.cpp

@@ -172,7 +172,7 @@ float FOC_Controlroller::velocity_open_loop() {
 	const float Uq = voltage_power_supply / 2.0f;
 
 	setPhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f =
+	// CAN0_transmiter.testfloat_union.f =
 		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
 
 	return limit_voltage;

Robot_V2/Algorithm/DengFOC.h

@@ -43,12 +43,12 @@ class FOC_Controlroller {
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
 
-	PIDController vel_loop = PIDController{0.13f, 0.4f, 2.8e-4, 1000000,
+	PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000,
 										   voltage_power_supply}; // 12V
-	PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
+	PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
 											 voltage_power_supply}; // 12V
 	PIDController current_loop =
-		PIDController{0.01f, 0.001f, 0, 100000, voltage_power_supply}; // 12V
+		PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
 
 	void setPhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);

Robot_V2/Algorithm/Filter.cpp

@@ -7,8 +7,9 @@
 
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
-EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
+extern u32 system_runtime_us;
-EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
+EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
+EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
 
 constexpr u8 SIZE_FLOAT = sizeof(float);
 
@@ -30,7 +31,6 @@ float medium_filter(float values[], u8 size) {
 	return sorted[size / 2]; // 返回中值
 }
 
-
 // EncoderVelocityKalman类的实现
 /*!
 	\brief      构造函数，初始化编码器速度卡尔曼滤波器参数
@@ -100,7 +100,6 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
 			 measurement_noise),
 	  last_angle(initial_angle), initialized(true) {}
 
-
 /*!
 	\brief      更新卡尔曼滤波器状态
 	\param[in]  measured_angle 测量的角度值 (degree)
@@ -127,16 +126,20 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
 	\brief      获取当前角度估计值
 	\retval     当前角度估计值 (degree)
 */
-float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*3.75e3; }
+float EncoderAngleKalman::getVelocity() const {
+	static u32 last_us100_counter = system_runtime_us;
+	float vel = (kalman.getState() - last_angle) * 1e4 *
+				(system_runtime_us - last_us100_counter);
+	last_us100_counter = system_runtime_us;
+	return vel;
+}
 
 /*!
 	\brief      设置过程噪声协方差
 	\param[in]  q 过程噪声协方差值
 	\retval     none
 */
-void EncoderAngleKalman::setProcessNoise(float q) {
+void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
-	kalman.setProcessNoise(q);
-}
 
 /*!
 	\brief      设置测量噪声协方差

Robot_V2/Algorithm/Filter.h

@@ -138,9 +138,9 @@ class EncoderAngleKalman {
   private:
 	KalmanFilter<float> kalman;
 	bool initialized;
+	float last_angle;
 
   public:
-	float last_angle;
 	EncoderAngleKalman(float initial_angle, float initial_error_covariance,
 					   float process_noise, float measurement_noise);
 

Robot_V2/RTE/Device/GD32C103CB/gd32c10x_fmc.c

@@ -33,6 +33,7 @@ OF SUCH DAMAGE.
 */
 
 #include "gd32c10x_fmc.h"
+#include "stdio.h"
 
 /* FMC mask */
 #define LOW_8BITS_MASK             ((uint32_t)0x000000FFU)
@@ -329,6 +330,7 @@ fmc_state_enum fmc_doubleword_program(uint32_t address, uint64_t data)
 fmc_state_enum fmc_word_program(uint32_t address, uint32_t data)
 {
     fmc_state_enum fmc_state = fmc_ready_wait(FMC_TIMEOUT_COUNT); 
+	printf("fmc_status: 0x%x\r\n", fmc_state);
 
     if(FMC_READY == fmc_state){
         /* set the PG bit to start program */

Robot_V2/Robot_V2.cproject.yml

@@ -12,6 +12,8 @@ project:
         - file: sys/initial.h
         - file: sys/health.h
         - file: sys/health.cpp
+        - file: sys/flash.h
+        - file: sys/flash.cpp
     - group: device
       files:
         - file: device/Can.cpp

Robot_V2/device/Can.cpp

@@ -3,6 +3,8 @@
 #include "Filter.h"
 #include "adc.h"
 #include "ang_sensor.h"
+#include "core_cmFunc.h"
+#include "flash.h"
 #include "gd32c10x.h"
 #include "gd32c10x_can.h"
 #include "gd32c10x_dma.h"
@@ -27,6 +29,7 @@ extern FOC_Controlroller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern EncoderVelocityKalman Vel_kalman;
+extern FlashEEPROM flashEEP;
 
 // 添加DMA相关变量
 dma_parameter_struct dma_rx_parameter;
@@ -628,9 +631,10 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
 	pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f));
 	pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f));
-	transmit_data[10] = (transmit_data[10] & ~0x07) |
+	transmit_data[10] =
+		(transmit_data[10] & ~0x07) |
 		((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) |
-						 ((ThermalStatus & 0x01) << 2));
+		 ((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x01) << 3));
 
 	pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
 	pack_float_to_bytes(transmit_data, 13, rt_angle_union);
@@ -692,8 +696,8 @@ void CAN_transmiter::communication_check() {
 			FOC.target_velocity = target_velocity_union.f;
 		} else if (receive_message.rx_efid == 0x00010301) {
 			motor_reset();
-		}
+			flashEEP.save_angle(666, false);
-		else {
+		} else {
 			printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
 		}
 		can0_receive_flag = RESET;

Robot_V2/device/Can.h

@@ -110,6 +110,7 @@ class CAN_transmiter {
 
 	bool MagStatus = 0;						   		// 磁状态码
 	bool ThermalStatus = 0;						   	// 热能状态码
+	bool EEPROMStatus = 0;							// EEPROM状态码
 	void communication_check();						// 通信检查
 };
 

Robot_V2/device/Timer.cpp

@@ -14,8 +14,6 @@
 
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
 constexpr int TIMER2_PERIOD = 99;	  // 1MHz / 100 = 100us
-constexpr int TIMER3_PRESCALER = 119; // 120MHz / 120 = 1MHz
-constexpr int TIMER3_PERIOD = 9999;	  // 1MHz / 10000 = 100Hz = 10ms
 
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
 constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
@@ -25,13 +23,13 @@ constexpr int TIMER1_CH2_PULSE = 0;
 constexpr int TIMER1_CH3_COMPARE_VALUE =
 	(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
 
-static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
+volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
 extern u16 adc_value[7];
 extern FOC_Controlroller FOC;
 extern TaskScheduler scheduler;
 extern CAN_transmiter CAN0_transmiter;
 // 添加系统运行时间计数器
-static volatile std::u64 system_runtime_us = 0;
+extern u32 cnt_test;
 
 /**
  * @brief 设置引脚中断优先级
@@ -44,7 +42,7 @@ void nvic_config(void) {
 	nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
 	/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
 	nvic_irq_enable(TIMER2_IRQn, 0, 1);
-	nvic_irq_enable(TIMER1_IRQn, 0, 1);
+	nvic_irq_enable(TIMER1_IRQn, 0, 2);
 }
 
 /**
@@ -99,13 +97,6 @@ void User_timer_init(void) {
 	nvic_config();
 }
 
-/*!
-	\brief      获取系统运行时间(毫秒)
-	\param[in]  none
-	\param[out] none
-	\retval     系统运行时间，单位为毫秒
-*/
-uint64_t get_system_runtime_us(void) { return system_runtime_us; }
 
 /**
  * @brief   TIMER2中断处理函数
@@ -277,7 +268,7 @@ void timer1_pwm_config(void) {
  * @brief   TIMER1中断处理函数
  * @return void
  */
-extern "C" void TIMER1_IRQHandler(void) {
+void TIMER1_IRQHandler(void) {
 	// 检查CH3比较中断标志并处理
 	if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) {
 		// 清除CH3比较中断标志

Robot_V2/device/ang_sensor.cpp

@@ -1,12 +1,15 @@
 #include "ang_sensor.h"
 #include "Filter.h"
 #include "core_cmFunc.h"
+#include "gd32c10x_can.h"
 #include "gd32c10x_dma.h"
 #include "gd32c10x_gpio.h"
 #include "gd32c10x_spi.h"
 #include "initial.h"
+#include "flash.h"
 #include <cstdio>
 #include <cstring>
+#include "Can.h"
 #include <stdint.h>
 
 #define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
@@ -28,8 +31,10 @@ volatile bool spi_dma_tx_complete = false;
 volatile bool spi_dma_rx_complete = false;
 
 angle_sensor KTH7823; // 角度传感器初始化
+extern FlashEEPROM flashEEP;
 extern EncoderAngleKalman Angle_kalman;
 extern EncoderVelocityKalman Vel_kalman;
+	extern CAN_transmiter CAN0_transmiter;		// CAN初始化
 
 /**
  * @brief SPI DMA initialization
@@ -194,21 +199,27 @@ void KTH7823_spi_init(void) {
 	spi_enable(SPI0);
 }
 
+
 /**
- * @brief KTH7823 初始化
+* @ 角度传感器类 初始化
+* @return 0:初始化成功 1:初始化失败
  */
-void angle_sensor::KTH7823_init(void) {
+u8 angle_sensor::init(void) {
 	KTH7823_gpio_init(); // GPIO初始化
 	KTH7823_spi_init();	 // SPI初始化
 	spi_dma_init();		 // DMA初始化
-}
-/**
- * @brief 角度传感器构造函数
- */
-u8 angle_sensor::init(void) {
-	KTH7823_init();
 	return 0;
 }
+/**
+* @ 角度传感器类 设置当前角度
+* @param angle 输出级角度值
+* @return 0成功/1失败
+ */
+u8 angle_sensor::angle_set(float angle){
+	m_cumulativeAngle = flashEEP.read_angle()*REDUCTION_RATIO; // 从Flash读取上次保存的角度
+	return 0;
+}
+
 
 /**
  * @brief 读取传感器数据

Robot_V2/device/ang_sensor.h

@@ -21,9 +21,9 @@ class angle_sensor {
 	float getAngle(void) const;
 	float getRawAngle(void) const;
 	float getcurrent_sensorangle(void) const;
+	u8 angle_set(float angle);
 
   protected:
-	void KTH7823_init(void);
 	float m_cumulativeAngle;
 	float current_angle_deg;
 	float current_angle_rad;

Robot_V2/main.cpp

@@ -7,16 +7,20 @@
 #include "ang_sensor.h"
 #include "core_cm4.h"
 #include "core_cmFunc.h"
-#include "motor.h"
+#include "flash.h"
 #include "health.h"
 #include "initial.h"
+#include "motor.h"
 #include "systick.h"
 #include <cstdio>
 #include <stdint.h>
 
 TaskScheduler scheduler = {0};
+volatile u32 system_runtime_us = 0;
+volatile u32 cnt_test = 0;
 int main(void) {
 	systick_config();  // 系统时钟初始化
+	unused_pin_init(); // 未使用引脚初始化
 	USART0_init();	   // 调试串口初始化
 	User_timer_init(); // 任务Timer初始化
 	motor_init();	   // 电机驱动初始化
@@ -28,8 +32,10 @@ int main(void) {
 	extern EncoderAngleKalman Angle_kalman;		// 角度滤波
 	extern ThermalController thermalController; // 热量控制器
 	extern FOC_Controlroller FOC;				// FOC控制
+	extern FlashEEPROM flashEEP;				// EEPROM类
 
 	u8 init_status = 0;
+	init_status += flashEEP.init();						  // EEPROM初始化
 	init_status += KTH7823.init();						  // 角度传感器初始化
 	init_status += sampler.init();						  // 采样器初始化
 	init_status += CAN0_transmiter.init();				  // CAN初始化
@@ -39,7 +45,6 @@ int main(void) {
 		std::printf("初始化失败\r\n");
 	}
 
-	u32 cnt_test = 0;
 	watchdog_init(500); // 看门狗初始化
 	angsens_MG_check(); // 角度传感器磁感应强度测试
 	while (1) {
@@ -48,7 +53,6 @@ int main(void) {
 			scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性
 		scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
 		__enable_irq();
-		cnt_test++;
 
 		// 按照优先级和重要性处理任务
 		if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -59,11 +63,11 @@ int main(void) {
 			FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
 		}
 		if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
-			CAN0_transmiter.testfloat_union.f =  (Angle_kalman.getstate()-Angle_kalman.last_angle)*10000.0f; // 速度滤波前的速度值，单位: deg/s
 			FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
 		}
 		if (currentTasks &
 			(1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
+			system_runtime_us++;
 			switch (FOC.control_mode) {
 			case 0: {
 				FOC.drv_hold(0.95); // 占空比置0
@@ -71,6 +75,8 @@ int main(void) {
 			}
 			case 1: {
 				FOC.angle_control(FOC.target_angle); // 角度闭环控制
+				// FOC.angle_control_with_velocity_limit(FOC.target_angle,
+				// FOC.limit_velocity); // 角度闭环控制，带速度限制
 				break;
 			}
 			case 2: {
@@ -102,8 +108,7 @@ int main(void) {
 			angsens_MG_check();
 		}
 		if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
-			sampler.update_vbus_voltage();
+			power_manage();
-			FOC.update_power();
 		}
 		if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
 			// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测

Robot_V2/sys/flash.cpp

@@ -0,0 +1,344 @@
+#include "flash.h"
+#include "ang_sensor.h"
+#include "core_cmFunc.h"
+#include "gd32c10x_fmc.h"
+#include <Can.h>
+#include <cstdio>
+#include <stdint.h>
+
+FlashEEPROM flashEEP = FlashEEPROM();
+extern angle_sensor KTH7823;
+extern CAN_transmiter CAN0_transmiter;
+
+// FlashEEPROM类实现
+FlashEEPROM::FlashEEPROM() {
+	// 构造函数可以为空，或者做一些初始化工作
+}
+
+/**
+ * @brief  初始化Flash EEPROM模拟
+ * @param  None
+ * @retval None
+ */
+u8 FlashEEPROM::init() {
+	// 解锁Flash
+	fmc_unlock();
+
+	// 读取保存的参数
+	u16 angle;
+
+	// 尝试从Flash读取参数，如果不存在则使用默认值
+	if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) {
+		angle = 0; // 默认值
+		std::printf("角度初始化为: %d\r\n", angle);
+	} else {
+		std::printf("从Flash读取到角度数: %d\r\n", angle);
+	}
+
+	KTH7823.angle_set((float)angle);
+	// save_angle(200, false); // 重置保存标志以允许再次保存
+	return 0;
+}
+
+/**
+ * @brief  从Flash EEPROM读取数据
+ * @param  virtualAddr: 虚拟地址
+ * @param  data: 指向存储读取数据的指针
+ * @retval FlashEepromStatus: 操作状态
+ */
+FlashEepromStatus FlashEEPROM::read(uint16_t virtualAddr,
+									uint16_t *data) const {
+	// 检查参数有效性
+	if (data == nullptr || virtualAddr == 0xFFFF) {
+		return FLASH_EEPROM_ERROR;
+	}
+
+	// 从Flash末尾开始向前搜索，找到最新的数据
+	for (uint32_t address = EEPROM_END_ADDRESS - sizeof(uint32_t);
+		 address >= EEPROM_START_ADDRESS; address -= sizeof(uint32_t)) {
+
+		uint32_t data_word = *((uint32_t *)address);
+
+		// 检查是否是有效数据（非擦除状态）
+		if (data_word != 0xFFFFFFFF) {
+			uint16_t addr = (uint16_t)(data_word & 0xFFFF);
+
+			// 找到匹配的虚拟地址
+			if (addr == virtualAddr) {
+				*data = (uint16_t)((data_word >> 16) & 0xFFFF);
+				return FLASH_EEPROM_OK;
+			}
+		}
+	}
+
+	// 未找到指定虚拟地址的数据
+	return FLASH_EEPROM_NOT_FOUND;
+}
+
+/**
+ * @brief  向Flash EEPROM写入数据
+ * @param  virtualAddr: 虚拟地址
+ * @param  data: 要写入的数据
+ * @retval FlashEepromStatus: 操作状态
+ */
+FlashEepromStatus FlashEEPROM::write(uint16_t virtualAddr, uint16_t data) {
+	// 参数检查
+	if (virtualAddr == 0xFFFF) {
+		return FLASH_EEPROM_INVALID_ADDR;
+	}
+
+	// 查找下一个可写入的位置
+	uint32_t next_address = EEPROM_START_ADDRESS;
+
+	// 遍历Flash页面寻找下一个可写入位置
+	for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS;
+		 address += sizeof(uint32_t)) {
+
+		uint32_t data_word = *((uint32_t *)address);
+
+		// 如果找到已使用的地址且与要写入的地址相同，检查数据是否需要更新
+		if ((data_word & 0xFFFF) == virtualAddr) {
+			uint16_t old_data = (uint16_t)((data_word >> 16) & 0xFFFF);
+			if (old_data == data) {
+				// 数据相同，无需更新
+				return FLASH_EEPROM_OK;
+			}
+		}
+		// 找到空位置
+		if (data_word == 0xFFFFFFFF) {
+			next_address = address;
+			break;
+		}
+		// 更新下一个可能的写入位置（如果需要格式化）
+		next_address = address + sizeof(uint32_t);
+	}
+
+	// 如果有足够空间，直接写入
+	if (next_address < EEPROM_END_ADDRESS) {
+		u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF);
+		fmc_state_enum fmc_status =
+			fmc_word_program(next_address, new_data_word);
+		if (fmc_status != FMC_READY) {
+			return FLASH_EEPROM_WRITE_ERROR;
+		}
+
+		return FLASH_EEPROM_OK;
+	} else {
+		// 空间不足，需要格式化页面
+		FlashEepromStatus status = format();
+		if (status != FLASH_EEPROM_OK) {
+			return status;
+		}
+
+		// 格式化后在页面开头写入数据
+		uint32_t new_data_word =
+			((uint32_t)data << 16) | (virtualAddr & 0xFFFF);
+		fmc_state_enum fmc_status =
+			fmc_word_program(EEPROM_START_ADDRESS, new_data_word);
+
+		if (fmc_status != FMC_READY) {
+			return FLASH_EEPROM_WRITE_ERROR;
+		}
+
+		return FLASH_EEPROM_OK;
+	}
+}
+
+/**
+ * @brief  格式化Flash EEPROM页面
+ * @param  None
+ * @retval FlashEepromStatus: 操作状态
+ */
+FlashEepromStatus FlashEEPROM::format() {
+	fmc_state_enum fmc_status;
+
+	// 创建临时缓冲区存储唯一地址的最新数据
+	uint32_t valid_entries[EEPROM_MAX_SIZE / sizeof(uint32_t)];
+	uint16_t valid_count = 0;
+
+	// 收集有效的数据项（去重，保留最新的）
+	for (uint32_t address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS;
+		 address += sizeof(uint32_t)) {
+
+		uint32_t data_word = *((uint32_t *)address);
+
+		// 检查是否是有效数据项
+		if (data_word != 0xFFFFFFFF) {
+			uint16_t addr = (uint16_t)(data_word & 0xFFFF);
+			uint16_t value = (uint16_t)((data_word >> 16) & 0xFFFF);
+
+			// 查找是否已经有相同虚拟地址的数据项
+			uint8_t found = 0;
+			for (uint16_t i = 0; i < valid_count; i++) {
+				uint16_t existing_addr = (uint16_t)(valid_entries[i] & 0xFFFF);
+				if (existing_addr == addr) {
+					// 更新为最新的数据
+					valid_entries[i] = ((uint32_t)value << 16) | addr;
+					found = 1;
+					break;
+				}
+			}
+
+			// 如果是新的虚拟地址，添加到有效数据列表
+			if (!found && valid_count < (EEPROM_MAX_SIZE / sizeof(uint32_t))) {
+				valid_entries[valid_count] = ((uint32_t)value << 16) | addr;
+				valid_count++;
+			}
+		}
+	}
+
+	// 擦除整个页面
+	fmc_status = fmc_page_erase(EEPROM_START_ADDRESS);
+	if (fmc_status != FMC_READY) {
+		return FLASH_EEPROM_ERASE_ERROR;
+	}
+
+	// 重新写入有效的数据项
+	for (uint16_t i = 0; i < valid_count; i++) {
+		fmc_status = fmc_word_program(
+			EEPROM_START_ADDRESS + i * sizeof(uint32_t), valid_entries[i]);
+		if (fmc_status != FMC_READY) {
+			return FLASH_EEPROM_WRITE_ERROR;
+		}
+	}
+
+	return FLASH_EEPROM_OK;
+}
+
+/**
+ * @brief 更新参数并保存到Flash
+ * @param kp 新的KP值
+ * @param ki 新的KI值
+ * @param kd 新的KD值
+ */
+void FlashEEPROM::update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd) {
+	write(ADDR_MOTOR_KP, kp);
+	write(ADDR_MOTOR_KI, ki);
+	write(ADDR_MOTOR_KD, kd);
+}
+
+/**
+ * @brief 更新系统配置并保存到Flash
+ * @param config 新的系统配置值
+ */
+void FlashEEPROM::update_system_config(uint16_t config) {
+	write(ADDR_SYSTEM_CONFIG, config);
+}
+
+/**
+ * @brief 保存角度数据到Flash
+ * @param angle 角度数据
+ * @return FlashEepromStatus: 操作状态
+ */
+FlashEepromStatus FlashEEPROM::save_angle(float angle,
+										  bool saved) { // 解锁Flash
+	// 解锁Flash
+	fmc_unlock();
+
+	// 读取保存的参数
+	u16 angle2;
+
+	// 尝试从Flash读取参数，如果不存在则使用默认值
+	if (read(ADDR_ANGLE_DATA, &angle2) != FLASH_EEPROM_OK) {
+		angle = 0; // 默认值
+		std::printf("角度初始化为: %d\r\n", angle2);
+	} else {
+		std::printf("从Flash读取到角度数: %d\r\n", angle2);
+	}
+
+	// 读取保存的参数
+	if (!angle_saved) {
+		printf("saved_angle: %u\r\n", (u16)angle);
+		FlashEepromStatus status = write(ADDR_ANGLE_DATA, (u16)angle);
+		if (status == FLASH_EEPROM_OK) {
+			angle_saved = saved;			  // 标记已保存过
+			CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态
+		} else {
+			CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态
+		}
+		CAN0_transmiter.testfloat_union.f = status; // CAN发送测试
+		return status;
+	}
+	return FLASH_EEPROM_OK; // 已保存过，直接返回
+}
+
+/**
+ * @brief 读取角度数据从Flash
+ * @return float: 角度数据
+ */
+float FlashEEPROM::read_angle() {
+	u16 angle = 0;
+	if (read(ADDR_ANGLE_DATA, &angle) == FLASH_EEPROM_OK) {
+		return (float)angle;
+	}
+	return 0.0f; // 默认返回0度
+}
+
+/**
+ * @brief 增加错误计数并保存到Flash
+ */
+void FlashEEPROM::increment_error_count(void) {
+	uint16_t error_count = 0;
+
+	// 读取当前错误计数
+	FlashEepromStatus read_status = read(ADDR_ERROR_COUNT, &error_count);
+	if (read_status == FLASH_EEPROM_OK ||
+		read_status == FLASH_EEPROM_NOT_FOUND) {
+		// 增加错误计数（如果是NOT_FOUND，则error_count已经是0）
+		error_count++;
+
+		// 保存更新后的错误计数
+		write(ADDR_ERROR_COUNT, error_count);
+	}
+}
+
+/**
+ * @brief 示例主函数 - 展示Flash EEPROM的使用方法
+ */
+void flash_eeprom_example_main(void) {
+	std::printf("\r\n=== Flash EEPROM 使用示例 ===\r\n");
+
+	// 初始化Flash EEPROM
+	flashEEP.init();
+
+	// 模拟参数更新
+	std::printf("\r\n--- 更新PID参数 ---\r\n");
+	flashEEP.update_pid_parameters(120, 80, 30);
+
+	// 模拟系统配置更新
+	std::printf("\r\n--- 更新系统配置 ---\r\n");
+	flashEEP.update_system_config(0x03);
+
+	// 模拟错误发生
+	std::printf("\r\n--- 模拟错误计数增加 ---\r\n");
+	flashEEP.increment_error_count();
+	flashEEP.increment_error_count();
+
+	// 再次读取参数验证保存是否成功
+	std::printf("\r\n--- 验证保存的数据 ---\r\n");
+	uint16_t kp_value, ki_value, kd_value;
+	uint16_t system_config;
+	uint16_t error_count;
+
+	// if (flashEEP.read(ADDR_MOTOR_KP, &kp_value) == FLASH_EEPROM_OK) {
+	// 	std::printf("当前KP值: %d\r\n", kp_value);
+	// }
+
+	// if (flashEEP.read(ADDR_MOTOR_KI, &ki_value) == FLASH_EEPROM_OK) {
+	// 	std::printf("当前KI值: %d\r\n", ki_value);
+	// }
+
+	// if (flashEEP.read(ADDR_MOTOR_KD, &kd_value) == FLASH_EEPROM_OK) {
+	// 	std::printf("当前KD值: %d\r\n", kd_value);
+	// }
+
+	// if (flashEEP.read(ADDR_SYSTEM_CONFIG, &system_config) == FLASH_EEPROM_OK)
+	// { 	std::printf("当前系统配置: 0x%02X\r\n", system_config);
+	// }
+
+	// if (flashEEP.read(ADDR_ERROR_COUNT, &error_count) == FLASH_EEPROM_OK) {
+	// 	std::printf("当前错误计数: %d\r\n", error_count);
+	// }
+
+	std::printf("\r\n=== 示例结束 ===\r\n");
+}

Robot_V2/sys/flash.h

@@ -0,0 +1,64 @@
+#ifndef __FLASH_H
+#define __FLASH_H
+
+#include "gd32c10x.h"
+#include <cstdint>
+
+// EEPROM操作状态
+typedef enum {
+	FLASH_EEPROM_OK = 0,	   // 操作成功
+	FLASH_EEPROM_ERROR,		   // 操作失败
+	FLASH_EEPROM_NOT_FOUND,	   // 未找到指定数据
+	FLASH_EEPROM_INVALID_ADDR, // 无效地址
+	FLASH_EEPROM_WRITE_ERROR,  // 写入错误
+	FLASH_EEPROM_ERASE_ERROR   // 擦除错误
+} FlashEepromStatus;
+
+// Flash EEPROM类
+class FlashEEPROM {
+  public:	
+  	// EEPROM emulation constants
+	static constexpr uint32_t EEPROM_START_ADDRESS =
+		0x0801F800; // Flash最后一页(2KB)
+	static constexpr uint32_t EEPROM_END_ADDRESS = 0x08020000; // Flash结束地址
+	static constexpr uint32_t EEPROM_PAGE_SIZE = 0x800;		   // 页大小2KB
+	static constexpr uint32_t EEPROM_MAX_SIZE = 0x800;		   // EEPROM最大容量
+
+	// 定义用户自定义的虚拟地址
+	static constexpr uint16_t ADDR_MOTOR_KP = 0x0010;	   // 电机KP参数地址
+	static constexpr uint16_t ADDR_MOTOR_KI = 0x0011;	   // 电机KI参数地址
+	static constexpr uint16_t ADDR_MOTOR_KD = 0x0012;	   // 电机KD参数地址
+	static constexpr uint16_t ADDR_SYSTEM_CONFIG = 0x0020; // 系统配置参数地址
+	static constexpr uint16_t ADDR_ERROR_COUNT = 0x0030;   // 错误计数地址
+
+	// 角度数据存储地址
+	static constexpr u16 ADDR_ANGLE_DATA = 0x0021;
+	bool angle_saved = false;
+	// 构造函数
+	FlashEEPROM();
+
+	// 初始化Flash EEPROM模拟
+	u8 init();
+
+	// 从Flash EEPROM读取数据
+	FlashEepromStatus read(uint16_t virtualAddr, uint16_t *data) const;
+
+	// 向Flash EEPROM写入数据
+	FlashEepromStatus write(uint16_t virtualAddr, uint16_t data);
+
+	// 格式化Flash EEPROM页面
+	FlashEepromStatus format();
+
+	void flash_eeprom_example_init(void);
+	void update_pid_parameters(uint16_t kp, uint16_t ki, uint16_t kd);
+	void update_system_config(uint16_t config);
+	void increment_error_count(void);
+	FlashEepromStatus save_angle(float angle,bool saved);
+	float read_angle();
+
+  private:
+
+};
+
+void flash_eeprom_example_main(void);
+#endif /* __FLASH_H */

Robot_V2/sys/health.cpp

@@ -2,11 +2,13 @@
 #include "Can.h"
 #include "DengFOC.h"
 #include "adc.h"
+#include "flash.h"
 #include <stdint.h>
 
 extern sampler sampler;
 extern CAN_transmiter CAN0_transmiter;
 extern FOC_Controlroller FOC;
+extern FlashEEPROM flashEEP;
 ThermalController thermalController; // 热量控制器初始化
 
 #define max(a, b, c)                                                           \
@@ -86,3 +88,16 @@ void angsens_MG_check(void) {
 	bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
 	CAN0_transmiter.MagStatus = MGH or MGL;
 }
+
+void power_manage(void) {
+	float last_vbus = sampler.get_vbus_voltage();
+	sampler.update_vbus_voltage();
+	FOC.update_power();
+	float vbus = sampler.get_vbus_voltage();
+	if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) {
+		FOC.shut_down();
+		// flashEEP.save_angle(FOC.rt_angle,false);
+	} else {
+		FOC.resume();
+	}
+}

Robot_V2/sys/health.h

@@ -35,5 +35,6 @@ class ThermalController {
 };
 
 void angsens_MG_check(void);
+void power_manage(void);
 
 #endif

Robot_V2/sys/initial.cpp

@@ -1,4 +1,6 @@
 #include "initial.h"
+#include "gd32c10x_gpio.h"
+#include "gd32c10x_rcu.h"
 #include <stdint.h>
 
 
@@ -31,3 +33,16 @@ int fputc(int ch, FILE *f) throw() {
 		;
 	return ch;
 }
+
+
+void unused_pin_init() { 
+	rcu_periph_clock_enable(RCU_GPIOA); // 使能GPIOA时钟
+	rcu_periph_clock_enable(RCU_GPIOB); // 使能GPIOB时钟
+	rcu_periph_clock_enable(RCU_GPIOC); // 使能GPIOC时钟
+	gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12);
+	gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15);
+	gpio_init(GPIOC, GPIO_MODE_IPU, GPIO_OSPEED_2MHZ, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15);
+	gpio_bit_reset(GPIOA, GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_12);
+	gpio_bit_reset(GPIOB, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_15);
+	gpio_bit_reset(GPIOC, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15);
+}

Robot_V2/sys/initial.h

@@ -19,6 +19,7 @@ int fputc(int ch, FILE *f);						// 声明fputc函数以重载标准库实现
 
 u8 USART0_init(void);
 u8 watchdog_init(u32 time);
+void unused_pin_init();
 // 任务标志定义
 typedef enum {
 	TASK_MOTOR_CONTROL = 0,