da

分支 (1)

管理

管理

da

FOC_speed_Torque
/
motor.c

#include "motor.h"
#include "arm_math.h"
#include "tzmx.h"
#include "user_fcn.h"
#include <stdlib.h>
#include "pid_control.h"

#include "MTPA.h"


float ID_REF = 1.0F;
float IQ_REF = 0.0F;


float D_PI_I =0.0F;                //-1.5
float D_PI_KB = 50.0F;
float D_PI_LOW_LIMIT = -V_Limit;
float D_PI_P = 0.0F;               //-3.5
float D_PI_UP_LIMIT = V_Limit;

float Q_PI_I = 1.0F;
float Q_PI_KB = 50.0F;
float Q_PI_LOW_LIMIT = -V_Limit;
float Q_PI_P = 0.0F;
float Q_PI_UP_LIMIT = V_Limit;

float SPEED_PI_I = 10.0F;
float SPEED_PI_KB = 0.015F;
float SPEED_PI_LOW_LIMIT = -7.0F;
float SPEED_PI_P = 0.00F;
float SPEED_PI_UP_LIMIT = 7.0F;


 u32 hPhaseA_OffSet;//2050                 2062
 u32 hPhaseB_OffSet;//2016                 2048
 u32 hPhasebus_OffSet;//2047
int32_T sector;
u8 State;
CURRENT_ABC_DEF Current_Iabc;
CURRENT_ALPHA_BETA_DEF Current_Ialpha_beta;
VOLTAGE_ALPHA_BETA_DEF Voltage_Alpha_Beta;
TRANSF_COS_SIN_DEF Transf_Cos_Sin;
CURRENT_DQ_DEF Current_Idq;
VOLTAGE_DQ_DEF Voltage_DQ;
VOLTAGE_DQ_DEF Voltage_DQ1;


CURRENT_PID_DEF Current_D_PID;
CURRENT_PID_DEF Current_Q_PID;
SPEED_PID_DEF Speed_Pid;
u8 PWM4Direction=PWM2_MODE;
float Speed_Ref = 0.0F;
float Speed_Pid_Out;
float Speed_Fdk;

float adc_p_speed;

int32_t circle_num=0;

/* extern */
extern float ref_in;
extern float ref_out;
extern float Uq;
extern float Kpp;

//encoder
s16 hPrevious_angle, hSpeed_Buffer[SPEED_BUFFER_SIZE], hRot_Speed;
static u8 bSpeed_Buffer_Index = 0;
static volatile u16 hEncoder_Timer_Overflow;
static bool bIs_First_Measurement = TRUE;
static bool bError_Speed_Measurement = FALSE;
static u16 hSpeedMeas_Timebase_500us = SPEED_SAMPLING_TIME;
volatile u8 bPID_Speed_Sampling_Time_500us = PID_SPEED_SAMPLING_TIME;
float cnt;
volatile u8 ref_Signal_tim_500us = Ref_Signal_Time;


extern float pid_out_ud;
extern float pid_out_uq;


extern float rxbuffer2;
extern float IQ;
extern float speed;


typedef struct
{
  int encode_old;                  /* 上一次计数值 */
  int encode_now;                  /* 当前计数值 */
  float speed;                     /* 编码器速度 */
} ENCODE_TypeDef;


ENCODE_TypeDef g_encode;

float speed_motor;
float  g_timx_encode_count;


//通道捕获值
uint32_t PWM_RisingCount,PWM_FallingCount;
float PWM_Duty,PWM_Period,PWM_Frequency;


//溢出标志位
int8_t enc_over_flag;


//转矩控制相关变量

// PI控制器参数
const float Kp_torque = 1.0;  // 比例增益
const float Ki_torque = 0.01; // 积分增益

float id_ref, iq_ref;

// 全局变量
float torque_error_integral = 0; // 转矩误差积分

// 电流限制
const float Imax = 500.0; // 最大电流限制


//相线电流
CURRENT_ABC_DEF iabc;
//母线电流
float Bus_current;


float angle;


CURRENT_DQ_DEF Current_Idq1;

float ID=0.0f;


//串口停止命令
 extern float stop;


float seepd_setpoint;


float U_BUS;


SVPWM_Def test_def;


#define cnt_clk 64000000/(15+1)//计数器频率
#define arr 65535//自定重装载值
uint32_t CCR1,CCR2,end_flag;//存捕获寄存器获取的值的变量
float duty_cycle,frequency;//频率，占空比


typedef struct
{
	float period;
	float frq;
	float high_time;
	float duty;
	float angle;	 // 0~360
	float angle_rad; // ??????
	float electronic_angle;
	float angle_rad_offset;
} PWM_ENCODER_Def;

PWM_ENCODER_Def pwm_encoder;


void board_config(void)
{
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,0);
  	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,0);

	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,PWM_PERIOD-1);

	HAL_ADCEx_InjectedStart_IT(&hadc1);

	HAL_TIM_Encoder_Start_IT(&ENC_htim,TIM_CHANNEL_ALL);
  HAL_TIM_Base_Start_IT(&ENC_htim);


	HAL_TIM_Base_Start_IT(&htim3);
       HAL_TIM_Base_Start_IT(&htim4);

	PID_Init();
	PID_init();


        __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
	//__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);

        //读取电流偏置
        GetCurrentOffset();

        PI_Controller_Init();
        //开启tim4的输入捕获
        HAL_TIM_IC_Start_IT(&htim4, TIM_CHANNEL_1);
	HAL_TIM_IC_Start_IT(&htim4, TIM_CHANNEL_2);
        //__HAL_TIM_CLEAR_IT(&htim4,TIM_CHANNEL_1);


        //MTPAL列表初始化
        MTPA_Init();


        State=START;


}
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc)
{
	if(State==START)


		Start_Up();
	else if(State==RUN)
  {
      //限流保护
     //CurrentLimitProtection();
      //电流环，16k
		foc_algorithm_step();

//        HAL_ADCEx_InjectedStart_IT(&hadc1);
//        __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);


  }
  else if(State==STOP)
  {
     ClosePWM_Protection();


  }


}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	if (htim->Instance == TIM4)
	{
		if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
		{
			pwm_encoder.period = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
			pwm_encoder.frq = 1/(pwm_encoder.period * 0.00000025);
			pwm_encoder.duty = (pwm_encoder.high_time / pwm_encoder.period); //0.994是编码器角度补偿误差系数
		}
		if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2)
		{
			pwm_encoder.high_time = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
		}
	}
}


void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if (htim->Instance == ENC_TIM)
     {
        if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&ENC_htim))   /* 判断CR1的DIR位 */
        {
            g_timx_encode_count--;                                      /* DIR位为1，也就是递减计数 */
            enc_over_flag--;

        }
        else
        {
            g_timx_encode_count++;                                      /* DIR位为0，也就是递增计数 */
            enc_over_flag++;

        }

     }


	if(htim->Instance == TIM3)//1ms
	{


          speed_computer(gtim_get_encode(), 1);                                  /* 中位平均值滤除编码器抖动数据，5ms计算一次速度 */


		        //get_speed();


          //电机状态如果处于运行，则开启堵转保护
          if(State==RUN)
          {
            //CheckStallProtection(speed_motor);


               if(stop!=0.0f)
          {
             State=STOP;

          }

          if(U_BUS<57.0f)
          {
            State=STOP;
          }


          }


          static int ms_100;
          ms_100++;
          if(ms_100==10)
          {

           U_BUS= temper_trans();


            printf("%.1f\n",speed_motor);
           //printf("%.1f,%.1f,",Current_Idq.Iq,Current_Idq.Id);
           //printf("%.1f,%.1f\n",Current_Idq1.Iq,Current_Idq1.Id);


            //printf("%.1f,%.3f,%.1f,%f\n",pwm_encoder.frq,pwm_encoder.duty,pwm_encoder.period,pwm_encoder.high_time);


            //printf("%.1f,%.1f,%.1f\n",Current_Iabc.Ia,Current_Iabc.Ib,Current_Iabc.Ic);
            //printf("%.1f,%.1f,%.1f\n",Current_Ialpha_beta.Ialpha,Current_Ialpha_beta.Ibeta,ENC_Get_Electrical_Angle());
            //Current_Ialpha_beta
            //printf("%.1f,%.1f\n",ENC_TIM->CNT,Get_pwm_angle());


            //printf("%.2f,%.2f\n",get_encode_angle(),Get_pwm_angle());

            //printf("%d\n",TIM3->CNT);
           //printf("\n");


//            id_pid.kp=2.4f;        //2.0             2.4
//            id_pid.ki=0.2f;       //0.01    0.05     0.2
//            id_pid.kd=0.0f;
//
//            iq_pid.kp=2.40f;        //2.0
//            iq_pid.ki=0.20f;        //0.01
//            iq_pid.kd=0.0f;


            ms_100=0;
          }

          //printf("%.1f\n",ENC_Get_Electrical_Angle());


          if(State==RUN)
          {

//            static int16_t temp =-15;
//
//            if(IQ<-14)
//            {
//
//             temp=0;
//            }

            //IQ = ApplyRampFunction(10,IQ,2,0.001);


           static float Torque;


            static int32_t ms;
            ms++;
            if(ms==1000)
            {
               speed=-1000.0f;
               Torque=0.7f;
            }
            else if (ms==10000)
            {
               speed=-1500.0f;
            }
            else if (ms==20000)
            {
              speed=-500.0f;
            }
            else if (ms==3000)
            {
               Torque=1.6f;
            }


            //速度环
            seepd_setpoint = ApplyRampFunction(speed,seepd_setpoint,500.0f,0.001f);

	    IQ = PID_Controller(&speed_pid_1,seepd_setpoint,speed_motor);


            //转矩环


            //mtpa_idq_ref=TorqueControlLoop( Torque,300.0f);


            }


          //printf("%.2f,%.2f,%.2f\n",get_Iab().Ia,get_Iab().Ib,get_Iab().Ic);


         // TIM2


//		get_DC_BUS();
//		temper_trans();

		//printf("%.2f,%.2f\n",Current_Idq.Iq,Current_Idq.Id);

         //printf("%.2f\n",ENC_Get_Electrical_Angle());
	}


}


/*FOC Core part 10Khz*/
void foc_algorithm_step(void)
{

//     uint32_t current_time = HAL_GetTick(); // 获取当前时间 (ms)
//   volatile static uint32_t stall_detection_start_time,time;

  //cnt+=0.001f;


      //电流采样
      Current_Iabc =get_Iab();


	clarke_transform(Current_Iabc.Ia,Current_Iabc.Ib,Current_Iabc.Ic,&Current_Ialpha_beta.Ialpha,&Current_Ialpha_beta.Ibeta);

        // Park 变换
        park_transform(Current_Ialpha_beta.Ialpha, Current_Ialpha_beta.Ibeta, ENC_Get_Electrical_Angle()-PI, &Current_Idq1.Id, &Current_Idq1.Iq);


	Clarke_Transf(Current_Iabc,&Current_Ialpha_beta);
	Angle_To_Cos_Sin(ENC_Get_Electrical_Angle(),&Transf_Cos_Sin); //when use openloop, angle=cnt;ENC_Get_Electrical_Angle() Get_pwm_angle
	Park_Transf(Current_Ialpha_beta,Transf_Cos_Sin,&Current_Idq);


       //Voltage_DQ = Current_feedforward(Current_Idq1);

        //速度环的电流给定
          Voltage_DQ.Vq=PI_Controller(&q_axis_controller,IQ,Current_Idq.Iq);
	  Voltage_DQ.Vd=PI_Controller(&d_axis_controller,ID,Current_Idq.Id);

          //转矩环的电流给定
//          Voltage_DQ.Vq=PI_Controller(&q_axis_controller,mtpa_idq_ref.iq,Current_Idq.Iq);
//	  Voltage_DQ.Vd=PI_Controller(&d_axis_controller,mtpa_idq_ref.iq,Current_Idq.Id);


        //开环电压给定
//        Voltage_DQ.Vd=0.0f;
//        Voltage_DQ.Vq=IQ;


//	      Current_PID_Calc(0.0f,Current_Idq1.Id,&Voltage_DQ.Vd,&Current_D_PID);
//        Current_PID_Calc(IQ,Current_Idq1.Iq,&Voltage_DQ.Vq,&Current_Q_PID);


	Rev_Park_Transf(Voltage_DQ,Transf_Cos_Sin,&Voltage_Alpha_Beta);


        //SVPWM(&Voltage_Alpha_Beta,&test_def);


        SVPWM_Calculate_GPT_2(Voltage_Alpha_Beta.Valpha,Voltage_Alpha_Beta.Vbeta);


	//SVPWM_Calc(Voltage_Alpha_Beta,Udc,PWM_TIM_PULSE_TPWM);


        //ClosePWM_Protection();


}
/*Motor Start & Encoder Align part*/
void Start_Up(void)
{
	static u32 wTimebase=0;
	wTimebase++;
	if(wTimebase<=T_ALIGNMENT_PWM_STEPS)
	{
		//Clarke_Transf(get_Iab(),&Current_Ialpha_beta);

                //printf("%f\n",(float)ALIGNMENT_ANGLE/360.0f *2.0f*PI);
               // printf("%f\n",ID_REF);


		Angle_To_Cos_Sin((float)ALIGNMENT_ANGLE/360.0f *2.0f*PI,&Transf_Cos_Sin); //when use openloop, angle=cnt;
                Angle_To_Cos_Sin(0,&Transf_Cos_Sin);


		Park_Transf(Current_Ialpha_beta,Transf_Cos_Sin,&Current_Idq);
		IQ_REF = 0.0;
		ID_REF = I_ALIGNMENT * (float)wTimebase / (float)T_ALIGNMENT_PWM_STEPS;
		Current_PID_Calc(ID_REF,Current_Idq.Id,&Voltage_DQ.Vd,&Current_D_PID);
		Current_PID_Calc(IQ_REF,Current_Idq.Iq,&Voltage_DQ.Vq,&Current_Q_PID);

                Voltage_DQ.Vd=3.0f;
	        Voltage_DQ.Vq=0.0f;
	        Angle_To_Cos_Sin(cnt,&Transf_Cos_Sin);
	        cnt+=0.00f;

		Rev_Park_Transf(Voltage_DQ,Transf_Cos_Sin,&Voltage_Alpha_Beta);
		//SVPWM_Calc(Voltage_Alpha_Beta,Udc,PWM_TIM_PULSE_TPWM);


                //SVPWM_Calculate_GPT_2(Voltage_Alpha_Beta.Valpha,Voltage_Alpha_Beta.Vbeta);
                //SVPWM(&Voltage_Alpha_Beta,&test_def);


	}


	else
	{

                PID_Init();
		wTimebase = 0;
		Voltage_DQ.Vd =Voltage_DQ.Vq=0.0f;
		IQ_REF = 0.0F;
		ID_REF = 0.0F;
		ENC_ResetEncoder();
		__HAL_TIM_SET_COUNTER(&ENC_htim,(uint32_t)Get_pwm_angle());
                //__HAL_TIM_SET_COUNTER(&ENC_htim,0);
		ENC_Clear_Speed_Buffer();
		State=RUN;


                //关闭PWM捕获
//                HAL_TIM_Base_Stop_IT(&htim4);
//                HAL_TIM_IC_Stop_IT(&htim4, TIM_CHANNEL_1);
//	        HAL_TIM_IC_Stop_IT(&htim4, TIM_CHANNEL_2);


                //printf("%.3f\n",pwm_encoder.duty);
                //ClosePWM_Protection();


	}


}


