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;