Motor_v2/Robot_V2/Algorithm/FOC.h

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

class FOC_Controller {

  public:
	FOC_Controller();

	// 初始化函数
	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;

	bool over_current = false;
	bool voltage_error = false;
	bool is_shut_down = false;
	bool thermal_shutdown = false; // 过热关机标志
	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 Ua = 0, Ub = 0, Uc = 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;

	float velocity_open_loop(void);
	float angle_control(void);
	float angle_control_with_velocity_limit(void);
	float MIT_Mode(void);
	float velocity_control(void);
	float current_control(void);

	void update_power(float voltage);
	void DFOC_alignSensor(int _PP);
	void shut_down(void);
	void resume(void);
	void drv_stop(void);
	float drv_hold(void);
	float PWM_out(void);
	void shut_down_thermal(void);	// 过热关机
	void resume_from_thermal(void); // 过热恢复
	void fast_voltage_check(void);	// 快速电压检测（100us周期调用）

	// 齿槽转矩补偿
	bool cogging_comp_enabled = false;
	float cogging_comp_scale = COGGING_COMP_SCALE;
	float cogging_table[COGGING_TABLE_SIZE] = {0}; // 齿槽补偿表-正转 (mA)
	float cogging_table_rev[COGGING_TABLE_SIZE] = {0}; // 齿槽补偿表-反转 (mA)
	inline float get_cogging_compensation(float angle_el);

	// === 校准相关（DEBUG，平时不启用）===
	// 启用方法：config.h 中 FOC_DEBUG 改为 1
	#if FOC_DEBUG
	volatile bool cogging_calib_request = false;
	void calibrate_cogging_tick(void);
	enum class CogCalibState : u8 {
		IDLE = 0,
		SETTLING,
		SAMPLING,
		STOPPING,
		DONE
	};
	CogCalibState cog_calib_state = CogCalibState::IDLE;
	u8 cog_calib_run = 0;
	u32 cog_calib_sample_count = 0;
	u32 cog_calib_settle_tick = 0;
	u32 cog_calib_stop_tick = 0;
	u8 cog_calib_saved_mode = 0;
	float cog_calib_saved_vel = 0;
	static float cog_calib_run_data[6][COGGING_TABLE_SIZE]; // 3正+3反
	static float cog_calib_accum[COGGING_TABLE_SIZE];
	static u32 cog_calib_count[COGGING_TABLE_SIZE];
	#endif // FOC_DEBUG

	// 此处默认值不作数
	PIDController vel_loop =
		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
					  vel_loop_Kd, 10000, voltage_power_supply}; // 12V
	PIDController angle_loop =
		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
					  10000000, voltage_power_supply}; // 12V
	PIDController current_loop = PIDController{
		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
		current_loop_Kd, 1500, voltage_power_supply}; // 12V
	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 / limit_voltage, 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 / limit_voltage, current_loop_Ki,
		current_loop_Kd, 1500, voltage_power_supply}; // 12V

	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 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 setSVPWM_PhaseVoltage(float Uq, float Ud);
	float cal_Iq_Id(float current_a, float current_b, float current_c);
};

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

void calibrate_zero_position(void);
inline float _normalizeAngle(float angle);
inline float _electricalAngle();

#endif