feat(FOC): 增加闭环速度控制算法

- 重构setPhaseVoltage函数实现，提高执行效率
- 增加update_power函数，动态更新电源电压
- 优化velocity_open_loop函数，调用、setPhaseVoltage避免重复代码
- 实现angle_control和velocity_control闭环控制函数
- 调整PID控制器参数初始化及增加参数设置接口
- 改进电流和编码器速度卡尔曼滤波器实现
- 更新CAN通信数据打包逻辑，增加测试字段
- 在定时器中断中集成多种控制模式切换逻辑
- 完善角度传感器数据处理，改进速度计算方法
- 减小速度滤波数组大小，提升响应速度

Robot_V2/Algorithm/DengFOC.cpp

@@ -1,4 +1,5 @@
 #include "DengFOC.h"
+#include "Can.h"
 #include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
@@ -14,21 +15,22 @@
 constexpr double PI = 3.1415926535897932384626433832795f;
 constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
 constexpr float RAD2DEG = 360.0f / 6.28318530718f;
+constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
 constexpr float _1_SQRT3 = 0.57735026919f;
 constexpr float _2_SQRT3 = 1.15470053838f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11;
-constexpr float DIR = -1.0;
+constexpr float DIR = 1.0;
 constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
 constexpr float zero_electric_angle = 0.0f;
 
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern CAN_transmiter CAN0_transmiter;
 
 CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
-
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
@@ -65,15 +67,17 @@ FOC_Controlroller::FOC_Controlroller() { ; }
  */
 inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 											   float angle_el) {
-	angle_el = _normalizeAngle(angle_el);
+	// 帕克逆变换 (直接计算，避免函数调用开销)
-	// 帕克逆变换
+	const float sin_el = sin(angle_el);
-	Ualpha = -Uq * sin(angle_el);
+	const float cos_el = cos(angle_el);
-	Ubeta = Uq * cos(angle_el);
+	Ualpha = -Uq * sin_el;
+	Ubeta = Uq * cos_el;
+
+	// 克拉克逆变换 (预计算常量值)
+	Ua = Ualpha + v_half;
+	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
+	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
 
-	// 克拉克逆变换
-	Ua = Ualpha + voltage_power_supply / 2;
-	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
-	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
 	// 计算占空比
 	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
 	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
@@ -84,6 +88,33 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 	Set_3Pwm(dc_a, dc_b, dc_c);
 	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
+/**
+ * @brief 电机关机
+ *
+ */
+void FOC_Controlroller::shut_down() {
+	is_shut_down = 100;
+	gpio_bit_reset(GPIOB, GPIO_PIN_12);
+}
+/**
+ * @brief 电机恢复
+ *
+ */
+void FOC_Controlroller::resume() {
+	over_current = 0;
+	temp_error = 0;
+	voltage_error = 0;
+	is_shut_down = 0;
+	gpio_bit_set(GPIOB, GPIO_PIN_12);
+}
+/**
+ * @brief 更新电源电压
+ *
+ */
+void FOC_Controlroller::update_power(void) {
+	voltage_power_supply = sampler.get_vbus_voltage();
+	v_half = voltage_power_supply * 0.5f;
+}
 
 /**
  * @brief 速度开环控制
@@ -94,36 +125,16 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 float FOC_Controlroller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	// 预计算常量 1e-4 = 100微秒控制周期
-	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
+	shaft_angle =
+		_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
 
 	// 直接计算电气角度，避免函数调用开销
-	float angle_el = shaft_angle * PP + zero_electric_angle;
+	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
-	angle_el = _normalizeAngle(angle_el);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
-	const float Ud = 0.0f;
 
-	// 帕克逆变换 (直接计算，避免函数调用开销)
+	setPhaseVoltage(Uq, 0.0f, angle_el);
-	const float sin_el = sin(angle_el);
-	const float cos_el = cos(angle_el);
-	Ualpha = -Uq * sin_el;
-	Ubeta = Uq * cos_el;
-
-	// 克拉克逆变换 (预计算常量值)
-	const float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	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 * 2);
-	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
-	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
-	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;
 }
@@ -142,55 +153,40 @@ float FOC_Controlroller::velocity_open_loop() {
 // 	std::printf("0电角度:%f", zero_electric_angle);
 // }
 
-void FOC_Controlroller::shut_down() {
+/**
-	is_shut_down = 100;
+ * @brief 角度闭环控制
-	gpio_bit_reset(GPIOB, GPIO_PIN_12);
+ *
-}
+ * @param target_angle: 目标角度
-
+ * @return float: q轴电压
-void FOC_Controlroller::resume() {
+ */
-	over_current = 0;
+float FOC_Controlroller::angle_control(float target_angle) {
-	temp_error = 0;
+	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
-	voltage_error = 0;
+	static float Kp = 2.0f;
-	is_shut_down = 0;
+	float Uq =
-	gpio_bit_set(GPIOB, GPIO_PIN_12);
+		1.0f - _constrain(Kp * (target_angle - DIR * shaft_angle) * RAD2DEG,
-}
+						  -voltage_power_supply, voltage_power_supply);
-
+	static float Ud = 0.0f;
-void FOC_Controlroller::update_power(void) {
+	// 直接计算电气角度，避免函数调用开销
-	voltage_power_supply = sampler.get_vbus_voltage();
+	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f, angle_el);
+	return shaft_angle;
 }
 
 /**
- * @brief 角度闭环控制
+ * @brief 速度闭环控制
  *
  * @param target_velocity: 目标速度
  * @return float: q轴电压
  */
