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;
 }
 
 /**

Robot_V2/device/Can.cpp

@@ -537,6 +537,7 @@ void CAN_transmiter::communication_check() {
 			target_current_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 3).f;
 			FOC.current_loop.set_limit(target_current_union.f);
+			FOC.vel_inMITloop.set_limit(target_current_union.f);
 			// 目标速度
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 7).f;
@@ -557,7 +558,6 @@ void CAN_transmiter::communication_check() {
 
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
 			FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
-			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config / voltage;
 
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
 			FOC.angle_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
@@ -565,6 +565,7 @@ void CAN_transmiter::communication_check() {
 
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
 			FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
+			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
 
 		} else if (receive_message.rx_efid == CANFD_RESET_ID) {
 			u8 first_byte = receive_message.rx_data[0];

Robot_V2/device/Timer.cpp

@@ -96,7 +96,6 @@ void TIMER2_IRQHandler(void) {
 		// 每50us执行一次的操作 - 电机控制
 		scheduler.taskFlags |=
 			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
-		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
 
 		// 使用标志位控制，每100us执行一次其他任务
 		static bool isEvenCall = false;
@@ -105,6 +104,7 @@ void TIMER2_IRQHandler(void) {
 		if (isEvenCall) {
 			// 每100us执行一次的操作 - 保持原有频率
 
+			scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
 			switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
 			case 0: {
 				scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);

Robot_V2/device/ang_sensor.cpp

@@ -265,7 +265,6 @@ float angle_sensor::update(void) {
 
 	m_cumulativeAngle += delta;
 	m_lastAngle = current_angle_deg;
-
 	return m_cumulativeAngle;
 }