float get_DC_BUS(void)
{
	static float temp,out_k,out_k1;
	float a=0.1;
	temp = HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_4);
	temp = temp*SAMPLE_VOL_CON_FACTOR - 0.2f;

	out_k = a*temp+(1-a)*out_k1;
	out_k1 = out_k;

//	if(temp<Udc*0.8 || temp>Udc*1.2 )
//	{
//		shut_pwm();
//		State=FAULT;
//	}
	return out_k;
}

void shut_pwm(void)
{
	HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_3);

	HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_3);
}

float temper_trans(void)
{
	float Rt = 0.0f;// R now
	float R = 10000.0f;//10k
	float T0 = 237.15f+25.0f;//trans K
	float B = 3380.0f;
	float K = 273.15f;
	static float VR=0.0f;
	uint16_t adc_v[1];
	float temp;
	static float out_k,out_k1;
	float a=0.1;


	for(int i=0;i<1;i++)
	{
		HAL_ADC_Start(&hadc1);
		HAL_ADC_PollForConversion(&hadc1,5);
		adc_v[i]=HAL_ADC_GetValue(&hadc1);
	}


	VR = (float)((adc_v[0] / 4096.0f * 3.3f)*31.0f);


	return VR;
}


float get_anlog_in(void)
{
	static float temp,out_k,out_k1;
	float a=0.1;
	uint16_t adc_v[2];
	for(int i=0;i<2;i++)
	{
		HAL_ADC_Start(&hadc1);
		HAL_ADC_PollForConversion(&hadc1,50);
		adc_v[i]=HAL_ADC_GetValue(&hadc1);
	}

	adc_p_speed=adc_v[1];
	adc_p_speed = adc_p_speed-1060;  //0--2200
	if(adc_p_speed<=0)
		adc_p_speed=0;
	adc_p_speed=adc_p_speed/2200.0f;
	temp=adc_p_speed;
	out_k = a*temp+(1-a)*out_k1;
	out_k1 = out_k;
	return out_k;
}


void ENC_Clear_Speed_Buffer(void)
{
  u32 i;

  for (i=0;i<SPEED_BUFFER_SIZE;i++)
  {
    hSpeed_Buffer[i] = 0;
  }
  bIs_First_Measurement = TRUE;
}
void ENC_ResetEncoder(void)
{
  ENC_TIM->CNT = COUNTER_RESET;
}


/**********************************************************************************************************
get Theta
**********************************************************************************************************/


//Z信号编码较零
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin==GPIO_PIN_10)
  {


//    if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&ENC_htim)==1)
//    {
//      ENC_TIM->CNT=3950;
//    }
//    else if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&ENC_htim)==0)
//    {
//      ENC_TIM->CNT=672;
//    }


    ENC_TIM->CNT=4090;


    //printf("%d\n",ENC_TIM->CNT);
    //printf("%.0f\n",pwm_encoder.high_time);
  }


}


//获取电角度  -3.14 ———— 3.14
float ENC_Get_Electrical_Angle(void)
{
	return ((float)(ENC_Get_Electrical_Angle_S16()) / 32768.0f*PI );
}


float ENC_Get_Mechanical_Angle_X(void)
{
	float angle;
//	angle = (float)(__HAL_TIM_GET_COUNTER(&ENC_TIM)) / ((float)(4*ENCODER_PPR)) * 2.0f*PI + (circle_num)*2.0f*PI;
	if ( (ENC_TIM->CR1 & TIM_COUNTERMODE_DOWN) != TIM_COUNTERMODE_DOWN)
	{
		angle = (float)(__HAL_TIM_GET_COUNTER(&ENC_htim)) / ((float)(4*ENCODER_PPR)) * 2.0f*PI + (circle_num)*2.0f*PI;
	}
	else
	{
		angle = (float)(__HAL_TIM_GET_COUNTER(&ENC_htim)) / ((float)(4*ENCODER_PPR)) * 2.0f*PI - (circle_num)*2.0f*PI;
	}

	return angle;
}

//s16格式电角度获取
s16 ENC_Get_Electrical_Angle_S16(void)
{
  s32 temp;

  temp = (s32)(__HAL_TIM_GET_COUNTER(&htim2)) * (s32)(U32_MAX / (4*ENCODER_PPR));
  temp *= (POLE_PAIR_NUM);
  return((s16)(temp/65536 ));   //s16 result
}

/**********************************************************************************************************
get Iab
**********************************************************************************************************/
CURRENT_ABC_DEF get_Iab(void)
{
	static CURRENT_ABC_DEF I_temp;
	static float Ia_temp,Ib_temp,Ic_temp;
//	Ia_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)-(s16)hPhaseA_OffSet)* SAMPLE_CURR_CON_FACTOR;
//	Ib_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)-(s16)hPhaseB_OffSet)* SAMPLE_CURR_CON_FACTOR;
//	Ic_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)-(s16)hPhaseC_OffSet)* SAMPLE_CURR_CON_FACTOR;


//        Ic_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)-(s16)hPhaseA_OffSet)* SAMPLE_CURR_CON_FACTOR;
//	Ia_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)-(s16)hPhaseB_OffSet)* SAMPLE_CURR_CON_FACTOR;
//	Ib_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)-(s16)hPhaseC_OffSet)* SAMPLE_CURR_CON_FACTOR;


        Ib_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)-(s16)hPhaseB_OffSet)* Current_ratio;
	Ia_temp = (s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)-(s16)hPhaseA_OffSet)* Current_ratio;

//        I_temp.Ia=Ia_temp;
//        I_temp.Ib=Ib_temp;
//        I_temp.Ic = -Ia_temp - Ib_temp;


        I_temp.Ia=Ia_temp;
        I_temp.Ib=Ib_temp;
        I_temp.Ic = -Ia_temp - Ib_temp;


        Bus_current=(s16)((s16)HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)-(s16)hPhasebus_OffSet)* BUS_Current_ratio;


        //printf("%d,%d,%d\n",ADC1->JDR1,ADC1->JDR2,ADC1->JDR3);

        //printf("%d,%d,%d\n",ADC1->JDR1,ADC1->JDR2,ADC1->JDR3);
        //printf("%.1f,%.1f,%.1f\n",Ia_temp,Ib_temp,Ic_temp);


	return (I_temp);
}


void PID_Init(void)
{
	// Id- loop
	Current_D_PID.P_Gain = D_PI_P;
  Current_D_PID.I_Gain = D_PI_I;
  Current_D_PID.B_Gain = D_PI_KB;
  Current_D_PID.Max_Output = D_PI_UP_LIMIT;
  Current_D_PID.Min_Output = D_PI_LOW_LIMIT;
  Current_D_PID.I_Sum = 0.0f;
  //Iq- loop
  Current_Q_PID.P_Gain = Q_PI_P;
  Current_Q_PID.I_Gain = Q_PI_I;
  Current_Q_PID.B_Gain = Q_PI_KB;
  Current_Q_PID.Max_Output = Q_PI_UP_LIMIT;
  Current_Q_PID.Min_Output = Q_PI_LOW_LIMIT;
  Current_Q_PID.I_Sum = 0.0f;
	//Speed loop
	Speed_Pid.P_Gain = SPEED_PI_P;
  Speed_Pid.I_Gain = SPEED_PI_I;
  Speed_Pid.B_Gain = SPEED_PI_KB;
  Speed_Pid.Max_Output = SPEED_PI_UP_LIMIT;
  Speed_Pid.Min_Output = SPEED_PI_LOW_LIMIT;
  Speed_Pid.I_Sum = 0.0f;

}

