Merge branch 'master' of http://172.30.0.1:8418/cbb/Robot_v2

Robot_V2/Algorithm/FOC.cpp

@@ -25,6 +25,7 @@ extern CurrentKalman Current_kalman;
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
+<<<<<<< HEAD
 // 校准步骤（代码模板）
 void calibrate_zero_position(void) {
 	// Set_3Pwm(.25, 0, 0); // 起劲
@@ -49,14 +50,23 @@ void calibrate_zero_position(void) {
 	// Set_3Pwm(0, 0, 0); // 停止
 }
 
+=======
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
 inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 									   float current_c) {
 	// 计算电气角度
+<<<<<<< HEAD
 	float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
+=======
+	float angle_el = fmod(shaft_angle * PP, 360.0f);
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 
 	// Clarke变换 (使用三相电流)
+<<<<<<< HEAD
 	float I_alpha, I_beta;
+=======
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
 			  (1.0f / 3.0f) * current_c;
 	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -66,11 +76,17 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
 	float I_q, I_d;
 	I_q = I_beta * cos_el - I_alpha * sin_el;
+<<<<<<< HEAD
 	I_d = I_beta * sin_el + I_alpha * cos_el;
 
 	// 返回Iq值，乘以1000以转换为mA
 	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
 	// return I_q * 1000 - KE_FACTOR * rt_velocity;
+=======
+
+	return I_q * 1000.0f -
+		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 }
 
 /**
@@ -80,6 +96,7 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
  */
 u8 FOC_Controller::init() {
 	motor_init(); // 电机驱动初始化
+<<<<<<< HEAD
 
 	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
 	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
@@ -89,7 +106,22 @@ u8 FOC_Controller::init() {
 	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
 	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
 	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
+=======
+	vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
+					 vel_loop_Kd); // 设置电机速度控制参数
+	angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
+					   0.2f); // 设置电机角度控制参数
+	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
+						 7e-8); // 设置电机电流控制参数
 
+	vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
+							 vel_angloop_Kd); // 设置电机速度控制参数
+	ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
+							 ang_angloop_Ki,
+							 ang_angloop_Kd); // 设置电机角度控制参数
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
+
+	CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
 	return 0;
 }
 
@@ -140,6 +172,107 @@ inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
 	duty_b = Ub / voltage_power_supply * voltage_exchange;
 	duty_c = Uc / voltage_power_supply * voltage_exchange;
 
+<<<<<<< HEAD
+=======
+	Set_3Pwm(duty_a, duty_b, duty_c);
+}
+
+/**
+ * @brief 使用SVPWM算法设置电机输出（优化版）
+ *
+ * @param Uq: q轴电压
+ * @param Ud: d轴电压
+ */
+inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
+	// 计算电气角度（保持原有角度单位和范围）
+	float angle_el = fmod(shaft_angle * PP, 360.0f);
+
+	// 帕克逆变换
+	float sin_el, cos_el;
+	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
+	float Ualpha = Ud * cos_el - Uq * sin_el;
+	float Ubeta = Ud * sin_el + Uq * cos_el;
+
+	// 计算角度并归一化到 [0, 2π)
+	float angle = atan2f(Ubeta, Ualpha);
+	if (angle < 0)
+		angle += 2.0f * PI;
+
+	// 扇区与局部角度
+	int sector = (int)(angle * INV_PI_3) + 1;
+	if (sector < 1)
+		sector = 1;
+	else if (sector > 6)
+		sector = 6;
+	float local_angle = angle - (sector - 1) * PI_3;
+
+	// 模长计算
+	float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);
+
+	// 计算电压调制比，同时限制最大输出电压
+	float modulation_index =
+		(magnitude / voltage_power_supply); // 90%作为安全裕量
+
+	// 电压限制
+	if (limit_voltage > voltage_power_supply) {
+		limit_voltage = voltage_power_supply;
+	}
+	float limit_percent = limit_voltage / v_safe;
+	if (modulation_index > limit_percent) {
+		// 限制电压
+		modulation_index = limit_percent;
+		// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
+		magnitude = voltage_power_supply * limit_percent;
+	}
+
+	// 使用arm_sin_cos_f32计算local_angle的sin和cos
+	float sin_l, cos_l;
+	arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
+	float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
+	float T2 = modulation_index * sin_l;
+	float T0 = 1.0f - T1 - T2;
+
+	// 计算占空比（按规范化时间映射）
+	switch (sector) {
+	case 1:
+		duty_a = 0.5f * T0 + T1 + T2;
+		duty_b = 0.5f * T0 + T2;
+		duty_c = 0.5f * T0;
+		break;
+	case 2:
+		duty_a = 0.5f * T0 + T1;
+		duty_b = 0.5f * T0 + T1 + T2;
+		duty_c = 0.5f * T0;
+		break;
+	case 3:
+		duty_a = 0.5f * T0;
+		duty_b = 0.5f * T0 + T1 + T2;
+		duty_c = 0.5f * T0 + T2;
+		break;
+	case 4:
+		duty_a = 0.5f * T0;
+		duty_b = 0.5f * T0 + T1;
+		duty_c = 0.5f * T0 + T1 + T2;
+		break;
+	case 5:
+		duty_a = 0.5f * T0 + T2;
+		duty_b = 0.5f * T0;
+		duty_c = 0.5f * T0 + T1 + T2;
+		break;
+	case 6:
+		duty_a = 0.5f * T0 + T1 + T2;
+		duty_b = 0.5f * T0;
+		duty_c = 0.5f * T0 + T1;
+		break;
+	default:
+		duty_a = 0.0f;
+		duty_b = 0.0f;
+		duty_c = 0.0f;
+		break;
+	}
+
+	// 设置PWM输出
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 	Set_3Pwm(duty_a, duty_b, duty_c);
 }
 
