feat(FOC): 添加带速度限制的角度闭环控制功能

新增angle_control_with_velocity_limit函数，实现带速度限制的角度控制，包含角度PID和速度PID双环控制，防止电机转速过快导致的不稳定。 - 新增带速度限制的角度控制函数 - 配置新的PID控制器用于速度内环和角度外环
2025-12-23 11:55:09 +08:00
parent 8732174855
commit abd64da360
5 changed files with 44 additions and 14 deletions
@@ -209,6 +209,36 @@ float FOC_Controlroller::angle_control(float target_angle) {
 	return shaft_angle;
 }

+/**
+ * @brief 带速度限制的角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) {
+	// 计算角度误差
+	float angle_error = target_angle - rt_angle;
+	
+	// 使用角度PID控制器计算目标速度
+	float target_velocity = ang_inVelAngloop(angle_error);
+	
+	// 对目标速度进行限幅
+	target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity);
+	
+	// 使用速度PID控制器计算最终输出电压
+	float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
+	// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
+	
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f);
+	
+	return shaft_angle;
+}
+
 /**
 * @brief 角度闭环控制
 *