```

feat(DengFOC):
新增了基于三相电流采样的dq轴电流计算函数`cal_Iq_Id`
调整了开环控制中的电压限制逻辑
更新了默认电源电压值为24V。
KHT7823测试代码
```

Robot_V2/Algorithm/DengFOC.cpp

@@ -1,23 +1,41 @@
 #include "DengFOC.h"
+#include "Filter.h"
 #include "Timer.h"
+#include "adc.h"
+#include "pid.h"
 #include "systick.h"
 #include <cmath>
 #include <cstdio>
 #include <gd32c10x.h>
 #include <stdint.h>
 
-
 constexpr double PI = 3.1415926535897932384626433832795f;
+constexpr float _1_SQRT3 = 0.57735026919f;
+constexpr float _2_SQRT3 = 1.15470053838f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11, DIR = 1;
 constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
+constexpr float zero_electric_angle = 0.0f;
+
+extern sampler sampler;
+
+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);
+PIDController vel_loop_M0 = PIDController{2, 0, 0, 100000, 24.0 / 2.0};
+PIDController angle_loop_M0 = PIDController{2, 0, 0, 100000, 100};
+PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800};
+
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
 #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; }
+inline float _electricalAngle(float shaft_angle) {
+	return _normalizeAngle((float)(DIR * PP) * shaft_angle -
+						   zero_electric_angle);
+}
 
 /**
  * @brief 归一化角度
@@ -90,7 +108,7 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
 	angle_el = _normalizeAngle(angle_el);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
-	const float Uq = limit_voltage;
+	const float Uq = voltage_power_supply / 2.0f;
 	const float Ud = 0.0f;
 
 	// 帕克逆变换 (直接计算，避免函数调用开销)
@@ -106,9 +124,9 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
 	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);
+	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);
@@ -141,4 +159,60 @@ void FOC_Controlroller::shut_down() {
 void FOC_Controlroller::resume() {
 	is_shut_down = 0;
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
-}
+}
+
+// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
+float cal_Iq_Id(float current_a, float current_b, float angle_el) {
+	float I_alpha = current_a;
+	float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
+
+	float ct = cos(angle_el);
+	float st = sin(angle_el);
+	// float I_d = I_alpha * ct + I_beta * st;
+	float I_q = I_beta * ct - I_alpha * st;
+	return I_q;
+}
+
+void FOC_Controlroller::setTorque(float Uq, float angle_el) {
+	Uq =
+		_constrain(Uq, -(voltage_power_supply) / 2, (voltage_power_supply) / 2);
+	float Ud = 0;
+	angle_el = _normalizeAngle(angle_el);
+	// 帕克逆变换
+	Ualpha = -Uq * sin(angle_el);
+	Ubeta = Uq * cos(angle_el);
+
+	// 克拉克逆变换
+	Ua = Ualpha + voltage_power_supply / 2;
+	Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
+	Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
+	driver_output(Ua, Ub, Uc);
+}
+
+float FOC_Controlroller::DFOC_M0_Current() {
+	// 使用卡尔曼滤波后的三相电流计算Iq
+	float Iu_filtered = curU_kalman.update(sampler.get_current('U'), Ua);
+	float Iv_filtered = curV_kalman.update(sampler.get_current('V'), Ub);
+	float Iw_filtered = curW_kalman.update(sampler.get_current('W'), Uc);
+
+	// 根据基尔霍夫电流定律验证三相电流总和是否接近0
+	// float current_sum = Iu_filtered + Iv_filtered + Iw_filtered;
+
+	// 用三相滤波后的电流计算Iq（通过Clarke和Park变换）
+	// 方法1: 使用U和V相计算
+	float I_q_M0_ori =
+		cal_Iq_Id(Iu_filtered, Iv_filtered, _electricalAngle(shaft_angle));
+
+	// 方法2: 也可以使用其他两相的组合计算，然后取平均值以提高精度
+	// float I_q_UW = cal_Iq_Id(Iu_filtered, Iw_filtered,
+	// _electricalAngle(shaft_angle)); float I_q_VW = cal_Iq_Id(Iv_filtered,
+	// Iw_filtered, _electricalAngle(shaft_angle)); float I_q_M0_ori = (I_q_UV +
+	// I_q_UW + I_q_VW) / 3.0f;
+
+	return I_q_M0_ori;
+	// return 0;
+}
+void FOC_Controlroller::DFOC_M0_setTorque(float Target) // 电流力矩环
+{
+	//   setTorque(current_loop_M0(Target-DFOC_M0_Current()),_electricalAngle());
+}

Robot_V2/Algorithm/DengFOC.h

@@ -1,9 +1,3 @@
-// 灯哥开源，遵循GNU协议，转载请著名版权！
-// GNU开源协议（GNU General Public License,
-// GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。
-// 该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。
-// 仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源
-// 你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了
 // 函数声明
 #ifndef _DENGFOC_H
 #define _DENGFOC_H
@@ -13,7 +7,7 @@ class FOC_Controlroller {
 	FOC_Controlroller();
 
 	float control_mode = 0;
-	float limit_voltage = 2.0f;
+	float limit_voltage = 0.0f;
 	float limit_velocity = 0.0f;
 	float target_angle = 0.0f;
 	float target_velocity = 0.0f;
@@ -30,10 +24,13 @@ class FOC_Controlroller {
 	float shaft_angle = 0;
 	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
 	float zero_electric_angle = 0;
-	float voltage_power_supply = 12.0f; // 电源电压
+	float voltage_power_supply = 24.0f; // 电源电压
 
 	void driver_output(float Ua, float Ub, float Uc);
 	void setPhaseVoltage(float Uq, float Ud, float angle_el);
+	void setTorque(float Uq,float angle_el);
+	float DFOC_M0_Current();
+	void DFOC_M0_setTorque(float Target);
 };
 
 float _normalizeAngle(float angle);
@@ -42,4 +39,4 @@ float _electricalAngle(float shaft_angle);
 float DFOC_M0_Angle();
 float serial_motor_target();
 
-#endif
+#endif

Robot_V2/Algorithm/pid.cpp

@@ -0,0 +1,52 @@
+#include "pid.h"
+#include "Timer.h"
+#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+
+PIDController::PIDController(float P, float I, float D, float ramp, float limit)
+    : P(P)
+    , I(I)
+    , D(D)
+    , output_ramp(ramp)    // PID控制器加速度限幅
+    , limit(limit)         // PID控制器输出限幅
+    , error_prev(0.0f)
+    , output_prev(0.0f)
+    , integral_prev(0.0f)
+{
+    timestamp_prev = get_system_runtime_us() ;
+}
+
+// PID 控制器函数
+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;
+    
+    // P环
+    float proportional = P * error;
+    // Tustin 散点积分（I环）
+    float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
+    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);
+
+    if(output_ramp > 0){
+        // 对PID的变化速率进行限制
+        float output_rate = (output - output_prev)/Ts;
+        if (output_rate > output_ramp)
+            output = output_prev + output_ramp*Ts;
+        else if (output_rate < -output_ramp)
+            output = output_prev - output_ramp*Ts;
+    }
+    // 保存值（为了下一次循环）
+    integral_prev = integral;
+    output_prev = output;
+    error_prev = error;
+    timestamp_prev = timestamp_now;
+    return output;
+}

Robot_V2/Robot_V2.cproject.yml

@@ -27,6 +27,8 @@ project:
         - file: Algorithm/Filter.h
         - file: Algorithm/DengFOC.cpp
         - file: Algorithm/DengFOC.h
+        - file: Algorithm/pid.cpp
+        - file: pid.h
   components:
     - component: ARM::CMSIS:CORE
     - component: Device:Startup

Robot_V2/device/ADC.cpp

@@ -1,14 +1,16 @@
 #include "adc.h"
 #include "Filter.h"
 #include "math.h"
-#include <cstring>
 #include <cstdio>
+#include <cstring>
 #include <stdint.h>
 
 constexpr float ERROR_ADC_VAL = 255.0f;
 constexpr float ADC_SCALE = 3.3f / 4096.0f;
 constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
 constexpr float VCC_FACTOR = 11.0f;
+// constexpr float CURRENT_OFFSET = 0 - 4096.0f / 2.0f;
+constexpr float CURRENT_OFFSET = 0;
 constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 constexpr float MAX_TEMP = 150.0f;
@@ -18,18 +20,23 @@ constexpr float MIN_CURRENT = -0.1f;
 constexpr float MAX_VCC = 65.0f;
 constexpr float MIN_VCC = -0.1f;
 
-u16 adc_value[7] = {0}; // 显式初始化数组元素为0
+u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 
 /**
  * @brief 采样类初始化
  */
 void sampler::init() {
-	for (int i = 0; i < 7; i++) {
+	for (int i = 0; i < FILTER_SIZE + 1; i++) {
 		mcu_temp_samples[i] =
 			(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
-		currU_samples[i] = adc_value[2] * ADC_SCALE * CURRENT_FACTOR;
-		currV_samples[i] = adc_value[3] * ADC_SCALE * CURRENT_FACTOR;
-		currW_samples[i] = adc_value[4] * ADC_SCALE * CURRENT_FACTOR;
+
+		currU_samples[i] =
+			(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
+		currV_samples[i] =
+			(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
+		currW_samples[i] =
+			(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
+
 		vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
 
 		float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
@@ -43,13 +50,13 @@ void sampler::init() {
  * 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
  */
 float sampler::get_mcu_temp() {
-	tempora_sample = medium_filter(mcu_temp_samples, 7);
+	tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
 	// 检查温度读数是否在有效范围内
 	if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
 		return ERROR_ADC_VAL;
 	}
 	return tempora_sample;
-}	
+}
 
 /**
  * @brief 获取电机驱动温度
@@ -59,7 +66,7 @@ float sampler::get_mcu_temp() {
  * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
  */
 float sampler::get_drv_temp() {
-	tempora_sample = medium_filter(drv_temp_samples, 7);
+	tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1);
 	// 检查温度读数是否在有效范围内
 	if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
 		return ERROR_ADC_VAL;
@@ -77,15 +84,15 @@ float sampler::get_drv_temp() {
 float sampler::get_current(char type) {
 	switch (type) {
 	case 'U': {
-		tempora_sample = medium_filter(currU_samples, 7);
+		tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1);
 		break;
 	}
 	case 'V': {
-		tempora_sample = medium_filter(currV_samples, 7);
+		tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1);
 		break;
 	}
 	case 'W': {
-		tempora_sample = medium_filter(currW_samples, 7);
+		tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1);
 		break;
 	}
 	default: {
@@ -106,11 +113,13 @@ float sampler::get_current(char type) {
  * 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
  */
 float sampler::get_vbus_voltage() {
+	tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
 	// 检查电压读数是否在有效范围内
-	if (vbus_samples[4] > MAX_VCC || vbus_samples[4] < MIN_VCC) {
+	if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC ||
+		vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
 		return ERROR_ADC_VAL;
 	}
-	return vbus_samples[4];
+	return tempora_sample;
 }
 
 /**
@@ -120,36 +129,41 @@ float sampler::get_vbus_voltage() {
  * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
  */
 void sampler::update_mcu_temp() {
-	memmove(mcu_temp_samples, mcu_temp_samples + 1, 6 * sizeof(float));
-	mcu_temp_samples[6] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
+	memmove(mcu_temp_samples, mcu_temp_samples + 1, FILTER_SIZE * sizeof(float));
+	mcu_temp_samples[FILTER_SIZE] =
+		(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
 }
 
 /**
  * @brief 更新MCU温度
  */
 void sampler::update_drv_temp() {
-	memmove(drv_temp_samples, drv_temp_samples + 1, 6 * sizeof(float));
+	memmove(drv_temp_samples, drv_temp_samples + 1, FILTER_SIZE * sizeof(float));
 	float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
-	drv_temp_samples[6] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
+	drv_temp_samples[FILTER_SIZE] =
+		1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
 }
 
 /**
  * @brief 更新相电流
  */
 void sampler::update_current() {
-	memmove(currU_samples, currU_samples + 1, 6 * sizeof(float));
-	memmove(currV_samples, currV_samples + 1, 6 * sizeof(float));
-	memmove(currW_samples, currW_samples + 1, 6 * sizeof(float));
-	
-	currU_samples[6] = adc_value[2] * ADC_SCALE;
-	currV_samples[6] = adc_value[3] * ADC_SCALE;
-	currW_samples[6] = adc_value[4] * ADC_SCALE;
+	memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float));
+	memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float));
+	memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float));
+
+	currU_samples[FILTER_SIZE] =
+		(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
+	currV_samples[FILTER_SIZE] =
+		(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
+	currW_samples[FILTER_SIZE] =
+		(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
 }
 
 /**
  * @brief 更新供电电压
  */
 void sampler::update_vbus_voltage() {
-	memmove(vbus_samples, vbus_samples + 1, 6 * sizeof(float));
-	vbus_samples[6] = adc_value[5] * ADC_SCALE;
+	memmove(vbus_samples, vbus_samples + 1, FILTER_SIZE * sizeof(float));
+	vbus_samples[FILTER_SIZE] = adc_value[5] * ADC_SCALE;
 }

Robot_V2/device/adc.h

@@ -6,29 +6,29 @@ extern "C" {
 #endif
 
 #include "stdint.h"
-
+constexpr u8 FILTER_SIZE = 6;
 class sampler {
-	private:
-	float vbus_samples[7];
-	float currU_samples[7];
-	float currV_samples[7];
-	float currW_samples[7];
-	float mcu_temp_samples[7];
-	float drv_temp_samples[7];
+  private:
+	float vbus_samples[FILTER_SIZE + 1];
+	float currU_samples[FILTER_SIZE + 1];
+	float currV_samples[FILTER_SIZE + 1];
+	float currW_samples[FILTER_SIZE + 1];
+	float mcu_temp_samples[FILTER_SIZE + 1];
+	float drv_temp_samples[FILTER_SIZE + 1];
 
 	float tempora_sample;
+
   public:
 	void init();
 	void update_mcu_temp(void);
 	void update_drv_temp(void);
 	void update_vbus_voltage(void);
 	void update_current(void);
-	
+
 	float get_mcu_temp(void);
 	float get_drv_temp(void);
 	float get_vbus_voltage(void);
 	float get_current(char);
-
 };
 
 #ifdef __cplusplus

Robot_V2/device/tle5012b.cpp

@@ -5,335 +5,131 @@
 #include "stdbool.h"
 #include "systick.h"
 
-#define SPI_TX_OFF()                                                                                \
-	do {                                                                                          \
-		gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \
-	} while(0)
-#define SPI_TX_ON()                                                                             \
-	do {                                                                                      \
-		gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \
-	} while(0)
+#define SPI_TX_OFF()                                                           \
+	do {                                                                       \
+		gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ,             \
+				  GPIO_PIN_5);                                                 \
+	} while (0)
+#define SPI_TX_ON()                                                            \
+	do {                                                                       \
+		gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_5);      \
+	} while (0)
 
-#define SPI_CS_ENABLE()	 gpio_bit_reset(GPIOA, GPIO_PIN_15)
+#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
 #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
 
-
-/**
- * @brief SPI command definitions for TLI5012B sensor
- * @note All commands are 16-bit values following the TLI5012B protocol
- */
-
-/* SPI operation types */
-static constexpr uint16_t WRITE = 0;  /**< SPI Write Operation */
-static constexpr uint16_t READ = 1;   /**< SPI Read Operation */
-
-/* SPI communication constants */
-static constexpr uint16_t DUMMY_BYTE = 0xFFFF;  /**< Dummy byte for SPI read operations */
-
-static constexpr uint16_t WR_REG = 0x0000;  /**< Write command - MSB is 0 */
-static constexpr uint16_t RD_REG = 0x8000;  /**< Read command - MSB is 1 */
-
-static constexpr uint16_t LockValue_LADDR = 0x0000;  /**< Lock value for addresses 0x00-0x04 */
-static constexpr uint16_t LockValue_HADDR = 0x5000;  /**< Lock value for addresses 0x05-0x11 */
-
-/* Update buffer access command */
-static constexpr uint16_t UPD_CMD = 0x0400;  /**< Update command - bit 10 is 1, access update buffer */
-
-/* Read command values */
-static constexpr uint16_t READ_STAT_VALUE = 0x8001;   /**< Read status register */
-static constexpr uint16_t READ_ACSTAT_VALUE = 0x8011; /**< Read activation status register */
-static constexpr uint16_t READ_ANGLE_VALUE = 0x8021;  /**< Read angle value register */
-static constexpr uint16_t READ_SPEED_VALUE = 0x8031;  /**< Read speed value register */
-
-/* Write command values and their corresponding data */
-static constexpr uint16_t WRITE_MOD1_VALUE = 0x5061;  /**< Write to MOD1 register (0_1010_0_000110_0001) */
-static constexpr uint16_t MOD1_VALUE = 0x0001;        /**< MOD1 register configuration value */
-
-static constexpr uint16_t WRITE_MOD2_VALUE = 0x5081;  /**< Write to MOD2 register (0_1010_0_001000_0001) */
-static constexpr uint16_t MOD2_VALUE = 0x0809;        /**< MOD2 register configuration value */
-
-static constexpr uint16_t WRITE_MOD3_VALUE = 0x5091;  /**< Write to MOD3 register (0_1010_0_001001_0001) */
-static constexpr uint16_t MOD3_VALUE = 0x0000;        /**< MOD3 register configuration value */
-
-static constexpr uint16_t WRITE_MOD4_VALUE = 0x50E1;  /**< Write to MOD4 register (0_1010_0_001110_0001) */
-static constexpr uint16_t MOD4_VALUE = 0x0080;        /**< MOD4 register value: 12-bit 4096, absolute count disable 0x0080; enable 0x0000 */
-
 /**
  * @brief Send or read half word through SPI
  *
  * @param[in] spi_periph SPI peripheral
  * @param[in] i_HalfWord data
  */
-uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
-{
-    uint32_t to;
-    uint16_t data = 0xFFFF;      
-    bool ok = true;
+uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord) {
+	uint32_t to;
+	uint16_t data = 0xFFFF;
+	bool ok = true;
 
-    /* ---------- Send data ---------- */
-    to = 200;
-    while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to);
-    if (to == 0) ok = false;
+	/* ---------- Send data ---------- */
+	to = 200;
+	while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to)
+		;
+	if (to == 0)
+		ok = false;
 
-    if (ok)
-    {
-        if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) 
-        {
-            (void)spi_i2s_data_receive(spi_periph);   /* throw Dummy */               
-        }
-        spi_i2s_data_transmit(spi_periph, i_HalfWord);
-        /* wait rx succ*/
-        to = 200;
-        while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) && --to);
-        if (to == 0) ok = false;
-        else
-            data = spi_i2s_data_receive(spi_periph);   /* throw Dummy */
-    }
+	if (ok) {
+		if (RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) {
+			(void)spi_i2s_data_receive(spi_periph); /* throw Dummy */
+		}
+		spi_i2s_data_transmit(spi_periph, i_HalfWord);
+		/* wait rx succ*/
+		to = 200;
+		while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) && --to)
+			;
+		if (to == 0)
+			ok = false;
+		else
+			data = spi_i2s_data_receive(spi_periph); /* throw Dummy */
+	}
 
-    /* ---------- wait bsu idle ---------- */
-    if (ok)
-    {
-        to = 200;
-        while ((SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS))  \
-        && SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) && --to);
-        if (to == 0) ok = false;
-    }
+	/* ---------- wait bsu idle ---------- */
+	if (ok) {
+		to = 200;
+		while ((SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS)) &&
+			   SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) && --to)
+			;
+		if (to == 0)
+			ok = false;
+	}
 
-    return ok ? data : 0xFFFF;
+	return ok ? data : 0xFFFF;
 }
 
-/**
- * @brief Write data to TLI5012B register
- * @param i_Cmd Command word to send
- * @param i_Data Data to write to register
- * @return Register response value, 0xFFFF if error
- * @note This function performs a complete SPI write transaction:
- */
-uint16_t cdd_TLI5012_WriteReg(uint16_t i_Cmd, uint16_t i_Data)
-{
-    uint16_t result = 0xFFFF;          /* Default error code */
-    SPI_CS_ENABLE();
-    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
-    result = spi_rw_half_word(SPI0, i_Cmd);
-    result = spi_rw_half_word(SPI0, i_Data);
-    SPI_TX_OFF();  /* Configure MOSI as input to avoid conflicts */
-    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
-    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
-    result = spi_rw_half_word(SPI0, DUMMY_BYTE);
-    SPI_CS_DISABLE();
-    SPI_TX_ON();  /* Restore MOSI as output */
-    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
-    return result;
-}
-
-/**
- * @brief Read data from TLI5012B register
- * @param i_Cmd Command word to send
- * @param i_Data Not used in read operation (kept for compatibility)
- * @return Register data value, 0xFFFF if error
- * @note This function performs a complete SPI read transaction:
- */
-uint16_t cdd_TLI5012_ReadReg(uint16_t i_Cmd, uint16_t * const i_Data)
-{
-    uint16_t result = 0xFFFF;          /* Default error code */
-    SPI_CS_ENABLE();
-    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
-    *i_Data = spi_rw_half_word(SPI0, i_Cmd);
-    SPI_TX_OFF();  /* Configure MOSI as input to avoid conflicts */
-    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
-    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
-    *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE);    
-    result = spi_rw_half_word(SPI0, DUMMY_BYTE);
-    SPI_CS_DISABLE();
-    SPI_TX_ON();  /* Restore MOSI as output */
-    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
-    return result;
-}
-
-void tle5012b_gpio_init(void)
-{
+void KHT7823_gpio_init(void) {
 	rcu_periph_clock_enable(RCU_GPIOA);
-    rcu_periph_clock_enable(RCU_GPIOB);
-    rcu_periph_clock_enable(RCU_SPI0);
-    rcu_periph_clock_enable(RCU_AF);
-    /* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
-    gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
-
-    /* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
-    gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
-    /* PA15 → NSS, 推挽输出 */
-    gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
-    /* PB3 → SCK, PB5 → MOSI, 推挽输出 */
-    gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_3 | GPIO_PIN_5);
-    /* PB4 → MISO, 浮空输入 */
-    gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4 | GPIO_PIN_10);
+	rcu_periph_clock_enable(RCU_GPIOB);
+	rcu_periph_clock_enable(RCU_SPI0);
+	rcu_periph_clock_enable(RCU_AF);
+	/* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
+	gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
 
+	/* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
+	gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
+	/* PA15 → NSS, 推挽输出 */
+	gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
+	/* PB3 → SCK, PB5 → MOSI, 推挽输出 */
+	gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ,
+			  GPIO_PIN_3 | GPIO_PIN_5);
+	/* PB4 → MISO, 浮空输入 */
+	gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ,
+			  GPIO_PIN_4 | GPIO_PIN_10);
 }
 /**
  * @brief Initialize SPI module
  *
  * @details Configures SPI hardware and initializes necessary settings
  */