@@ -249,7 +382,11 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
  *
  */
 void FOC_Controller::shut_down() {
+<<<<<<< HEAD
 	drv_stop();
+=======
+	is_shut_down = 1;
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 	motor_sleep();
 	is_shut_down = true;
 }
@@ -313,7 +450,11 @@ void FOC_Controller::update_power(float voltage) {
 		voltage_error = false;
 		voltage_power_supply = voltage;
 		v_half = voltage * 0.5f;
+<<<<<<< HEAD
 		v_safe = voltage * 1.1f;
+=======
+		v_safe = voltage * 1.05f;
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 	} else {
 		voltage_error = true;
 	}
@@ -328,6 +469,7 @@ void FOC_Controller::update_power(float voltage) {
 float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
+<<<<<<< HEAD
 										 MOTOR_FREQUENCY / 2,
 					   360.0f);
 	// 获取电机轴角度
@@ -339,6 +481,19 @@ float FOC_Controller::velocity_open_loop() {
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
+=======
+										 TIMER2_FREQUENCY,
+					   360.0f);
+
+	// 因为在开环控制中，Ud始终为0，只控制Uq
+	const float Uq = voltage_power_supply / 2.0f;
+
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+	// CAN0_transmiter.testfloat_union.f = Uq;
+	// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+	// 		  sampler.get_current('W'));
+
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 	return limit_voltage;
 }
 
@@ -358,6 +513,84 @@ float FOC_Controller::angle_control() {
 						   sampler.get_current('W'));
 	// 设置相电压
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
+<<<<<<< HEAD
+=======
+	return shaft_angle;
+}
+
+/**
+ * @brief 带速度限制的角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controller::angle_control_with_velocity_limit() {
+	// 计算角度误差
+	float angle_error = target_angle - rt_angle;
+
+	// 使用角度PID控制器计算目标速度
+	float target_velocity = ang_inVelAngloop(angle_error);
+
+	// 使用速度PID控制器计算最终输出电压
+	// 根据目标速度大小，选择不同的PID参数
+	if (target_velocity < -120.0f) {
+		vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
+	} else if (target_velocity < -30.0f) {
+		vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
+	} else {
+		vel_inVelAngloop.set_P(vel_angloop_p);
+	}
+	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
+
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+
+	return shaft_angle;
+}
+
+/**
+ * @brief 带速度限制的角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controller::MIT_Mode() {
+	// 第一层：角度环（外环）
+	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
+	float angle_error = rt_angle - target_angle;
+	float target_velocity =
+		ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
+
+	// 第二层：速度环（中环）
+	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
+	float velocity_error = target_velocity - rt_velocity;
+	float target_current =
+		vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
+
+	CAN0_transmiter.testfloat_union.f = target_current;
+	// 第三层：电流环（内环）
+	// 计算当前实际电流
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+
+	// 通过电流环PID控制器计算输出电压
+	float current_error = target_current - rt_current;
+	float Uq = current_loop(current_error);
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+
+>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f
 	return shaft_angle;
 }
 

Robot_V2/Algorithm/FOC.h

@@ -16,10 +16,12 @@ class FOC_Controller {
 	u8 control_mode = 0;
 	float limit_voltage = 0.0f;
 
+
 	float target_angle = 0.0f;
 	float target_velocity = 0.0f;
 	float target_current = 0.0f;
 
+
 	float rt_angle = 0.0f;
 	float rt_velocity = 0.0f;
 	float rt_current = 0.0f;
@@ -100,7 +102,10 @@ class FOC_Controller {
 	float Ualpha, Ubeta = 0;
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
+	float startup_ramp_val = 0.0f;      // 软启动系数
+	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
 	void setPhaseVoltage(float Uq, float Ud);
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);
 };
 

Robot_V2/Algorithm/pid.cpp

@@ -68,23 +68,4 @@ void PIDController::set_PID(float P, float I, float D) {
  * @param P: P参数
  * @return void
  */
