// 函数声明
#ifndef _DENGFOC_H
#define _DENGFOC_H
#include <stdint.h>
class FOC_Controlroller {
  public:
	FOC_Controlroller();

	float control_mode = 0;
	float limit_voltage = 0.0f;
	float limit_velocity = 0.0f;
	float target_angle = 0.0f;
	float target_velocity = 0.0f;
	float rt_angle = 0.0f;
	float rt_velocity = 0.0f;
	u8 over_current = 0;
	u8 temp_error = 0;
	u8 voltage_error = 0;
	u8 is_shut_down = 0;

	float velocity_open_loop(float target_velocity);
	void DFOC_alignSensor(int _PP, int _DIR);
	void shut_down(void);
	void resume(void);

  protected:
	std::u64 last_time_us = 0;
	std::u64 now_us = 0;
	float shaft_angle = 0;
	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
	float zero_electric_angle = 0;
	float voltage_power_supply = 24.0f; // 电源电压

	void driver_output(float Ua, float Ub, float Uc);
	void setPhaseVoltage(float Uq, float Ud, float angle_el);
	void setTorque(float Uq,float angle_el);
	float DFOC_M0_Current();
	void DFOC_M0_setTorque(float Target);
};

float _normalizeAngle(float angle);
void DFOC_Vbus(float power_supply);
float _electricalAngle(float shaft_angle);
float DFOC_M0_Angle();
float serial_motor_target();

#endif