-float FOC_Controlroller::angle_control(float target_angle) {
+float FOC_Controlroller::velocity_control(float target_velocity) {
-	float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD;
+	// PID 控制计算
-	float Kp = 2.0f;
+	float Uq = v_half - vel_loop(target_velocity - rt_velocity);
 
-	float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG,
+	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
-						  -voltage_power_supply, voltage_power_supply);
-	shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD);
-	float Ud = 0.0f;
 	// 直接计算电气角度，避免函数调用开销
 	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
-
+	// 设置相电压
-	// 帕克逆变换
+	setPhaseVoltage(Uq, 0.0f, angle_el);
-	Ualpha = -Uq * sin(angle_el);
+	return rt_velocity;
-	Ubeta = Uq * cos(angle_el);
-
-	// 克拉克逆变换
-	Ua = Ualpha + voltage_power_supply / 2;
-	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
-	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
-	// 计算占空比
-	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);
-	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
-	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
-	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
-	Set_3Pwm(dc_a, dc_b, dc_c);
-	return Sensor_angel;
 }

Robot_V2/Algorithm/DengFOC.h

@@ -21,8 +21,7 @@ class FOC_Controlroller {
 
 	float velocity_open_loop();
 	float angle_control(float target_angle);
-	// void SET_VEL_PID(float P, float I, float D, float ramp);   // 速度环PID设置
+	float velocity_control(float target_velocity);
-	// void SET_ANGLE_PID(float P, float I, float D, float ramp); // 角度环PID设置
 
 	void update_power(void);
 	void DFOC_alignSensor(int _PP);
@@ -36,9 +35,10 @@ class FOC_Controlroller {
 	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
 	float zero_electric_angle = 0;
 	float voltage_power_supply = 24.0f; // 电源电压
+	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
 	PIDController vel_loop =
-		PIDController{2, 0, 0, 100000, voltage_power_supply / 2};
+		PIDController{0.8f, 3.0f, 0, 100000, voltage_power_supply};
 	PIDController angle_loop = PIDController{2, 0, 0, 100000, 100};
 
 	void setPhaseVoltage(float Uq, float Ud, float angle_el);

Robot_V2/Algorithm/Filter.cpp

@@ -1,9 +1,15 @@
 #include "Filter.h"
+#include "ang_sensor.h"
 #include <algorithm>
 #include <array>
 #include <cstring>
 #include <stdint.h>
 
+
+extern angle_sensor KTH7823;
+
+EncoderVelocityKalman Vel_kalman(KTH7823.getVelocity(), 1e-5f, 1e-6f, 0.05f);
+
 // 队列实现
 void CircularBuffer::enqueue(const float value) {
 	buffer[head] = value;
@@ -73,54 +79,92 @@ float medium_filter(float values[], u8 size) {
 */
 CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
 							 float Q, float R_noise)
-	: x(initial_current), P(1.0f), // 初始误差协方差
+	: kalman(initial_current, 1.0f, Q, R_noise), L(L), R_m(R), T(T) {}
-	  Q(Q),						   // 过程噪声
-	  R(R_noise),				   // 测量噪声
-	  L(L), R_m(R), T(T) {
-		
-		CircularBuffer buffer;
-	  }
 /*!
 	\brief      更新卡尔曼滤波器状态
-	\param[in]  measured_current 测量电流值
+	\param[in]  measured_value 测量值
 	\param[in]  applied_voltage 应用电压值
 	\retval     估计的电流值
 	\note       实现卡尔曼滤波的预测和更新步骤
 */
-float CurrentKalman::update(float measured_current, float applied_voltage) {
+float CurrentKalman::update(float measured_value, float applied_voltage) {
 	// 1. 预测步骤 - 基于电机模型
-	float predicted_current = x + T * (applied_voltage - R_m * x) / L;
+	float predicted_current = kalman.getState() + T * (applied_voltage - R_m * kalman.getState()) / L;
 	
-	// 2. 计算预测误差协方差
+	// 2. 预测误差协方差已经在kalman.predict中处理
-	float P_pred = P + Q;
 	
-	// 3. 计算卡尔曼增益
+	// 3. 执行预测
-	float K = P_pred / (P_pred + R);
+	kalman.predict(predicted_current);
 	
-	// 4. 更新状态估计
+	// 4. 更新步骤 - 直接使用测量值更新
-	x = predicted_current + K * (measured_current - predicted_current);
+	return kalman.update(measured_value);
-
-	// 5. 更新估计误差协方差
-	P = (1 - K) * P_pred;
-
-	return x;
 }
 /*!
 	\brief      获取当前电流估计值
 	\retval     当前电流估计值
 */
-float CurrentKalman::getCurrent() const { return x; }
+float CurrentKalman::getCurrent() const { return kalman.getState(); }
 /*!
 	\brief      设置过程噪声协方差
 	\param[in]  q 过程噪声协方差值
 	\retval     none
 */
-void CurrentKalman::setProcessNoise(float q) { Q = q; }
+void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
 /*!
 	\brief      设置测量噪声协方差
 	\param[in]  r 测量噪声协方差值
 	\retval     none
 */
-void CurrentKalman::setMeasurementNoise(float r) { R = r; }
+void CurrentKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); }
 
+// EncoderVelocityKalman类的实现
+/*!
+	\brief      构造函数，初始化编码器速度卡尔曼滤波器参数
+	\param[in]  initial_velocity 初始速度值 (rad/s)
+	\param[in]  T 控制周期(s)
+	\param[in]  Q 过程噪声协方差(典型值: 1e-4 ~ 1e-2)
+	\param[in]  R 测量噪声协方差(典型值: 速度范围的1%~5%)
+	\retval     none
+*/
+EncoderVelocityKalman::EncoderVelocityKalman(float initial_velocity, float T,
+											 float Q, float R)
+	: kalman(initial_velocity, T, Q, R) {}
 
+/*!
+	\brief      更新卡尔曼滤波器状态
+	\param[in]  measured_velocity 测量的速度值 (rad/s)
+	\retval     估计的速度值 (rad/s)
+	\note       实现卡尔曼滤波的预测和更新步骤，假设速度变化符合随机游走模型
+*/
+float EncoderVelocityKalman::update(float measured_velocity) {
+	// 1. 预测步骤 - 假设速度符合随机游走模型（加速度为0）
+	//    由于速度变化缓慢，我们可以简单预测速度保持不变
+	kalman.predict(kalman.getState());
+
+	// 2. 更新步骤 - 直接使用测量值更新
+	return kalman.update(measured_velocity);
+}
+
+/*!
+	\brief      获取当前速度估计值
+	\retval     当前速度估计值 (rad/s)
+*/
+float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); }
+
+/*!
+	\brief      设置过程噪声协方差
+	\param[in]  q 过程噪声协方差值
+	\retval     none
+*/
+void EncoderVelocityKalman::setProcessNoise(float q) {
+	kalman.setProcessNoise(q);
+}
+
+/*!
+	\brief      设置测量噪声协方差
+	\param[in]  r 测量噪声协方差值
+	\retval     none
+*/
+void EncoderVelocityKalman::setMeasurementNoise(float r) {
+	kalman.setMeasurementNoise(r);
+}

Robot_V2/Algorithm/Filter.h

@@ -34,6 +34,92 @@ class CircularBuffer {
 
 float medium_filter(float values[], u8 size);
 
+/*!
+	\brief 通用卡尔曼滤波器模板类
+	\note  可用于各种基于线性系统的状态估计
+*/
+template <typename T> class KalmanFilter {
+  private:
+	T x; // 状态估计
+	T P; // 估计误差协方差
+	T Q; // 过程噪声协方差
+	T R; // 测量噪声协方差
+
+  public:
+	/*!
+		\brief 构造函数
+		\param[in] initial_state 初始状态值
+		\param[in] initial_error_covariance 初始误差协方差
+		\param[in] process_noise 过程噪声协方差
+		\param[in] measurement_noise 测量噪声协方差
+	*/
+	KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
+				 T measurement_noise)
+		: x(initial_state), P(initial_error_covariance), Q(process_noise),
+		  R(measurement_noise) {}
+
+	/*!
+		\brief 预测步骤
+		\param[in] state_prediction 状态预测值
+		\retval none
+	*/
+	void predict(T state_prediction) {
+		x = state_prediction;
+		P += Q;
+	}
+
+	/*!
+		\brief 更新步骤
+		\param[in] measurement 测量值
+		\param[in] measurement_prediction 测量预测值
+		\retval 估计的状态值
+	*/
+	T update(T measurement, T measurement_prediction) {
+		T y = measurement - measurement_prediction; // 测量残差
+		T S = P + R;								// 残差协方差
+		T K = P / S;								// 卡尔曼增益
+
+		x += K * y;		 // 更新状态估计
+		P = (1 - K) * P; // 更新误差协方差
+
+		return x;
+	}
+
+	/*!
+		\brief 直接更新步骤（适用于状态直接可测的情况）
+		\param[in] measurement 测量值
+		\retval 估计的状态值
+	*/
+	T update(T measurement) {
+		T K = P / (P + R); // 卡尔曼增益
+
+		x += K * (measurement - x); // 更新状态估计
+		P = (1 - K) * P;			// 更新误差协方差
+
+		return x;
+	}
+
+	/*!
+		\brief 获取当前状态估计值
+		\retval 当前状态估计值
+	*/
+	T getState() const { return x; }
+
+	/*!
+		\brief 设置过程噪声协方差
+		\param[in] process_noise 过程噪声协方差值
+		\retval none
+	*/
+	void setProcessNoise(T process_noise) { Q = process_noise; }
+
+	/*!
+		\brief 设置测量噪声协方差
+		\param[in] measurement_noise 测量噪声协方差值
+		\retval none
+	*/
+	void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
+};
+
 /*!
 	\brief 电流卡尔曼滤波器类
 	\note  基于电机模型的卡尔曼滤波器实现，用于电流状态估计
@@ -45,10 +131,7 @@ float medium_filter(float values[], u8 size);
 */
 class CurrentKalman {
   private:
-	float x;   // 电流状态估计
+	KalmanFilter<float> kalman;
-	float P;   // 估计误差协方差
-	float Q;   // 过程噪声协方差
-	float R;   // 测量噪声协方差
 	float L;   // 电机电感(H)
 	float R_m; // 电机电阻(Ω)
 	float T;   // 控制周期(s)
@@ -66,6 +149,28 @@ class CurrentKalman {
 	void setMeasurementNoise(float r);
 };
 
+/*!
+	\brief 编码器速度卡尔曼滤波器类
+	\note  用于滤除编码器速度测量噪声的卡尔曼滤波器实现
+*/
+class EncoderVelocityKalman {
+  private:
+	KalmanFilter<float> kalman;
+
+  public:
+	EncoderVelocityKalman(float initial_velocity,
+						  float initial_error_covariance, float process_noise,
+						  float measurement_noise);
+
+	float update(float measured_velocity);
+
+	float getVelocity() const;
+
+	void setProcessNoise(float q);
+
+	void setMeasurementNoise(float r);
+};
+
 #endif // __cplusplus
 
 #endif // _FILTER_H

Robot_V2/Algorithm/pid.cpp

@@ -1,18 +1,17 @@
 #include "pid.h"
 #include "Timer.h"
-#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+#define _constrain(amt, low, high)                                             \
+	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
+#include "Can.h"
 
+extern CAN_transmiter CAN0_transmiter;
 
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
-    : P(P)
+	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
-    , I(I)
+	  ,
-    , D(D)
+	  limit(limit) // PID控制器输出限幅
-    , output_ramp(ramp)    // PID控制器加速度限幅
+	  ,
-    , limit(limit)         // PID控制器输出限幅
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {
-    , error_prev(0.0f)
-    , output_prev(0.0f)
-    , integral_prev(0.0f)
-{
 	timestamp_prev = get_system_runtime_us();
 }
 
@@ -21,7 +20,8 @@ float PIDController::operator() (float error){
 	// 计算两次循环中间的间隔时间
 	unsigned long timestamp_now = get_system_runtime_us();
 	float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
-    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+	if (Ts <= 0 || Ts > 0.5f)
+		Ts = 1e-3f;
 
 	// P环
 	float proportional = P * error;
@@ -30,7 +30,6 @@ float PIDController::operator() (float error){
 	integral = _constrain(integral, -limit, limit);
 	// D环（微分环节）
 	float derivative = D * (error - error_prev) / Ts;
-
 	// 将P,I,D三环的计算值加起来
 	float output = proportional + integral + derivative;
 	output = _constrain(output, -limit, limit);
@@ -48,5 +47,20 @@ float PIDController::operator() (float error){
 	output_prev = output;
 	error_prev = error;
 	timestamp_prev = timestamp_now;
+
 	return output;
 }
+
+/**
+ * @brief 设置PID参数
+ *
+ * @param P: P参数
+ * @param I: I参数
+ * @param D: D参数
+ * @return void
+ */
+void PIDController::set_PID(float P, float I, float D) {
+	P = P;
+	I = I;
+	D = D;
+}

Robot_V2/device/Can.cpp

@@ -1,5 +1,6 @@
 #include "Can.h"
 #include "DengFOC.h"
+#include "Filter.h"
 #include "adc.h"
 #include "ang_sensor.h"
 #include "gd32c10x.h"
@@ -23,6 +24,7 @@ extern CAN_transmiter CAN0_transmiter;
 extern FOC_Controlroller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern EncoderVelocityKalman Vel_kalman;
 
 // 添加DMA相关变量
 dma_parameter_struct dma_rx_parameter;
@@ -431,7 +433,8 @@ void CAN_transmiter::update(void) {
 	motor_temp = sampler.get_drv_temp() * 100;
 	Power_Voltage = sampler.get_vbus_voltage() * 1000;
 	rt_angle_union.f = KTH7823.getAngle();
-	rt_velocity_union.f = KTH7823.getVelocity();
+	// rt_velocity_union.f = KTH7823.getVelocity();
+	rt_velocity_union.f = FOC.rt_velocity;
 	Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
 
 	Target_Control_Mode = FOC.control_mode;
@@ -440,6 +443,7 @@ void CAN_transmiter::update(void) {
 	u_current_union.f = sampler.get_current('U') * 1000;
 	w_current_union.f = sampler.get_current('W') * 1000;
 	v_current_union.f = sampler.get_current('V') * 1000;
+	// testfloat_union.f = KTH7823.getVelocity();
 }
 
 /*!
@@ -621,7 +625,8 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
 	pack_float_to_bytes(transmit_data, 13, rt_angle_union);
 	pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
- 	success_flag += CANFD_Send(STATUS_ID, 24, transmit_data);
+	pack_float_to_bytes(transmit_data, 21, testfloat_union);
+	success_flag += CANFD_Send(STATUS_ID, 32, transmit_data);
 
 	// 打包config数据
 	transmit_data[0] = controller_Mode;
@@ -677,7 +682,7 @@ void CAN_transmiter::communication_check() {
 
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 11).f;
-			FOC.target_velocity = target_velocity_union.f * ANGLE_FACTOR;
+			FOC.target_velocity = target_velocity_union.f;
 
 			can0_receive_flag = RESET;
 		}

Robot_V2/device/Can.h

@@ -106,6 +106,7 @@ class CAN_transmiter {
 	can_receive_message_struct receive_message; // 接收消息结构体
 	FlagStatus can0_receive_flag;				// CAN0接收标志
 	FlagStatus can0_send_flag;					// CAN0发送标志
+	float_union_t testfloat_union = {0};			// 测试用
 
 	void communication_check();					// 通信检查
 };

Robot_V2/device/Timer.cpp

@@ -1,6 +1,7 @@
 #include "Timer.h"
 #include "Can.h"
 #include "DengFOC.h"
+#include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
 #include "ang_sensor.h"
@@ -9,6 +10,7 @@
 #include "gd32c10x_misc.h"
 #include "gd32c10x_rcu.h"
 #include "gd32c10x_timer.h"
+#include <cstdio>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -35,6 +37,7 @@ extern FOC_Controlroller FOC;
 // 声明外部采样器对象
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern EncoderVelocityKalman Vel_kalman;
 
 /**
  * @brief 设置引脚中断优先级
@@ -156,6 +159,7 @@ uint64_t get_system_runtime_us(void) { return system_runtime_us; }
  * @return void
  */
 void TIMER2_IRQHandler(void) {
+	static u8 mode = 0;
 	// 检查更新中断标志并处理
 	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
 		// 清除更新中断标志
@@ -168,9 +172,15 @@ void TIMER2_IRQHandler(void) {
 		// 每100us执行一次的操作
 		sampler.update_current(); // 更新电流采样值
 		KTH7823.update();		  // 更新角度传感器值
-								  // FOC.velocity_open_loop(); // 速度开环控制
+		FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
-		// FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
+
-		FOC.angle_control(FOC.target_angle);
+		if (FOC.control_mode == 1) {
+			FOC.angle_control(FOC.target_angle); // 角度闭环控制
+		} else if (FOC.control_mode == 2) {
+			FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
+		} else if (FOC.control_mode == 4) {
+			FOC.velocity_open_loop(); // 速度开环控制
+		}
 
 		if (total_us100_counter >= 10) {
 			// 每1ms执行的代码
@@ -178,7 +188,8 @@ void TIMER2_IRQHandler(void) {
 
 			// 设置CAN发送标志
 			CAN0_transmiter.can0_send_flag = SET;
-
+			mode = FOC.control_mode;
+			// 过流检测
 			if (sampler.get_current('U') > 2000 ||
 				sampler.get_current('V') > 2000 ||
 				sampler.get_current('W') > 2000) {
@@ -202,7 +213,7 @@ void TIMER2_IRQHandler(void) {
 			if (ms10_counter >= 10) {
 				ms10_counter = 0;
 				// 可以添加每10ms执行的代码
-
+				FOC.update_power();
 				static u8 ms100_counter = 0;
 				ms100_counter++;
 				if (ms100_counter >= 10) {

Robot_V2/device/ang_sensor.cpp

@@ -10,9 +10,14 @@
 #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
 
 constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
+constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
 constexpr u16 DEFAULT_COMMAND = 0x0000;
 constexpr u16 FAULT_RECEIVE = 0xFFFF;
 constexpr u16 REDDUCTION_RATIO = 25;
+constexpr float VELOCITY_SCALE = 1e4f;
+constexpr float ANGLE_WRAP_THRESHOLD = 270.0f;
+constexpr float ANGLE_WRAP_VALUE = 360.0f;
+constexpr int SIZE_FLOAT = sizeof(float);
 
 // DMA传输完成标志
 volatile bool spi_dma_tx_complete = false;
@@ -203,7 +208,7 @@ float angle_sensor::read_sensorAngle(void) {
 
 	SPI_CS_ENABLE();
 	// 使用DMA发送命令并接收数据
-	if (spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data) == 0) {
+	if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
 		SPI_CS_DISABLE();
 		return u16data * ANGLE_FACTOR;
 	} else {
@@ -218,6 +223,7 @@ float angle_sensor::read_sensorAngle(void) {
  * @return 累积角度值
  */
 float angle_sensor::update(void) {
+
 	float currentAngle = read_sensorAngle();
 
 	if (m_firstUpdate) {
@@ -231,16 +237,16 @@ float angle_sensor::update(void) {
 
 	// 处理角度绕回情况（0度和360度之间的跳变）
 	// 如果变化超过180度，说明发生了绕回
-	if (delta > 270.0f) {
+	if (delta > ANGLE_WRAP_THRESHOLD) {
 		// 从接近360度绕回到接近0度
-		delta -= 360.0f;
+		delta -= ANGLE_WRAP_VALUE;
-	} else if (delta < -270.0f) {
+	} else if (delta < -ANGLE_WRAP_THRESHOLD) {
 		// 从接近0度绕到接近360度
-		delta += 360.0f;
+		delta += ANGLE_WRAP_VALUE;
 	}
 
-	std::memmove(velocity, velocity + 1, VEL_FILTER_SIZE * sizeof(float));
+	std::memmove(velocity, velocity + 1, SIZE_FLOAT * (VEL_FILTER_SIZE - 1));
-	velocity[VEL_FILTER_SIZE - 1] = delta * 1e4;
+	velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE;
 
 	m_cumulativeAngle += delta;
 	m_lastAngle = currentAngle;
@@ -279,7 +285,8 @@ float angle_sensor::getRawVelocity(void) const {
 	for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) {
 		velocity_filted += velocity[i];
 	}
-	return velocity_filted / VEL_FILTER_SIZE;
+	return velocity_filted / (VEL_FILTER_SIZE - 1);
+	// return velocity[VEL_FILTER_SIZE - 1];
 }
 
 /**

Robot_V2/device/ang_sensor.h

@@ -11,7 +11,7 @@ u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
 int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 						 uint16_t *rx_data);
 
-constexpr u16 VEL_FILTER_SIZE = 24;
+constexpr u16 VEL_FILTER_SIZE = 3;
 
 class angle_sensor {
   public:

Robot_V2/main.cpp

@@ -7,7 +7,6 @@
 #include "core_cm4.h"
 #include "initial.h"
 #include "systick.h"
-#include <cstdio>
 
 // 全局变量定义
 CAN_transmiter CAN0_transmiter; // CAN初始化

Robot_V2/pid.h

@@ -8,6 +8,7 @@ public:
     ~PIDController() = default;
 
     float operator() (float error);
+	void set_PID(float P, float I, float D);
 
     float P; //!< 比例增益(P环增益)
     float I; //!< 积分增益（I环增益）