feat(FOC): 添加开关机功能并优化定时器配置

Robot_V2/Algorithm/DengFOC.cpp

@@ -3,8 +3,10 @@
 #include "systick.h"
 #include <cmath>
 #include <cstdio>
+#include <gd32c10x.h>
 #include <stdint.h>
 
+
 constexpr double PI = 3.1415926535897932384626433832795f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11, DIR = 1;
@@ -15,7 +17,6 @@ constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-
 inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
 
 /**
@@ -81,17 +82,17 @@ void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
  */
 float FOC_Controlroller::velocity_open_loop(float target_velocity) {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	// 预计算常量 1e-4 = 100微秒控制周期 / 1秒 = 100e-6
+	// 预计算常量 1e-4 = 100微秒控制周期
 	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
-	
+
 	// 直接计算电气角度，避免函数调用开销
 	float angle_el = shaft_angle * PP + zero_electric_angle;
 	angle_el = _normalizeAngle(angle_el);
-	
+
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = limit_voltage;
 	const float Ud = 0.0f;
-	
+
 	// 帕克逆变换 (直接计算，避免函数调用开销)
 	const float sin_el = sin(angle_el);
 	const float cos_el = cos(angle_el);
@@ -103,13 +104,16 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
 	Ua = Ualpha + v_half;
 	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
 	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
-	
+
 	// 计算占空比
+	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
+	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
+	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
 	const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
 	const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
 	const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
 	Set_3Pwm(dc_a, dc_b, dc_c);
-	
+
 	return limit_voltage;
 }
 
@@ -128,3 +132,13 @@ void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) {
 	setPhaseVoltage(0, 0, _3PI_2);
 	std::printf("0电角度:%f", zero_electric_angle);
 }
+
+void FOC_Controlroller::shut_down() {
+	is_shut_down = 100;
+	gpio_bit_reset(GPIOB, GPIO_PIN_12);
+}
+
+void FOC_Controlroller::resume() {
+	is_shut_down = 0;
+	gpio_bit_set(GPIOB, GPIO_PIN_12);
+}

Robot_V2/Algorithm/DengFOC.h