-void PIDController::set_P(float pp) { this->P = pp; }
-
-/**
- * @brief 设置输出限幅
- *
- * @param limit: 输出限幅
- * @return void
- */
-void PIDController::set_limit(float limit) { this->limit = limit; }
-
-/**
- * @brief 重置PID控制器状态
- *
- * @return void
- */
-void PIDController::reset() {
-	error_prev = 0.0f;
-	output_prev = 0.0f;
-	integral_prev = 0.0f;
-}
+void PIDController::set_P_percent(float pp) { this->P = pp; }

Robot_V2/device/ADC.cpp

@@ -15,8 +15,7 @@ constexpr float VCC_FACTOR = 11.0f;
 constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controller FOC;
-
+extern FOC_Controlroller FOC;
 sampler sampler;		// 采样器初始化
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!

Robot_V2/device/Can.cpp

@@ -19,7 +19,9 @@
 #include <random>
 
 constexpr u16 CANX = CAN0;
-extern FOC_Controller FOC;
+
+CAN_transmiter CAN0_transmiter; // CAN初始化
+extern FOC_Controlroller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern EncoderAngleKalman Angle_kalman;	 // 角度滤波
@@ -573,47 +575,37 @@ void CAN_transmiter::communication_check() {
 		// }
 		if (current_message.rx_efid == CANFD_CMD_ID) {
 			// 控制模式
-			FOC.control_mode = current_message.rx_data[0];
-			// 目标电压限制
+			FOC.control_mode = receive_message.rx_data[0];
+			// 目标角度
 			FOC.limit_voltage =
-				float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
-			// 目标电流
-			target_current_union.f =
-				unpack_bytes_to_float(current_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(current_message.rx_data, 7).f;
-			FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
-			FOC.ang_inMITloop.set_limit(target_velocity_union.f);
+				float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
+			// 目标电流&目标速度
+			FOC.limit_velocity =
+				unpack_bytes_to_float(receive_message.rx_data, 3).f;
+			FOC.target_current = FOC.limit_velocity;
 			// 目标角度
 			target_angle_union.f =
-				unpack_bytes_to_float(current_message.rx_data, 11).f;
-			// 赋值
+				unpack_bytes_to_float(receive_message.rx_data, 7).f;
 			FOC.target_angle = target_angle_union.f;
+			// 目标速度
+			target_velocity_union.f =
+				unpack_bytes_to_float(receive_message.rx_data, 11).f;
 			FOC.target_velocity = target_velocity_union.f;
 			FOC.target_current = target_current_union.f;
 
 			// 解析PID参数
 			float voltage = FOC.limit_voltage;
-			// PID参数
-			u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
-			FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
-			FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
-
-			P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
-			FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
-			FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
-			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
-
-			P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
-			FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
-			FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
-			FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
-		} else if (current_message.rx_efid == CANFD_RESET_ID) {
-			u8 first_byte = current_message.rx_data[0];
-			u8 eigth_byte = current_message.rx_data[7];
+			FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage);
+			P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
+			FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage);
+			P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
+			FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage);
+			P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
+			FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config /
+											   voltage);
+		} else if (receive_message.rx_efid == CANFD_RESET_ID) {
+			u8 first_byte = receive_message.rx_data[0];
+			u8 eigth_byte = receive_message.rx_data[7];
 			// 电机重置
 			if (first_byte & 0x01) {
 				motor_reset();

Robot_V2/device/Timer.cpp

@@ -11,7 +11,7 @@
 
 volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
 extern u16 adc_value[7];
-extern FOC_Controller FOC;
+extern FOC_Controlroller FOC;
 extern TaskScheduler scheduler;
 extern CAN_transmiter CAN0_transmiter;
 // 添加系统运行时间计数器
@@ -131,49 +131,79 @@ void TIMER2_IRQHandler(void) {
     }
     timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
 
-    // 固定任务：每 50us 执行电机控制
-    scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) | 
-                           (1u << TASK_VELOCITY_UPDATE) | 
-                           (1u << TASK_MOTOR_CONTROL);
+		// 每50us执行一次的操作 - 电机控制
+		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
+		scheduler.taskFlags |=
+			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
 
-    // 每 100us 执行一次的任务（使用位运算替代布尔变量）
-    static u32 call_counter = 0;
-    call_counter++;
-    
-    if ((call_counter & 1u) == 0u) {  // 偶数次调用（每 100us）
-        // 使用查找表设置 1ms 周期内的任务
-        scheduler.taskFlags |= task_schedule_100us[total_us100_counter];
-        
-        total_us100_counter++;
-        if (total_us100_counter >= 10u) {
-            total_us100_counter = 0;
-            
-            // 每 1ms 执行的任务
-            static u32 ms1_counter = 0;
-            ms1_counter++;
-            if (ms1_counter >= 10u) {
-                ms1_counter = 0;
-                
-                // 每 10ms 执行的任务
-                static u32 ms10_counter = 0;
-                ms10_counter++;
-                if (ms10_counter >= 10u) {
-                    ms10_counter = 0;
-                    
-                    // 每 100ms 执行的任务
-                    static u32 ms100_counter = 0;
-                    ms100_counter++;
-                    if (ms100_counter >= 10u) {
-                        ms100_counter = 0;
-                        // 每 1s 任务预留位置
-                    }
-                    scheduler.taskFlags |= task_schedule_100ms[ms100_counter];
-                }
-                scheduler.taskFlags |= task_schedule_10ms[ms10_counter];
-            }
-            scheduler.taskFlags |= task_schedule_1ms[ms1_counter];
-        }
-    }
+		// 使用标志位控制，每100us执行一次其他任务
+		static bool isEvenCall = false;
+		isEvenCall = !isEvenCall;
+
+		if (isEvenCall) {
+			// 每100us执行一次的操作 - 保持原有频率
+
+			switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
+			case 0: {
+				scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
+				break;
+			}
+			case 1: {
+				scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
+				break;
+			}
+			case 2: {
+				scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
+				break;
+			}
+			case 3: {
+				break;
+			}
+			case 9: {
+				scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
+				break;
+			}
+			}
+
+			total_us100_counter++;
+			if (total_us100_counter > 9) {
+				total_us100_counter = 0;
+				// 可以添加每1ms执行的代码，无负载均衡
+				static u8 ms1_counter = 0;
+
+				switch (ms1_counter) { // 每10ms执行的代码，负载均衡
+				case 0: {
+					scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
+					break;
+				}
+				case 1: {
+					scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
+					break;
+				}
+				case 2: {
+					break;
+				}
+				}
+				ms1_counter++;
+				if (ms1_counter > 9) {
+					ms1_counter = 0;
+					// 可以添加每10ms执行的代码，无负载均衡
+					static u8 ms10_counter = 0;
+					ms10_counter++;
+					if (ms10_counter > 9) {
+						ms10_counter = 0;
+						// 可以添加每100ms执行的代码
+						static u8 sec_counter = 0;
+						sec_counter++;
+						if (sec_counter > 9) {
+							sec_counter = 0;
+							// 可以添加每1s执行的代码
+						}
+					}
+				}
+			}
+		}
+	}
 }
 
 /**

Robot_V2/main.cpp

@@ -16,7 +16,7 @@ extern angle_sensor KTH7823;				// 角度传感器
 extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
 extern EncoderAngleKalman Angle_kalman;		// 角度滤波
 extern ThermalController thermalController; // 热量控制器
-extern FOC_Controller FOC;					// FOC控制
+extern FOC_Controller FOC;				// FOC控制
 extern FlashEEPROM flashEEP;				// EEPROM类
 extern VersionManager Version;
 
@@ -95,35 +95,21 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
 		motor_runtime_50us++;
 
-		// 静态变量用于跟踪上一次的控制模式
-		static int last_control_mode = -1;
+		// 函数指针数组，包含各种控制模式对应的函数
+		static FOCFunctionPtr control_functions[] = {
+			&FOC_Controlroller::drv_hold, // 0: 占空比置
+			&FOC_Controlroller::
+				angle_control_with_velocity_limit, // 1:
+												   // 角度闭环控制，带速度限制
+			&FOC_Controlroller::velocity_control,  // 2: 速度闭环控制
+			&FOC_Controlroller::current_control,   // 3: 电流闭环控制
+			&FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制
+		};
 
 		// 边界检查，防止数组越界
-		if (FOC.control_mode >= 0 &&
-			FOC.control_mode < 7) { // 更新为7个控制模式
-
-			// 检查控制模式是否发生变化
-			if (FOC.control_mode != last_control_mode) {
-				// 重置所有PID控制器的状态，避免积分项累积
-				FOC.vel_loop.reset();
-				FOC.angle_loop.reset();
-				FOC.current_loop.reset();
-				FOC.vel_inVelAngloop.reset();
-				FOC.ang_inVelAngloop.reset();
-				FOC.ang_inMITloop.reset();
-				FOC.vel_inMITloop.reset();
-				FOC.current_inMITloop.reset();
-
-				// 更新上次控制模式
-				last_control_mode = FOC.control_mode;
-			}
-			if (!FOC.is_shut_down) {
-				// 调用对应的控制函数
-				(FOC.*control_functions[FOC.control_mode])();
-			} else {
-				// 调用对应的控制函数
-				FOC.drv_stop();
-			}
+		if (FOC.control_mode >= 0 && FOC.control_mode < 5) {  // 更新为7个控制模式
+			// 调用对应的控制函数
+			(FOC.*control_functions[FOC.control_mode])();
 		}
 	}
 	if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值

Robot_V2/sys/config.h

@@ -1,6 +1,5 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
-#include "gd32c10x_eval.h"
 #include <cstdint>
 
 // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
@@ -88,50 +87,17 @@ constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f;		// 降低过程噪声
 constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声
 
 // PID配置
-constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
-constexpr float current_loop_Ki = 0.55f;				  // 电流P
-constexpr float current_loop_Kd = 3e-4f;				  // 电流P
-
-constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
-constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
-constexpr float vel_loop_Tu = 17.0f * 1e-3f;
-constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
-constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
-
-constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
-constexpr float vel_angloop_Ki = 5.0f;				// 速度 速度-角度I
-constexpr float vel_angloop_Kd = 5e-05f;			// 速度 速度-角度D
-
-constexpr float DEFAULT_angle_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
-constexpr float ang_angloop_Ki = 0.00f;						 // 角度 速度-角度I
-constexpr float ang_angloop_Kd = 8e-04f;					 // 角度 速度-角度D
-
-constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
-
-constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P
-constexpr float angle_MIT_Ki = 0.0f;
-constexpr float angle_MIT_Kd = 8e-04f;
-
-constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
-constexpr float vel_MIT_Ki = 15.0f;
-constexpr float vel_MIT_Kd = 1e-04f;
-
-constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
-constexpr float current_MIT_Ki = 0.1f;
-constexpr float current_MIT_Kd = 3e-4f;
+constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P
+constexpr float vel_loop_p = 0.01f / 100 * 12;	   // 速度P
+constexpr float angle_loop_p = 20.0f / 100 * 12;	   // 角度P
+constexpr float vel_angloop_p = 0.12f / 100 * 12;   // 速度 速度-角度P
+constexpr float ang_angloop_p = 100.0f / 100 * 12;  // 角度 速度-角度P
 
 // 常量定义
 constexpr u8 SIZE_FLOAT = sizeof(float);
 constexpr u8 SIZE_U32 = sizeof(uint32_t);
 constexpr float ADC_MAX = 65536.0f;
 constexpr float PI = 3.14159265358979323846f;
-constexpr float INV_PI_3 = 3.0f / PI;
-constexpr float PI_3 = PI / 3.0f;
-constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
-constexpr float _3PI_2 = 4.71238898038f;
-constexpr float SQRT3 = 1.7320508075688772f;
-constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
-constexpr float COS_PI_3 = 0.5f;
 constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
 constexpr float DEG2RAD = 1.0f / 180.0f * PI;
 constexpr float RAD2DEG = 180.0f / PI; // 修正RAD2DEG定义，应该是180/π
@@ -183,12 +149,8 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
 
 // TIMER2-任务调度
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
-constexpr int TIMER2_PERIOD = 49;	  // 1 / 1MHz * 50 = 50us
-constexpr int MOTOR_PERIOD = 2;		  // 100us/50us = 2
-constexpr float TIME_SLICE =
-	(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
-constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE;				 // 频率
-constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
+constexpr int TIMER2_PERIOD = 49;	  // 1MHz / 50 = 50us
+constexpr float TIMER2_FREQUENCY = 1 / 5e-5;
 
 // TIMER1-PWM
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz