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

2026-03-09 01:47:30 +08:00
parent a26dd1cc8e c9934849c5
commit 1ee2f68a89
8 changed files with 359 additions and 171 deletions
@@ -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;
 }