-void tle5012b_spi_init(void)
-{
-    spi_parameter_struct spi_init_str;
-    spi_struct_para_init(&spi_init_str);
+void KHT7823_spi_init(void) {
+	spi_parameter_struct spi_init_str;
+	spi_struct_para_init(&spi_init_str);
 
-    /* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */
-    spi_init_str.device_mode          = SPI_MASTER;
-    spi_init_str.trans_mode           = SPI_TRANSMODE_FULLDUPLEX;
-    spi_init_str.frame_size           = SPI_FRAMESIZE_16BIT;
-    spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
-    spi_init_str.nss                  = SPI_NSS_SOFT;      
-    spi_init_str.endian               = SPI_ENDIAN_MSB;
+	/* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */
+	spi_init_str.device_mode = SPI_MASTER;
+	spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
+	spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
+	spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
+	spi_init_str.nss = SPI_NSS_SOFT;
+	spi_init_str.endian = SPI_ENDIAN_MSB;
 
-    spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */
-    spi_init(SPI0, &spi_init_str);
-    spi_nss_output_enable(SPI0);  
-    spi_nssp_mode_enable(SPI0);
-    spi_enable(SPI0); 
-} 
-
-
-
-/**
- * @brief Initialize TLI5012B magnetic angle sensor
- * @note This function configures the sensor registers for proper operation
- * @return None
- */
-void cdd_TLI5012_Init(void)
-{
-	uint16_t u16Command = 0xFFFF;
-	uint16_t u16Data = 0x0000;
-	uint16_t u16Safe = 0x0000;
-	
-	/* ---------- Step 1: Read and configure STAT register ---------- */
-	/* Read STAT register to get current status */
-	u16Command = READ_STAT_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Write STAT register, set Slave_Number = 00 */
-	u16Command = 0x0001;
-	u16Data &= 0x9FFF;  /* Clear bits 13-14 (Slave_Number) */
-	u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
-	printf("\r\n @Joe [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Read STAT register again to verify write operation */
-	u16Command = READ_STAT_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* ---------- Step 2: Read ACSTAT register ---------- */
-	u16Command = READ_ACSTAT_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* ---------- Step 3: Configure MOD1 register ---------- */
-	/* Read current MOD1 register value */
-	u16Command = RD_REG | WRITE_MOD1_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Write MOD1 register with configuration value */
-	u16Command = WRITE_MOD1_VALUE;
-	u16Data = MOD1_VALUE;
-	u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
-	printf("\r\n @Joe [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);    
-	
-	/* Verify MOD1 register write operation */
-	u16Command = RD_REG | WRITE_MOD1_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* ---------- Step 4: Configure MOD2 register ---------- */
-	/* Read current MOD2 register value */
-	u16Command = RD_REG | WRITE_MOD2_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Write MOD2 register with configuration value */
-	u16Command = WRITE_MOD2_VALUE;
-	u16Data = MOD2_VALUE;
-	u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
-	printf("\r\n @Joe [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Verify MOD2 register write operation */
-	u16Command = RD_REG | WRITE_MOD2_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);    
-
-	/* ---------- Step 5: Configure MOD3 register ---------- */
-	/* Read current MOD3 register value */
-	u16Command = RD_REG | WRITE_MOD3_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Write MOD3 register with configuration value */
-	u16Command = WRITE_MOD3_VALUE;
-	u16Data = MOD3_VALUE;
-	u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
-	printf("\r\n @Joe [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Verify MOD3 register write operation */
-	u16Command = RD_REG | WRITE_MOD3_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* ---------- Step 6: Configure MOD4 register ---------- */
-	/* Read current MOD4 register value */
-	u16Command = RD_REG | WRITE_MOD4_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Write MOD4 register with configuration value */
-	u16Command = WRITE_MOD4_VALUE;
-	u16Data = MOD4_VALUE;
-	u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
-	printf("\r\n @Joe [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
-	
-	/* Verify MOD4 register write operation */
-	u16Command = RD_REG | WRITE_MOD4_VALUE;
-	u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-	printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);    
+	spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */
+	spi_init(SPI0, &spi_init_str);
+	spi_nss_output_enable(SPI0);
+	spi_nssp_mode_enable(SPI0);
+	spi_enable(SPI0);
 }
 
+void KHT7823_init(void) {
+	KHT7823_gpio_init();
+	KHT7823_spi_init();
+}
 
+float KHT7823_readAngle() {
+	u16 u16Command = 0x4000;
 
-void Test_TLI5012B(void)
-{
-    uint16_t u16Command = 0xFFFF;
-    uint16_t u16Data = 0x0000;
-    uint16_t u16Safe = 0x0000;
-    float angle = 0;  
-    u16Command = READ_ANGLE_VALUE;
-    u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
-    
-    // 输出原始读取的数据，用于诊断
-    printf("\r\n @Joe RAW AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
-    
-    // 检查是否有错误标志位
-    if (u16Safe != 0xFFFF && u16Safe != 0x0000) {
-        printf("\r\n @Joe Warning: Non-ideal SafeWord detected \r\n");
-    }
-    
-    // 检查错误位（bit 15）
-    if (u16Data & 0x8000) {
-        printf("\r\n @Joe Error: Communication error detected (bit 15 set) \r\n");
-    }
-    
-    // 提取角度值(15位)
-    u16Data = u16Data & 0x7FFF;
-    printf ( "\r\n @Joe AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
-    
-    // 将15位角度值转换为角度(0-360度)
-    // 0x7FFF = 32767 对应 360度
-    angle = ((float)u16Data * 360.0f) / 32767.0f;
-    printf ( "\r\n @Joe angle = %f, AVL = %d \r\n", angle, (int16_t)u16Data);
-    
-    // 添加额外的传感器状态检查
-    uint16_t status_data, status_safe;
-    u16Command = READ_STAT_VALUE;
-    status_safe = cdd_TLI5012_ReadReg(u16Command, &status_data);
-    printf("\r\n @Joe STATUS = 0x%04X, Status SafeWord = 0x%04X \r\n", status_data, status_safe);
+	float ang = 0;
+
+	SPI_CS_ENABLE();
+	// 2. 发送命令1，同时接收回答1
+	SPI_DATA(SPI0) = u16Command;
+	while (RESET == spi_i2s_flag_get(SPI0, SPI_FLAG_TBE))
+		; // 等待发送缓冲区空
+	while (RESET == spi_i2s_flag_get(SPI0, SPI_FLAG_RBNE))
+		;						  // 等待接收缓冲区满
+	u16 u16data = SPI_DATA(SPI0); // 读取回答1
+
+	SPI_CS_DISABLE();
+
+	// 将15位角度值转换为角度(0-360度)
+	ang = ((float)u16data * 360.0f) / 32767.0f;
+
+	return ang;
 }

Robot_V2/device/tle5012b.h

@@ -6,13 +6,9 @@ extern "C" {
 #include "gd32c10x_eval.h"
 #include "stdio.h"
 
-void tle5012b_gpio_init(void);
-void tle5012b_spi_init(void);
+void KHT7823_init(void);
 u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
-u16 cdd_TLI5012_WriteReg(u16 i_Cmd, u16 i_Data);
-u16 cdd_TLI5012_ReadReg(u16 i_Cmd, u16 *const i_Data);
-void cdd_TLI5012_Init(void);
-void Test_TLI5012B(void);
+void KHT7823_readAngle(void);
 #ifdef __cplusplus
 }
 #endif

Robot_V2/main.cpp

@@ -15,7 +15,6 @@
 #include <cstdio>
 
 // 全局变量定义
-CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CAN_transmiter CAN0_transmiter;
 FOC_Controlroller FOC;
 sampler sampler;
@@ -29,14 +28,14 @@ int main(void) {
 	sampler.init();		// 采样器初始化
 	driver_init();	   // 电机驱动初始化
 
-	tle5012b_gpio_init(); // TLI5012B初始化
-	tle5012b_spi_init();
+	KHT7823_init();
 
 	watchdog_init(5000);
 	
 	while (1) {
 		/* clear the FWDGT reset flag */
 		// std::printf("1\n");
+		// KHT7823_init();
 		fwdgt_counter_reload();
 		CAN0_transmiter.communication_check();
 	}

Robot_V2/pid.h

@@ -0,0 +1,24 @@
+#ifndef PID_H
+#define PID_H
+
+class PIDController
+{
+public:
+    PIDController(float P, float I, float D, float ramp, float limit);
+    ~PIDController() = default;
+
+    float operator() (float error);
+
+    float P; //!< 比例增益(P环增益)
+    float I; //!< 积分增益（I环增益）
+    float D; //!< 微分增益（D环增益）
+    float output_ramp; 
+    float limit; 
+protected:
+    float error_prev; //!< 最后的跟踪误差值
+    float output_prev;  //!< 最后一个 pid 输出值
+    float integral_prev; //!< 最后一个积分分量值
+    unsigned long timestamp_prev; //!< 上次执行时间戳
+};
+
+#endif