/***************************************
current PID
B is Integral windup gain ,
usually, it is about I gain
***************************************/
void Current_PID_Calc(float ref_temp,float fdb_temp,float* out_temp,CURRENT_PID_DEF* current_pid_temp)
{
  float error;
  float temp;
 static float temp1;
  error = ref_temp - fdb_temp;
  temp = current_pid_temp->P_Gain * error + current_pid_temp->I_Sum;
  if (temp > current_pid_temp->Max_Output)
  {
    *out_temp = current_pid_temp->Max_Output;
  }
  else if (temp < current_pid_temp->Min_Output)
  {
    *out_temp = current_pid_temp->Min_Output;
  }
  else
  {
    *out_temp = temp;
  }

  temp1=(*out_temp - temp) * current_pid_temp->B_Gain;

  current_pid_temp->I_Sum += ((*out_temp - temp) * current_pid_temp->B_Gain + current_pid_temp->I_Gain * error) *FOC_PERIOD;
 // current_pid_temp->I_Sum +=current_pid_temp->I_Gain * error*FOC_PERIOD;
}

void Speed_Pid_Calc(float ref_temp,float fdb_temp,float* out_temp,SPEED_PID_DEF* current_pid_temp)
{

  float error;
  float temp;

  error =  ref_temp - fdb_temp;             //2*pi的作用是 单位转换   Hz转换为rad/s

  temp = (error + current_pid_temp->I_Sum) * current_pid_temp->P_Gain;


  if (temp > current_pid_temp->Max_Output) {
    *out_temp = current_pid_temp->Max_Output;
  } else if (temp < current_pid_temp->Min_Output) {
    *out_temp = current_pid_temp->Min_Output;
  } else {
    *out_temp = temp;
  }
  current_pid_temp->I_Sum += ((*out_temp - temp) * current_pid_temp->B_Gain + current_pid_temp->I_Gain* error) * SPEED_PID_PERIOD;
}


s16 ENC_Get_Mechanical_Speed(void)
{
  return(hRot_Speed);
}


s16 ENC_Calc_Rot_Speed(void)
{
  s32 wDelta_angle;
  u16 hEnc_Timer_Overflow_sample_one, hEnc_Timer_Overflow_sample_two;
  u16 hCurrent_angle_sample_one, hCurrent_angle_sample_two;
  signed long long temp;
  s16 haux;

  if (!bIs_First_Measurement)
  {
    // 1st reading of overflow counter
    hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow;
    // 1st reading of encoder timer counter
    hCurrent_angle_sample_one = ENC_TIM->CNT;
    // 2nd reading of overflow counter
    hEnc_Timer_Overflow_sample_two = hEncoder_Timer_Overflow;
    // 2nd reading of encoder timer counter
    hCurrent_angle_sample_two = ENC_TIM->CNT;

    // Reset hEncoder_Timer_Overflow and read the counter value for the next
    // measurement
    hEncoder_Timer_Overflow = 0;
    haux = ENC_TIM->CNT;

    if (hEncoder_Timer_Overflow != 0)
    {
      haux = ENC_TIM->CNT;
      hEncoder_Timer_Overflow = 0;
    }

    if (hEnc_Timer_Overflow_sample_one != hEnc_Timer_Overflow_sample_two)
    { //Compare sample 1 & 2 and check if an overflow has been generated right
      //after the reading of encoder timer. If yes, copy sample 2 result in
      //sample 1 for next process
      hCurrent_angle_sample_one = hCurrent_angle_sample_two;
      hEnc_Timer_Overflow_sample_one = hEnc_Timer_Overflow_sample_two;
    }

    if ( (ENC_TIM->CR1 & TIM_COUNTERMODE_DOWN) == TIM_COUNTERMODE_DOWN)
    {// encoder timer down-counting
      wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle -
                    (hEnc_Timer_Overflow_sample_one) * (4*ENCODER_PPR));
    }
    else
    {//encoder timer up-counting
      wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle +
                    (hEnc_Timer_Overflow_sample_one) * (4*ENCODER_PPR));
    }

    // speed computation as delta angle * 1/(speed sempling time)
    temp = (signed long long)(wDelta_angle * SPEED_SAMPLING_FREQ);
    temp *= 10;  // 0.1 Hz resolution
    temp /= (4*ENCODER_PPR);

  } //is first measurement, discard it
  else
  {
    bIs_First_Measurement = FALSE;
    temp = 0;
    hEncoder_Timer_Overflow = 0;
    haux = ENC_TIM->CNT;
    // Check if Encoder_Timer_Overflow is still zero. In case an overflow IT
    // occured it resets overflow counter and wPWM_Counter_Angular_Velocity
    if (hEncoder_Timer_Overflow != 0)
    {
      haux = ENC_TIM->CNT;
      hEncoder_Timer_Overflow = 0;
    }
  }

  hPrevious_angle = haux;

  return((s16) temp);
}


void ENC_Calc_Average_Speed(void)
{
  s32 wtemp;
  u16 hAbstemp;
  u32 i;
  u8 static bError_counter;
  float L_K=0.9f;
	static float Y_k,U_k,Y_k_1;
  wtemp = ENC_Calc_Rot_Speed();
  hAbstemp = ( wtemp < 0 ? - wtemp :  wtemp);

/* Checks for speed measurement errors when in RUN State and saturates if
                                                                    necessary*/
  if (State == RUN)
  {
    if(hAbstemp < MINIMUM_MECHANICAL_SPEED)
    {
      if (wtemp < 0)
      {
        wtemp = -(s32)(MINIMUM_MECHANICAL_SPEED);
      }
      else
      {
        wtemp = MINIMUM_MECHANICAL_SPEED;
      }
      bError_counter++;
    }
    else  if (hAbstemp > MAXIMUM_MECHANICAL_SPEED)
          {
            if (wtemp < 0)
            {
              wtemp = -(s32)(MAXIMUM_MECHANICAL_SPEED);
            }
            else
            {
              wtemp = MAXIMUM_MECHANICAL_SPEED;
            }
            bError_counter++;
          }
          else
          {
            bError_counter = 0;
          }

    if (bError_counter >= MAXIMUM_ERROR_NUMBER)
    {
     bError_Speed_Measurement = TRUE;
    }
    else
    {
     bError_Speed_Measurement = FALSE;
    }
  }
  else
  {
    bError_Speed_Measurement = FALSE;
    bError_counter = 0;
  }

/* Compute the average of the read speeds */

  hSpeed_Buffer[bSpeed_Buffer_Index] = (s16)wtemp;
  bSpeed_Buffer_Index++;

  if (bSpeed_Buffer_Index == SPEED_BUFFER_SIZE)
  {
    bSpeed_Buffer_Index = 0;
  }

  wtemp=0;

  for (i=0;i<SPEED_BUFFER_SIZE;i++)
    {
    wtemp += hSpeed_Buffer[i];
    }
  wtemp /= SPEED_BUFFER_SIZE;

  hRot_Speed = ((s16)(wtemp));


}