@@ -17,9 +17,12 @@ class FOC_Controlroller {
 	float limit_velocity = 0.0f;
 	float target_angle = 0.0f;
 	float target_velocity = 0.0f;
+	u8 is_shut_down = 0;
 
 	float velocity_open_loop(float target_velocity);
 	void DFOC_alignSensor(int _PP, int _DIR);
+	void shut_down(void);
+	void resume(void);
 
   private:
 	std::u64 last_time_us = 0;

Robot_V2/device/Can.cpp

@@ -596,10 +596,19 @@ void CAN_transmiter::communication_check() {
 		// }
 		if (receive_message.rx_efid == 0x18ff0200) {
 			FOC.control_mode = receive_message.rx_data[0];
-			FOC.limit_voltage =
+			if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 !=
-				float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
+				FOC.limit_voltage) {
-			FOC.limit_velocity =
+				FOC.limit_voltage =
-				unpack_bytes_to_float(receive_message.rx_data, 3).f;
+					float(unpack_bytes_to_u16(receive_message.rx_data, 1)) /
+					1000;
+				FOC.is_shut_down = 100; // 1s滤除启动电流
+			}
+			if (unpack_bytes_to_float(receive_message.rx_data, 3).f !=
+				FOC.limit_velocity) {
+				FOC.limit_velocity =
+					unpack_bytes_to_float(receive_message.rx_data, 3).f;
+				FOC.is_shut_down = 100; // 1s滤除启动电流
+			}
 			FOC.target_angle =
 				unpack_bytes_to_float(receive_message.rx_data, 7).f;
 			FOC.target_velocity =

Robot_V2/device/Timer.cpp

@@ -5,8 +5,10 @@
 #include "adc.h"
 #include "gd32c10x.h"
 #include "gd32c10x_gpio.h"
+#include "gd32c10x_misc.h"
 #include "gd32c10x_rcu.h"
 #include "gd32c10x_timer.h"
+#include "systick.h"
 #include <cstdio>
 #include <stdbool.h>
 #include <stdint.h>
@@ -18,6 +20,8 @@
 // 然后设置周期为99，即计数100次，1MHz/100 = 10kHz = 100微秒
 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
@@ -47,13 +51,14 @@ void nvic_config(void) {
 	nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
 	/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
 	nvic_irq_enable(TIMER2_IRQn, 1, 1);
+	nvic_irq_enable(TIMER3_IRQn, 1, 2);
 }
 
 /**
- * @brief 配置定时器1
+ * @brief 配置定时器2
  * @return void
  *
- *配置定时器1
+ *配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
  */
 void TIMER2_config(void) {
 	/* ----------------------------------------------------------------------------
@@ -90,6 +95,47 @@ void TIMER2_config(void) {
 	timer_enable(TIMER2);
 }
 
+/**
+ * @brief 配置定时器3
+ * @return void
+ *
+ *配置定时器3，用于诊断恢复
+ */
+void TIMER3_config(void) {
+	/* ----------------------------------------------------------------------------
+	TIMER3 Configuration:
+	TIMER3CLK = SystemCoreClock/120 = 1MHz, the period is 10ms(10000/1000000 =
+	10ms).
+	----------------------------------------------------------------------------
+  */
+	timer_parameter_struct timer_struct;
+
+	/* 使能TIMER3时钟 */
+	rcu_periph_clock_enable(RCU_TIMER3);
+
+	/* 复位TIMER3外设到默认状态 */
+	timer_deinit(TIMER3);
+	/* 初始化TIMER初始化参数结构体 */
+	timer_struct_para_init(&timer_struct);
+	/* TIMER3配置参数设置 */
+	timer_struct.prescaler =
+		TIMER3_PRESCALER; /* 预分频值：120分频(从0开始计数) */
+	timer_struct.alignedmode = TIMER_COUNTER_EDGE;	  /* 边沿对齐模式 */
+	timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
+	timer_struct.period =
+		TIMER3_PERIOD; /* 自动重装载值：10000次计数(从0开始计数) */
+	timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
+	timer_init(TIMER3, &timer_struct);
+
+	/* 清除更新中断标志位 */
+	timer_interrupt_flag_clear(TIMER3, TIMER_INT_FLAG_UP);
+	/* 使能更新中断 */
+	timer_interrupt_enable(TIMER3, TIMER_INT_UP);
+
+	/* 使能TIMER3 */
+	timer_enable(TIMER3);
+}
+
 /**
  * @brief   配置TIMER2中断
  * @return void
@@ -118,45 +164,52 @@ void TIMER2_IRQHandler(void) {
 	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
 		// 清除更新中断标志
 		timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
-		
+
 		// 增加总计数器 (使用更高效的位运算代替取模运算)
 		total_us100_counter++;
 		system_runtime_us += 100;
-		
+
 		// 在定时器中断中执行FOC控制，确保控制频率稳定 (最高频操作放在最前)
 		FOC.velocity_open_loop(FOC.target_velocity);
-		
+
 		// 每100us执行一次的操作
 		sampler.update_current();
 
 		// 每1ms执行的代码 (100us * 10 = 1ms)
 		if (total_us100_counter >= 10) {
 			total_us100_counter = 0; // 重置计数器
-			
+
 			// 设置CAN发送标志
 			CAN0_transmiter.can0_send_flag = SET;
-			
+
 			// 更新各种传感器数据
 			sampler.update_mcu_temp();
 			sampler.update_drv_temp();
 			sampler.update_vbus_voltage();
 
 			// 每10ms执行的代码 (1ms * 10 = 10ms)
-			static uint8_t ms10_counter = 0;
+			static u8 ms10_counter = 0;
 			ms10_counter++;
 			if (ms10_counter >= 10) {
 				ms10_counter = 0;
 				// 可以添加每10ms执行的代码
-				
+
 				// 每100ms执行的代码 (10ms * 10 = 100ms)
-				static uint8_t ms100_counter = 0;
+				static u8 ms100_counter = 0;
 				ms100_counter++;
 				if (ms100_counter >= 10) {
 					ms100_counter = 0;
 					// 可以添加每100ms执行的代码
-					
+					// if (sampler.get_current('U') > 1.0 ||
+					// 	sampler.get_current('V') > 1.0 ||
+					// 	sampler.get_current('W') > 1.0) {
+					// 	if (FOC.is_shut_down == 0) {
+					// 		FOC.shut_down();
+					// 	}
+					// }
+
 					// 每1s执行的代码 (100ms * 10 = 1s)
-					static uint8_t sec_counter = 0;
+					static u8 sec_counter = 0;
 					sec_counter++;
 					if (sec_counter >= 10) {
 						sec_counter = 0;
@@ -168,6 +221,38 @@ void TIMER2_IRQHandler(void) {
 	}
 }
 
+/**
+ * @brief   TIMER3中断处理函数
+ * @return void
+ */
+void TIMER3_IRQHandler(void) {
+	// 检查更新中断标志并处理
+	if (timer_interrupt_flag_get(TIMER3, TIMER_INT_UP) != RESET) {
+		// 清除更新中断标志
+		timer_interrupt_flag_clear(TIMER3, TIMER_INT_UP);
+		static u8 count = 0;
+		count++;
+		if (FOC.is_shut_down > 0)
+			FOC.is_shut_down--;
+		else
+			FOC.resume();
+
+		// 10ms测试代码
+		if (count >= 10) {
+			count = 0;
+			// 100ms测试代码
+
+			static u8 count_10 = 0;
+			count_10++;
+			if (count_10 >= 10) {
+				count_10 = 0;
+
+				// 1s测试代码
+			}
+		}
+	}
+}
+
 /**
  * @brief  TIMER1 GPIO配置函数
  */

Robot_V2/device/Timer.h

@@ -11,6 +11,8 @@ void User_timer_init(void);
 void Set_Pwm(uint16_t channel, float duty_cycle);
 void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2);
 void TIMER2_IRQHandler(void);
+void TIMER3_config(void);
+void TIMER3_IRQHandler(void);
 
 void timer1_pwm_config(void);
 void timer1_gpio_config(void);

Robot_V2/sys/initial.cpp

@@ -1,4 +1,5 @@
 #include "initial.h"
+#include "gd32c10x_timer.h"
 #include "systick.h"
 #include <stdint.h>
 #include "Timer.h"
@@ -124,6 +125,8 @@ u8 watchdog_init(u32 time) {
 void driver_init(void) {
 	timer1_gpio_config();
 	timer1_pwm_config();
+	TIMER3_config();
+	
 }
 
 int fputc(int ch, FILE *f) throw() {