feat(FOC): 添加电流卡尔曼滤波器并优化MIT控制模式

- 优化MIT控制模式的角度和速度计算
- 调整多个PID控制器的参数配置
- 增强卡尔曼滤波器的数值稳定性

Robot_V2/device/ADC.cpp

@@ -5,9 +5,9 @@
 #include "config.h"
 #include "math.h"
 #include "systick.h"
+#include <cstdint>
 #include <cstring>
 #include <gd32c10x_dma.h>
-#include <cstdint>
 
 constexpr float ADC_SCALE = 3.3f / 4096.0f;
 constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
@@ -16,6 +16,7 @@ constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 extern CAN_transmiter CAN0_transmiter;
 extern FOC_Controller FOC;
+
 sampler sampler;		// 采样器初始化
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!
@@ -167,20 +168,25 @@ float sampler::get_drv_temp() {
  * 通过ADC采样计算各相电流值
  */
 float sampler::get_current(char type) {
+
+	u8 idx = current_sample_index;
 	switch (type) {
 	case 'U': {
 		// return currU_samples[current_sample_index];
-		tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currU_samples[idx];
 		break;
 	}
 	case 'V': {
 		// return currV_samples[current_sample_index];
-		tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currV_samples[idx];
 		break;
 	}
 	case 'W': {
 		// return currW_samples[current_sample_index];
-		tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currW_samples[idx];
 		break;
 	}
 	default: {
@@ -254,8 +260,6 @@ void sampler::update_current() {
 	if (current_sample_index >= ADC_FILTER_SIZE - 1) {
 		current_sample_index = 0;
 	}
-	// 卡尔曼滤波更新
-	float voltage = FOC.voltage_power_supply;
 }
 
 /**