void Clarke_Transf(CURRENT_ABC_DEF Current_abc_temp,CURRENT_ALPHA_BETA_DEF* Current_alpha_beta_temp)
{
  Current_alpha_beta_temp->Ialpha = (Current_abc_temp.Ia - (Current_abc_temp.Ib + Current_abc_temp.Ic) * 0.5F) * 2.0F / 3.0F;
  Current_alpha_beta_temp->Ibeta = (Current_abc_temp.Ib - Current_abc_temp.Ic) * 0.866025388F * 2.0F / 3.0F;
}

void Angle_To_Cos_Sin(float angle_temp,TRANSF_COS_SIN_DEF* cos_sin_temp)
{
  cos_sin_temp->Cos = arm_cos_f32(angle_temp);
  cos_sin_temp->Sin = arm_sin_f32(angle_temp);
}

void Park_Transf(CURRENT_ALPHA_BETA_DEF current_alpha_beta_temp,TRANSF_COS_SIN_DEF cos_sin_temp,CURRENT_DQ_DEF* current_dq_temp)
{
  current_dq_temp->Id = current_alpha_beta_temp.Ialpha * cos_sin_temp.Cos + current_alpha_beta_temp.Ibeta * cos_sin_temp.Sin;
  current_dq_temp->Iq = -current_alpha_beta_temp.Ialpha * cos_sin_temp.Sin + current_alpha_beta_temp.Ibeta * cos_sin_temp.Cos;
}

void Rev_Park_Transf(VOLTAGE_DQ_DEF v_dq_temp,TRANSF_COS_SIN_DEF cos_sin_temp,VOLTAGE_ALPHA_BETA_DEF* v_alpha_beta_temp)
{
  v_alpha_beta_temp->Valpha = cos_sin_temp.Cos * v_dq_temp.Vd - cos_sin_temp.Sin * v_dq_temp.Vq;
  v_alpha_beta_temp->Vbeta  = cos_sin_temp.Sin * v_dq_temp.Vd + cos_sin_temp.Cos * v_dq_temp.Vq;
}


