Motor_v2/Robot_V2/Algorithm/FOC.h

/**
 * @file Algorithm/FOC.h
 * @brief 算法模块源文件
 * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
 */

// 函数声明
#ifndef _FOC_H
#define _FOC_H
#include "config.h"
#include "pid.h"
#include <cstdint>

/**
 * @brief FOC_Controller 类或结构体
 */
class FOC_Controller {

  public:
	FOC_Controller();

	// 初始化函数
	/**
	 * @brief 初始化对象或模块
	 * @return u8 返回值
	 */
	u8 init();

	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;

	// 关机事件标记：通知低频任务把当前状态落盘（不是"能不能运行"的判据，
	// 运行许可一律看 sys/run_inhibit.h）
	bool emergency_shutdown = false; // 紧急电压跌落关机标志

	float voltage_power_supply = 24.0f; // 电源电压
	float v_safe = voltage_power_supply / 1.05f;
	float duty_a = 0, duty_b = 0, duty_c = 0;

	// 快速电压检测相关成员（基于跌幅检测）
	float vbus_reference = 0.0f;	// 基准电压（峰值检测，跟踪当前额定电压）
	float last_vbus_fast = 0.0f;	// 上次电压值（快速检测用）
	u32 vbus_drop_count = 0;		 // 电压跌落计数器（防抖）

	float cur_loop_p = DEFAULT_current_loop_p;
	float vel_loop_p = DEFAULT_vel_loop_p;
	float ang_loop_p = DEFAULT_angle_loop_p;

	float vel_angloop_p = DEFAULT_vel_angloop_p;
	float ang_angloop_p = DEFAULT_angle_angloop_p;

	float ang_MIT_p = DEFAULT_angle_MIT_p;
	float vel_MIT_p = DEFAULT_vel_MIT_p;
	float cur_MIT_p = DEFAULT_current_MIT_p;

	/**
	 * @brief velocity_open_loop 函数
	 * @return float 返回值
	 */
	float velocity_open_loop(void);
	/**
	 * @brief angle_control 函数
	 * @return float 返回值
	 */
	float angle_control(void);
	/**
	 * @brief angle_control_with_velocity_limit 函数
	 * @return float 返回值
	 */
	float angle_control_with_velocity_limit(void);
	/**
	 * @brief MIT_Mode 函数
	 * @return float 返回值
	 */
	float MIT_Mode(void);
	/**
	 * @brief velocity_control 函数
	 * @return float 返回值
	 */
	float velocity_control(void);
	/**
	 * @brief current_control 函数
	 * @return float 返回值
	 */
	float current_control(void);

	/**
	 * @brief 更新 power 相关状态
	 * @param[in] voltage 参数
	 * @return void
	 */
	void update_power(float voltage);
	/**
	 * @brief DFOC_alignSensor 函数
	 * @param[in] _PP 参数
	 * @return void
	 */
	void DFOC_alignSensor(int _PP);
	/**
	 * @brief drv_hold 函数
	 * @return float 返回值
	 */
	float drv_hold(void);
	/**
	 * @brief PWM_out 函数
	 * @return float 返回值
	 */
	float PWM_out(void);
	/**
	 * @brief fast_voltage_check 函数
	 * @return void
	 */
	void fast_voltage_check(void);	// 快速电压检测（100us周期调用）

	// 此处默认值不作数
	// 注意：构造期 limit_voltage 尚为 0，分母一律用 voltage_power_supply(24V)，
	// 否则 P 初始化为 +inf；运行期各模式每周期 set_P/set_PID 覆盖。
	// 内环 output_ramp 保持原值：实测收紧到 2000 V/s 会在制动段引入相位滞后，
	// 导致到位抖动，已回退。
	PIDController vel_loop =
		PIDController{DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
					  vel_loop_Kd, 10000, voltage_power_supply};
	PIDController angle_loop =
		PIDController{DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
					  0.02f, 10000000, voltage_power_supply};
	PIDController current_loop = PIDController{
		DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
		current_loop_Kd, 1500, voltage_power_supply};
	PIDController vel_inVelAngloop =
		PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
					  vel_angloop_Kd, 1e6, v_safe};
	PIDController ang_inVelAngloop =
		PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
					  ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};

	PIDController ang_inMITloop =
		PIDController{DEFAULT_angle_MIT_p * 100 / voltage_power_supply, angle_MIT_Ki,
					  angle_MIT_Kd, 1e5, 100};
	PIDController vel_inMITloop = PIDController{
		DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
	PIDController current_inMITloop = PIDController{
		DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
		current_loop_Kd, 1500, voltage_power_supply};

	/**
	 * @brief 更新 pid_limits 相关状态
	 * @return void
	 */
	void update_pid_limits() {
		vel_loop.limit = voltage_power_supply;
		angle_loop.limit = voltage_power_supply;
		current_loop.limit = voltage_power_supply;
		vel_inVelAngloop.limit = voltage_power_supply;
	}

  protected:
	float shaft_angle = 0;
	float Ualpha, Ubeta = 0;
	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
	// 本周期电角度的 sin/cos：update_el_sincos() 每周期算一次，
	// Park 变换与逆 Park 变换共用，避免同一角度算两次三角函数
	float el_sin = 0.0f;
	float el_cos = 0.0f;
	/**
	 * @brief 每周期计算一次电角度 sin/cos 供 Park/逆Park 共用
	 * @param[in] angle_el 电角度（度）
	 * @return void
	 */
	inline void update_el_sincos(float angle_el);
	/**
	 * @brief 设置 setSVPWM_PhaseVoltage 对应的参数或状态
	 * @param[in] Uq 参数
	 * @param[in] Ud 参数
	 * @return void
	 */
	void setSVPWM_PhaseVoltage(float Uq, float Ud);
	/**
	 * @brief cal_Iq_Id 函数
	 * @param[in] current_a 参数
	 * @param[in] current_b 参数
	 * @param[in] current_c 参数
	 * @return float 返回值
	 * @note 电气角度三角函数用 update_el_sincos() 的预计算结果
	 */
	float cal_Iq_Id(float current_a, float current_b, float current_c);
	float update_angle_sync_uq_feedforward(u8 mode, float table[],
									 u16 table_size, float mech_angle,
									 float target_vel, float actual_vel,
									 float velocity_error);
	float get_mode3_velocity_p(float target_vel, float actual_vel);
	float get_mode3_velocity_scale(float target_vel, float actual_vel,
								   float min_scale);
};

// 定义函数指针类型
typedef float (FOC_Controller::*FOCFunctionPtr)(void);

/**
 * @brief calibrate_zero_position 函数
 * @return void
 */
void calibrate_zero_position(void);
#endif