void SVPWM_Calc(VOLTAGE_ALPHA_BETA_DEF v_alpha_beta_temp,float Udc_temp,float Tpwm_temp)
{

  float Tcmp1,Tcmp2,Tcmp3,Tx,Ty,f_temp,Ta,Tb,Tc;
	u16 Tcmp4 ;
	float  hDeltaDuty;
  sector = 0;
  Tcmp1 = 0.0F;
  Tcmp2 = 0.0F;
  Tcmp3 = 0.0F;
	Tcmp4 = 0;//计算采样时机
  if (v_alpha_beta_temp.Vbeta > 0.0F) {
    sector = 1;
  }

  if ((1.73205078F * v_alpha_beta_temp.Valpha - v_alpha_beta_temp.Vbeta) / 2.0F > 0.0F) {
    sector += 2;
  }

  if ((-1.73205078F * v_alpha_beta_temp.Valpha - v_alpha_beta_temp.Vbeta) / 2.0F > 0.0F) {
    sector += 4;
  }

  switch (sector) {
  case 1:
    Tx = (-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    Ty = (1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    break;

  case 2:
    Tx = (1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    Ty = -(1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp);
    break;

  case 3:
    Tx = -((-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    Ty = 1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp;
    break;

  case 4:
    Tx = -(1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp);
    Ty = (-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    break;

  case 5:
    Tx = 1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp;
    Ty = -((1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    break;

  default:
    Tx = -((1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    Ty = -((-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    break;
  }

  f_temp = Tx + Ty;
  if (f_temp > Tpwm_temp) {
    Tx /= f_temp;
    Ty /= (Tx + Ty);
  }

  Ta = (Tpwm_temp - (Tx + Ty)) / 4.0F;
  Tb = Tx / 2.0F + Ta;
  Tc = Ty / 2.0F + Tb;
  switch (sector) {
  case 1:
    Tcmp1 = Tb;
    Tcmp2 = Ta;
    Tcmp3 = Tc;

		if((u16)(PWM_PERIOD-(u16)Tcmp1)>TW_AFTER)
		{
			Tcmp4=PWM_PERIOD-1;
		}
		else
		{
			hDeltaDuty = (u16)(Tcmp1 - Tcmp2);
			if (hDeltaDuty > (u16)(PWM_PERIOD-(u16)Tcmp1)*2)
			{
					Tcmp4 = (u16)Tcmp1 - TW_BEFORE; // Ts before Phase A
			}
			else
			{
				Tcmp4 = (u16)Tcmp1 + TW_BEFORE;
				if (Tcmp4 >= PWM_PERIOD)
				{
					PWM4Direction=PWM1_MODE;
					Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
				}
			}
		}

    break;

  case 2:
    Tcmp1 = Ta;
    Tcmp2 = Tc;
    Tcmp3 = Tb;

		if((u16)(PWM_PERIOD-(u16)Tcmp2)>TW_AFTER)
			{
				Tcmp4=PWM_PERIOD-1;
			}
			else
			{
				hDeltaDuty = (u16)(Tcmp2 - Tcmp1);
				if (hDeltaDuty > (u16)(PWM_PERIOD-(u16)Tcmp2)*2)
				{
						Tcmp4 = (u16)Tcmp2 - TW_BEFORE; // Ts before Phase A
				}
				else
				{
					Tcmp4 = (u16)Tcmp2 + TW_BEFORE;
					if (Tcmp4 >= PWM_PERIOD)
					{
						PWM4Direction=PWM1_MODE;
						Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
					}
				}
			}

    break;

  case 3:
    Tcmp1 = Ta;
    Tcmp2 = Tb;
    Tcmp3 = Tc;

		if((u16)(PWM_PERIOD-(u16)Tcmp2)>TW_AFTER)
			{
				Tcmp4=PWM_PERIOD-1;
			}
			else
			{
				hDeltaDuty = (u16)(Tcmp2 - Tcmp3);
				if (hDeltaDuty > (u16)(PWM_PERIOD-(u16)Tcmp2)*2)
				{
						Tcmp4 = (u16)Tcmp2 - TW_BEFORE; // Ts before Phase A
				}
				else
				{
					Tcmp4 = (u16)Tcmp2 + TW_BEFORE;
					if (Tcmp4 >= PWM_PERIOD)
					{
						PWM4Direction=PWM1_MODE;
						Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
					}
				}
			}

    break;

  case 4:
    Tcmp1 = Tc;
    Tcmp2 = Tb;
    Tcmp3 = Ta;
		if((u16)(PWM_PERIOD-(u16)Tcmp3)>TW_AFTER)
			{
				Tcmp4=PWM_PERIOD-1;
			}
			else
			{
				hDeltaDuty = (u16)(Tcmp3 - Tcmp2);
				if (hDeltaDuty > (u16)(PWM_PERIOD-(u16)Tcmp3)*2)
				{
						Tcmp4 = (u16)Tcmp3 - TW_BEFORE; // Ts before Phase A
				}
				else
				{
					Tcmp4 = (u16)Tcmp3 + TW_BEFORE;
					if (Tcmp4 >= PWM_PERIOD)
					{
						PWM4Direction=PWM1_MODE;
						Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
					}
				}
			}
    break;

  case 5:
    Tcmp1 = Tc;
    Tcmp2 = Ta;
    Tcmp3 = Tb;
		if((u16)(PWM_PERIOD-(u16)Tcmp3)>TW_AFTER)
			{
				Tcmp4=PWM_PERIOD-1;
			}
			else
			{
				hDeltaDuty = (u16)(Tcmp3 - Tcmp1);
				if (hDeltaDuty > (u16)(PWM_PERIOD-(u16)Tcmp3)*2)
				{
						Tcmp4 = (u16)Tcmp3 - TW_BEFORE; // Ts before Phase A
				}
				else
				{
					Tcmp4 = (u16)Tcmp3 + TW_BEFORE;
					if (Tcmp4 >= PWM_PERIOD)
					{
						PWM4Direction=PWM1_MODE;
						Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
					}
				}
			}
    break;

  case 6:
    Tcmp1 = Tb;
    Tcmp2 = Tc;
    Tcmp3 = Ta;
			if((u16)(PWM_PERIOD-(u16)Tcmp1)>TW_AFTER)
			{
				Tcmp4=PWM_PERIOD-1;
			}
			else
			{
				hDeltaDuty = (u16)(Tcmp1 - Tcmp3);
				if (hDeltaDuty > (u16)(PWM_PERIOD-(u16)Tcmp1)*2)
				{
						Tcmp4 = (u16)Tcmp1 - TW_BEFORE; // Ts before Phase A
				}
				else
				{
					Tcmp4 = (u16)Tcmp1 + TW_BEFORE;
					if (Tcmp4 >= PWM_PERIOD)
					{
						PWM4Direction=PWM1_MODE;
						Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
					}
				}
			}
    break;
  }

//	if (PWM4Direction == PWM2_MODE)
//  {
//    //Set Polarity of CC4 High
//    TIM1->CCER &= 0xDFFF;
//  }
//  else
//  {
//    //Set Polarity of CC4 Low
//    TIM1->CCER |= 0x2000;
//  }

        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,(u16)Tcmp1);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,(u16)Tcmp2);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,(u16)Tcmp3);
	//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,(u16)Tcmp4);

}


//计算速度并滤波
float speed_computer(int32_t encode_now, uint8_t ms)
{
     uint8_t i = 0, j = 0;
    float temp = 0.0;
    float speed;
    static uint8_t sp_count = 0, k = 0;
    static float speed_arr[10] = {0.0};                     /* 存储速度进行滤波运算 */

    if (sp_count == ms)                                     /* 计算一次速度 */
    {
        /* 计算电机转速
           第一步 ：计算ms毫秒内计数变化量
           第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
           第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
           第四步 ：除以减速比即可得出电机转速
        */
        g_encode.encode_now = encode_now;                                /* 取出编码器当前计数值 */
        g_encode.speed = (g_encode.encode_now - g_encode.encode_old);    /* 计算编码器计数值的变化量 */

        speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / (ROTO_RATIO) );    /* 保存电机转速 */

        g_encode.encode_old = g_encode.encode_now;          /* 保存当前编码器的值 */

        /* 累计10次速度值，后续进行滤波*/
        if (k == 10)
        {
            for (i = 10; i >= 1; i--)                       /* 冒泡排序*/
            {
                for (j = 0; j < (i - 1); j++)
                {
                    if (speed_arr[j] > speed_arr[j + 1])    /* 数值比较 */
                    {
                        temp = speed_arr[j];                /* 数值换位 */
                        speed_arr[j] = speed_arr[j + 1];
                        speed_arr[j + 1] = temp;
                    }
                }
            }

            temp = 0.0;

            for (i = 2; i < 8; i++)                         /* 去除两边高低数据 */
            {
                temp += speed_arr[i];                       /* 将中间数值累加 */
            }

            temp = (float)(temp / 6);                       /*求速度平均值*/

            /* 一阶低通滤波
             * 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
             * 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
             * q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
             */
           speed_motor = (s16)( ((float)0.48 * temp) + (speed_motor * (float)0.52) );
            k = 0;
        }
        sp_count = 0;
    }
    sp_count ++;
    return speed;
}


int gtim_get_encode(void)
{
    return ( int32_t )__HAL_TIM_GET_COUNTER(&ENC_htim)+g_timx_encode_count*4095 ;   /* 总的编码器值 = 当前计数值 + 之前累计编码器的值 */

}


//获取电流采样偏置
void GetCurrentOffset(void)
{
 uint16_t i;
 uint16_t a_offset,b_offset,bus_offset;
 uint32_t a_offset_sum,b_offset_sum,bus_offset_sum;
 a_offset_sum = 0;
 b_offset_sum = 0;
 bus_offset_sum = 0;
 for(i=0;i<100;i++)
 {
	 a_offset = HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3);
	 b_offset = HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1);
   bus_offset =HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2);


	 //c_offset = current.ADC_c;
	 a_offset_sum = a_offset_sum + a_offset;
	 b_offset_sum = b_offset_sum + b_offset;
	 bus_offset_sum = bus_offset_sum + bus_offset;
	 HAL_Delay(1);
 }
 hPhaseA_OffSet = a_offset_sum/100;
 hPhaseB_OffSet = b_offset_sum/100;


 hPhasebus_OffSet= bus_offset_sum/100;


}


//获取PWM绝对角度值
float Get_pwm_angle(void)
{
   s32 temp;
   float angle=0.0f;
   angle = pwm_encoder.high_time;

   //angle = pwm_encoder.high_time;

//    angle=(pwm_encoder.high_time*(pwm_encoder.duty-0.00381f))/(pwm_encoder.period*0.99417f)*4096;
//
//   angle+=250;

//    temp = (s32)(angle * (s32)(U32_MAX / (4066)));
//    temp *= (POLE_PAIR_NUM);
//    temp=(s16)(temp/65536);


  temp = (s32)(angle) * (s32)(U32_MAX / (pwm_encoder.period));
  temp *= (POLE_PAIR_NUM);
  temp=((s16)(temp/65536 ));


   return fmodf(((float)(angle) * (2.0f * pi) / (4095 ))*POLE_PAIR_NUM,2*pi);;
   //return angle;

}


//测试使用速度，
void get_speed(void)
{
   static int32_t encode_last;
   int32_t num;
   static float speed;
   static uint8_t cnt;


   if(cnt==5)
   {
      cnt=0;

    int32_t encode ;

    encode= ENC_TIM->CNT + enc_over_flag*4095;

    //计数差值
   num = encode - encode_last;


   speed=(float)(g_encode.speed * ((1000 / 5) * 60.0) / REDUCTION_RATIO / (ROTO_RATIO) );


  //清零溢出标志位
  enc_over_flag=0;

  encode_last=ENC_TIM->CNT;


   }

   cnt++;


}


//斜坡函数
float motor_ramp(float t_value,float acc_time) //电机爬坡  爬坡时间s
{
	static float actual_value = 0.0f;
  static uint8_t cnt_ramp=0;
	float ram_acc_value ;
	float delta;
  if(cnt_ramp==0)
  {
     delta = acc_time/(1/PWM_FREQ);           //  acc_time/1/1600      320
	   ram_acc_value = (t_value-actual_value)/delta;
     cnt_ramp=1;
  }


  if(actual_value<t_value)
  {
    actual_value += ram_acc_value;
  }
  else
  {
    actual_value=t_value;
  }


   return actual_value;
}

// 斜坡函数
// target_speed：目标速度，由用户或控制算法设定。
// current_speed：当前速度，通过传感器测量或控制算法计算得出。
// ramp_rate：斜坡速率，定义速度变化的最大速率（单位：速度/秒）设为1000
// delta_time：时间步长，即控制循环的时间间隔。               0.001
float ApplyRampFunction(float target_speed, float current_speed, float ramp_rate, float delta_time)
{
    float speed_difference = target_speed - current_speed;

    if (speed_difference > ramp_rate * delta_time) {
        // 限制加速度
        current_speed += ramp_rate * delta_time;
    } else if (speed_difference < -ramp_rate * delta_time) {
        // 限制减速度
        current_speed -= ramp_rate * delta_time;
    } else {
        // 在加速度范围内，直接设置为目标速度
        current_speed = target_speed;
    }

    return current_speed;
}


//获取电角速度      给定机械速度转化为电角速度
float get_Electric_angular_velocity(float speed)
{
  float electric_angle;

  electric_angle = (float)(speed * PI / 30);
  electric_angle = electric_angle * POLE_PAIR_NUM;

  return electric_angle;
}


//电流前馈解耦
VOLTAGE_DQ_DEF Current_feedforward(CURRENT_DQ_DEF idq)
{
  VOLTAGE_DQ_DEF v_dq;
  v_dq.Vd=(float)(-get_Electric_angular_velocity(speed_motor)*Lq*idq.Iq);
  v_dq.Vq=(float)(get_Electric_angular_velocity(speed_motor)*(flux+(Ld*idq.Id)));

  return v_dq;


}


//转矩环
MTPA_Table_Entry TorqueControlLoop(float torque_ref,float speed_max)
{


  //seepd_setpoint = ApplyRampFunction(speed,seepd_setpoint,500.0f,0.001f);

  //static float torque;

  //torque=ApplyRampFunction(torque_ref,torque,1)


  static MTPA_Table_Entry IDQ;
  MTPA_Table_Entry IDQ_ref;

  IDQ_ref = Get_MTPA_iqd (torque_ref);

  //限速要加20的转速，防止在限速状态里来回切换
  if(speed_motor>speed_max)
  {
     speed_pid_1.max_output=IDQ_ref.iq;
     speed_pid_1.min_output=0;
     IDQ.iq = PID_Controller(&speed_pid_1,(speed_max+100),speed_motor);
     IDQ.id= 0.0f;

  }
  else
  {
       IDQ=IDQ_ref;
//    IDQ.iq=ApplyRampFunction(IDQ_ref.iq,IDQ.iq,30,0.0001f);
//    IDQ.id=ApplyRampFunction(IDQ_ref.id,IDQ.id,30,0.0001f);

  }


  return IDQ;


    // 读取转矩需求
    //float torque_ref = 1.0;


    // 计算电磁转矩
    //float torque_actual = 1.5 * POLE_PAIR_NUM * (flux * Current_Idq.Iq + (Ld - Lq) * Current_Idq.Id * Current_Idq.Iq);


    //斜坡函数防止急停急起   10Nm/s
    //torque_ref = ApplyRampFunction(torque_ref, torque_actual, 5, 0.001);


    //float  torque_control_output =PI_Controller(&d_axis_controller,torque_ref,torque_actual);
    //float  torque_control_output =1.0f;

//    // 计算转矩误差
    //float torque_error = torque_ref - torque_actual;
//
//    // PI控制器计算目标转矩对应的电流参考值
//    torque_error_integral += torque_error * Ki_torque;
//    float torque_control_output = torque_error * Kp_torque + torque_error_integral;


    // 使用MTPA策略计算d轴和q轴电流参考值

    //CalculateMTPACurrents(torque_control_output, &id_ref, &iq_ref);

//    // // 使用PI控制器调节d轴和q轴电流
//    // float id_actual = ReadCurrentD();
//    // float iq_actual = ReadCurrentQ();
//
//    float vd_ref = PIController(id_ref, Current_Idq.Id);
//    float vq_ref = PIController(iq_ref, Current_Idq.Iq);


}


// 示例PI控制器实现
float PIController(float ref, float actual)
{
    static float integral = 0;
    float error = ref - actual;
    integral += error * Ki_torque;
    return error * Kp_torque + integral;
}


//MTPA计算
void CalculateMTPACurrents(float Te, float *id_ref, float *iq_ref)
 {
     float id = -flux / (2 * (Ld - Lq));
    float iq = sqrt(pow((2 * Te) / (3 * POLE_PAIR_NUM * (Ld - Lq)), 2) - pow(id, 2));
    *id_ref = id;
    *iq_ref = iq;

    // 电流限制
    if (fabs(*id_ref) > Imax) {
        *id_ref = (*id_ref > 0) ? Imax : -Imax;
    }
    if (fabs(*iq_ref) > Imax) {
        *iq_ref = (*iq_ref > 0) ? Imax : -Imax;
    }
}


//限流保护         //加到电流环同频率
void CurrentLimitProtection(void)
{
  //获取相电流和母线电流
  iabc=get_Iab();
  //Bus_current;
  if (Bus_current > BUS_CURRENT_LIMIT ||
        iabc.Ia > PHASE_CURRENT_LIMIT ||
        iabc.Ib > PHASE_CURRENT_LIMIT ||
        iabc.Ic > PHASE_CURRENT_LIMIT)
  {

        // 触发保护机制 关闭PWM输出
       ClosePWM_Protection();
    }


}


// 慢速保护机制      //加到转矩环同频率
void MonitorProtection()
{
    //失速保护，，速度低于50则开启
    //  if (motor_speed < STALL_SPEED_THRESHOLD) {
    //     TriggerStallProtection();
    // }


}


//堵转保护
void CheckStallProtection(float motor_speed)
{
    uint32_t current_time = HAL_GetTick(); // 获取当前时间 (ms)
    static uint32_t stall_detection_start_time;


    if (motor_speed < STALL_SPEED_THRESHOLD) {
        if (stall_detection_start_time == 0) {
            // 开始检测失速
            stall_detection_start_time = current_time;
        } else if ((current_time - stall_detection_start_time) > STALL_TIME_WINDOW) {
            // 失速时间超过阈值，触发失速保护
            //TriggerStallProtection();
            ClosePWM_Protection();
        }
    } else {
        // 转速正常，重置失速检测时间
        stall_detection_start_time = 0;
    }
}

//关闭PWM输出
void ClosePWM_Protection(void)
{
  // 停止 TIM1 的所有通道的 PWM 输出
    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
    //HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);

    HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_1);
	  HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Stop(&htim1,TIM_CHANNEL_3);

    // 将 TIM1 的所有通道的 CCR 寄存器置为 0
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
    //__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);


}


// 定义 Clarke 变换函数
void clarke_transform(float i_a, float i_b, float i_c, float *i_alpha, float *i_beta) {
    *i_alpha = i_a;
    *i_beta = (i_a + 2 * i_b) / 1.7320508076f;
}

// 定义 Park 变换函数
void park_transform(float i_alpha, float i_beta, float theta, float *i_d, float *i_q) {
    float cos_theta = cosf(theta);
    float sin_theta = sinf(theta);
    *i_d = i_alpha * cos_theta + i_beta * sin_theta;
    *i_q = -i_alpha * sin_theta + i_beta * cos_theta;
}


//此SVPWM算法为正常
void SVPWM_Calculate_GPT_2(float V_alpha, float V_beta) {
    float Ta,Tb,Tc;
    float Vd = V_alpha/Udc;
    float Vq = V_beta/Udc;

    float T1, T2, T0;
    float sector;
    float Vref = sqrtf(Vd * Vd + Vq * Vq);
    float angle = atan2f(V_beta, V_alpha);

    if (angle < 0) {
        angle += 2 * PI;
    }

    sector = floorf(angle / (PI / 3)) + 1;

    float t1 = Vref * sinf((PI / 3) - fmodf(angle, PI / 3)) / sinf(PI / 3);
    float t2 = Vref * sinf(fmodf(angle, PI / 3)) / sinf(PI / 3);

    T1 = t1 ; // Normalize to PWM period
    T2 = t2 ; // Normalize to PWM period
    T0 = (1 - T1 - T2) / 2;

    switch ((int)sector) {
        case 1:
            Ta = T1 + T2 + T0;
            Tb = T2 + T0;
            Tc = T0;
            break;
        case 2:
            Ta = T1 + T0;
            Tb = T1 + T2 + T0;
            Tc = T0;
            break;
        case 3:
            Ta = T0;
            Tb = T1 + T2 + T0;
            Tc = T2 + T0;
            break;
        case 4:
            Ta = T0;
            Tb = T1 + T0;
            Tc = T1 + T2 + T0;
            break;
        case 5:
            Ta = T2 + T0;
            Tb = T0;
            Tc = T1 + T2 + T0;
            break;
        case 6:
            Ta = T1 + T2 + T0;
            Tb = T0;
            Tc = T1 + T0;
            break;
    }


     __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (uint16_t)(Ta*PWM_PERIOD));
     __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (uint16_t)(Tb*PWM_PERIOD));
     __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (uint16_t)(Tc*PWM_PERIOD));


}


//此SVPWM算法为正常
void SVPWM(VOLTAGE_ALPHA_BETA_DEF *U_alphaBeta, SVPWM_Def *svpwm)
{
    float sum;
    float k_svpwm;

    svpwm->Ts = 1.0f; // SVPWM的采样周期

    svpwm->U_alpha = U_alphaBeta->Valpha/Udc;
    svpwm->U_beta = U_alphaBeta->Vbeta/Udc;
    //电压归一化
    U_alphaBeta->Valpha=U_alphaBeta->Valpha/Udc;
    U_alphaBeta->Vbeta=U_alphaBeta->Vbeta/Udc;
	  // step1 计算u1、u2和u3
    // 计算SVPWM算法中的三个控制电压u1、u2和u3
    svpwm->u1 = U_alphaBeta->Vbeta;
    svpwm->u2 = -0.8660254f * U_alphaBeta->Valpha - 0.5f * U_alphaBeta->Vbeta; // sqrt(3)/2 ≈ 0.86603
    svpwm->u3 = 0.8660254f * U_alphaBeta->Valpha - 0.5f * U_alphaBeta->Vbeta;
	 // step2：扇区判断
    // 根据u1、u2和u3的正负情况确定所处的扇区
    svpwm->sector = (svpwm->u1 > 0.0f) + ((svpwm->u2 > 0.0f) << 1) + ((svpwm->u3 > 0.0f) << 2); // N=4*C+2*B+A

    // step3:计算基本矢量电压作用时间（占空比）
    // 根据扇区的不同，计算对应的t_a、t_b和t_c的值，表示生成的三相电压的时间
    switch (svpwm->sector)
    {
    case 5:
        // 扇区5
        svpwm->t4 = svpwm->u3;
        svpwm->t6 = svpwm->u1;
        sum = svpwm->t4 + svpwm->t6;
        if (sum > svpwm->Ts)
        {
            k_svpwm = svpwm->Ts / sum; //
            svpwm->t4 = k_svpwm * svpwm->t4;
            svpwm->t6 = k_svpwm * svpwm->t6;
        }
        svpwm->t7 = (svpwm->Ts - svpwm->t4 - svpwm->t6) / 2;
        svpwm->ta = svpwm->t4 + svpwm->t6 + svpwm->t7;
        svpwm->tb = svpwm->t6 + svpwm->t7;
        svpwm->tc = svpwm->t7;
        break;
      case 1:
        // 扇区1
        svpwm->t2 = -svpwm->u3;
        svpwm->t6 = -svpwm->u2;
        sum = svpwm->t2 + svpwm->t6;
        if (sum > svpwm->Ts)
        {
            k_svpwm = svpwm->Ts / sum; // 计算缩放系数
            svpwm->t2 = k_svpwm * svpwm->t2;
            svpwm->t6 = k_svpwm * svpwm->t6;
        }
        svpwm->t7 = (svpwm->Ts - svpwm->t2 - svpwm->t6) / 2;
        svpwm->ta = svpwm->t6 + svpwm->t7;
        svpwm->tb = svpwm->t2 + svpwm->t6 + svpwm->t7;
        svpwm->tc = svpwm->t7;
        break;
    case 3:
        // 扇区3
        svpwm->t2 = svpwm->u1;
        svpwm->t3 = svpwm->u2;
        sum = svpwm->t2 + svpwm->t3;
        if (sum > svpwm->Ts)
        {
            k_svpwm = svpwm->Ts / sum; //
            svpwm->t2 = k_svpwm * svpwm->t2;
            svpwm->t3 = k_svpwm * svpwm->t3;
        }
        svpwm->t7 = (svpwm->Ts - svpwm->t2 - svpwm->t3) / 2;
        svpwm->ta = svpwm->t7;
        svpwm->tb = svpwm->t2 + svpwm->t3 + svpwm->t7;
        svpwm->tc = svpwm->t3 + svpwm->t7;
        break;

    case 2:
        // 扇区2
        svpwm->t1 = -svpwm->u1;
        svpwm->t3 = -svpwm->u3;
        sum = svpwm->t1 + svpwm->t3;
        if (sum > svpwm->Ts)
        {
            k_svpwm = svpwm->Ts / sum; //
            svpwm->t1 = k_svpwm * svpwm->t1;
            svpwm->t3 = k_svpwm * svpwm->t3;
        }
        svpwm->t7 = (svpwm->Ts - svpwm->t1 - svpwm->t3) / 2;
        svpwm->ta = svpwm->t7;
        svpwm->tb = svpwm->t3 + svpwm->t7;
        svpwm->tc = svpwm->t1 + svpwm->t3 + svpwm->t7;
        break;

    case 6:
        // 扇区6
        svpwm->t1 = svpwm->u2;
        svpwm->t5 = svpwm->u3;
        sum = svpwm->t1 + svpwm->t5;
        if (sum > svpwm->Ts)
        {
            k_svpwm = svpwm->Ts / sum; //
            svpwm->t1 = k_svpwm * svpwm->t1;
            svpwm->t5 = k_svpwm * svpwm->t5;
        }
        svpwm->t7 = (svpwm->Ts - svpwm->t1 - svpwm->t5) / 2;
        svpwm->ta = svpwm->t5 + svpwm->t7;
        svpwm->tb = svpwm->t7;
        svpwm->tc = svpwm->t1 + svpwm->t5 + svpwm->t7;
        break;

    case 4:
        // 扇区4
        svpwm->t4 = -svpwm->u2;
        svpwm->t5 = -svpwm->u1;
        sum = svpwm->t4 + svpwm->t5;
        if (sum > svpwm->Ts)
        {
            k_svpwm = svpwm->Ts / sum; //
            svpwm->t4 = k_svpwm * svpwm->t4;
            svpwm->t5 = k_svpwm * svpwm->t5;
        }
        svpwm->t7 = (svpwm->Ts - svpwm->t4 - svpwm->t5) / 2;
        svpwm->ta = svpwm->t4 + svpwm->t5 + svpwm->t7;
        svpwm->tb = svpwm->t7;
        svpwm->tc = svpwm->t5 + svpwm->t7;
        break;


    default:
        break;
    }

		// step4：6路PWM输出
    //set_PWM_value(PWM_PERIOD*svpwm->ta,PWM_PERIOD*svpwm->tb,PWM_PERIOD*svpwm->tc);

       __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,PWM_PERIOD*svpwm->ta);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,PWM_PERIOD*svpwm->tb);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,PWM_PERIOD*svpwm->tc);
//    vofa_JustFloat_output(PWM_PERIOD * svpwm->ta,PWM_PERIOD * svpwm->tb,PWM_PERIOD * svpwm->tc,0.0f);
}


float get_encode_angle(void)
{

  return fmodf(((float)(TIM2->CNT) * (2.0f * pi) / (4095 ))*5,2*pi);
}