FOC_TEST/motor/adc.c

/**********************************
 * 文件名称: adc.c
 * 功能描述: ADC采样和电机控制模块
 * 主要功能:
 * 1. 温度传感器采样与转换
 * 2. 电流偏移校准
 * 3. FOC算法计算
 * 4. ADC中断处理
 * 
 * 注意事项:
 * - 使用ADC1进行电流和电压采样
 * - 使用ADC3进行UVW端电压和温度采样
 * - 采样频率为10kHz（与PWM频率同步）
 **********************************/
#include "main.h"
#include "arm_math.h"
#include "adc.h"
#include "board_config.h"



#define _constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// 测试变量
int32_t ia_test,ib_test,ic_test;

float Ua,Ualpha,Ub,Ubeta, Uc, dc_a,dc_b,dc_c;
//float err;

// 电流和电压变量
double Ia,Ib,Ic;
float Ia_test,Ib_test,Ic_test;
float Vbus;
float Temp;
// ADC采样值数组
uint16_t ADC3ConvertedValue[ADC3_DMA_BUFFER_SIZE];
uint16_t i = 0;

// 电流偏移校准值
uint32_t A_offset,B_offset,C_offset;
uint8_t get_offset_flag = 0;
uint8_t get_offset_sample_cnt = 0;

// 速度闭环控制标志和参考电流
u8 speed_close_loop_flag;
float Iq_ref = 0.0f;
float Id_ref = 0.0f;
// EKF相关变量
float EKF_Hz;

// 角度变量
float theta_add;
float theta;

// 电机参数（从其他模块导入）
extern float Rs;
extern float Ls;
extern float flux;

/**
 * @brief       根据热敏电阻阻值计算温度
 * @param       rt_kohm: 热敏电阻阻值，单位为千欧姆
 * @retval      温度值，单位为摄氏度
 */
float calculate_temperature(float rt_kohm)
{
    float temp;
    const float Rp_kohm = 10.0f;          /* 10K热敏电阻参考电阻 */
    const float T2 = (273.15f + 25.0f);   /* T2温度，单位为K */
    const float Bx = 3380.0f;             /* B值 */
    const float Ka = 273.15f;             /* 绝对零度，单位为℃ */
    
    temp = rt_kohm / Rp_kohm;             /* 电阻比值 */
    temp = log(temp);                     /* 取自然对数 */
    temp /= Bx;                           /* 除以B值 */
    temp += (1.0f / T2);                  /* 加上1/T2 */
    temp = 1.0f / temp;                   /* 取倒数 */
    temp -= Ka;                           /* 减去绝对零度 */
    
    return temp;
}

/**
 * @brief   获取电流偏移值
 * @param   a_offset    A相电流偏移值指针
 * @param   b_offset    B相电流偏移值指针
 * @note    采集128次求平均值，在PWM关闭时采集
 *          - ADC1->JDR2: A相电流采样值
 *          - ADC1->JDR3: B相电流采样值
 *          - 右移7位 = 除以128求平均值
 */
static void get_offset(uint32_t *a_offset, uint32_t *b_offset, uint32_t* c_offset)
{
  if(get_offset_sample_cnt<128)
  {
    *a_offset += ADC1->JDR1;
    *b_offset += ADC2->JDR1;
		*c_offset += ADC3->JDR1;
    get_offset_sample_cnt++;
  }
  else
  {
    *a_offset >>= 7; 
    *b_offset >>= 7;
		*c_offset >>= 7;
    get_offset_sample_cnt=0;
    TIM_CtrlPWMOutputs(PWM_TIM,DISABLE);
    get_offset_flag = 2;
  }
}

/**
 * @brief 开环角度更新
 */
static void Angle_OpenLoop_Update(float step)
{
    if(get_motor()->direction == MOTOR_CW)
        get_motor()->eleangle += step;
    else
        get_motor()->eleangle -= step;
}

static void FOC_parm_input(void)
{
		get_foc_input()->Id_ref = 0.0f;               
    get_foc_input()->Tpwm = PWM_TIM_PULSE_TPWM;         // FOC算法需要的PWM周期
    get_foc_input()->Udc = Vbus;
    get_foc_input()->Rs = Rs;
    get_foc_input()->Ls = Ls;
    get_foc_input()->flux = flux;
    
    get_foc_input()->ia = Ia;
    get_foc_input()->ib = Ib;
    get_foc_input()->ic = Ic;    
}
//static void FOC_parm_output(void)
//{
//		  EKF_Hz = get_foc_ouput()->EKF[2]/(2.0f*PI) ;
//}


static void uvw_current_and_vbus_get(float* vbus, double* iu, double* iv, double* iw)
{
		*vbus = ADC_TO_VBUS( ADC3ConvertedValue[ADC3_VBUS_INDEX]);
		*iu =  ADC_TO_CURRENT((int16_t)((int16_t)ADC1->JDR1 - (int16_t)A_offset ));    
		*iv =  ADC_TO_CURRENT((int16_t)((int16_t)ADC2->JDR1 - (int16_t)B_offset ));
		*iw = 	ADC_TO_CURRENT((int16_t)((int16_t)ADC3->JDR1 - (int16_t)C_offset ));
		Ia_test = *iu;
		Ib_test = *iv;
		Ic_test = *iw;
}


/**
 * @brief 修改后的motor_run()函数
 * @note 集成了完整的开环→闭环切换逻辑
 */
void motor_run(void)
{
    uvw_current_and_vbus_get(&Vbus, &Ia, &Ib, &Ic);
    //Hall_TypeDef* hall = Hall_Get();
		//MotorParam_t* motor = get_motor();

// Hall_Update_Interpolated_Angle();
  if(speed_close_loop_flag==0)         //速度环闭环切换控制，电机刚启动时速度环不闭环
  {                                    //并且电流参考值缓慢增加（防冲击），速度达到一定值
    if((Iq_ref<MOTOR_STARTUP_CURRENT)) //速度切入闭环
    {                                  //电流环在电机运行过程中全程闭环
      Iq_ref += 0.00003f;              //角度在电机刚启动时就闭环运行，无需强拖，得益于卡尔曼滤波做
    }                                  //状态观测器低速性能比教好
    else
    {
      speed_close_loop_flag=1;
    }
  }
  else
  {
    if(speed_close_loop_flag==1)
    {
      if(Iq_ref>(MOTOR_STARTUP_CURRENT/2.0f))
      {
        Iq_ref -= 0.001f;
      }
      else
      {
        speed_close_loop_flag=2;
      }
    }
  }
  
//     if(FOC_Output.EKF[2]>SPEED_LOOP_CLOSE_RAD_S)      //无感方式运行，状态观测器得到角度和速度信息
//  {
//    FOC_Input.Id_ref = 0.0f;
//    Speed_Fdk = FOC_Output.EKF[2];
//    FOC_Input.Iq_ref = Speed_Pid_Out;
//  }
//  else
//  {
//    FOC_Input.Id_ref = 0.0f;
//    FOC_Input.Iq_ref = Iq_ref;
//    Speed_Pid.I_Sum = Iq_ref;
//  }
//  FOC_Input.theta = FOC_Output.EKF[3];
//  FOC_Input.speed_fdk = FOC_Output.EKF[2];
//	EKF_Hz = FOC_Output.EKF[2]/(2.0f*PI);
//  FOC_Input.Id_ref = 0.0f;               
//  FOC_Input.Tpwm = PWM_TIM_PULSE_TPWM;         //FOC运行函数需要用到的输入信息
//  FOC_Input.Udc = Vbus;
//  FOC_Input.Rs = Rs;
//  FOC_Input.Ls = Ls;
//  FOC_Input.flux = flux;
//  
//  FOC_Input.ia = Ia;
//  FOC_Input.ib = Ib;
//  FOC_Input.ic = Ic;           
 // foc_algorithm_step();       //整个FOC运行函数（包括无感状态观测器，电流环，SVPWM，坐标变换，电机参数识别）
    // ========== FOC参数设置 ==========
    FOC_parm_input();
		get_foc_input()->Iq_ref = 1.0f;
		Hall_Update_Interpolated_Angle();
		static uint16_t cnt = 0;
		if(cnt <= 10000)
		{
			cnt++;
			theta += 0.01f;
			if(theta >= 2* PI)
			{
				theta -= 2 * PI;
			}
			if(theta < 0.0f)
			{
				theta += 2 * PI;
			}
			get_foc_input()->theta = theta;
			
		}else
		{
			get_foc_input()->theta = Hall_Get()->angle;
		}
			
  
    // ========== 执行FOC算法 ==========
     motor_foc_openloop_run();
    //motor_ekf_closeloop_run();
    // ========== PWM输出更新 ==========
    if(motor_start_stop == 1 )//&& motor->ctrl_state != CTRL_STATE_IDLE)
    {
        PWM_TIM->CCR1 = (u16)(FOC_Output.Tcmp1);
        PWM_TIM->CCR2 = (u16)(FOC_Output.Tcmp2);
        PWM_TIM->CCR3 = (u16)(FOC_Output.Tcmp3);
    }
    else
    {
        PWM_TIM->CCR1 = PWM_TIM_PULSE >> 1;
        PWM_TIM->CCR2 = PWM_TIM_PULSE >> 1;
        PWM_TIM->CCR3 = PWM_TIM_PULSE >> 1;
    }
    
    // ========== 停止命令处理 ==========
//    if(motor_start_stop == 0 && motor->ctrl_state != CTRL_STATE_IDLE && 
//       motor->ctrl_state != CTRL_STATE_STOPPING)
//    {
//        motor->ctrl_state = CTRL_STATE_STOPPING;
//    }
}


#if 0
/**
 * @brief   电机运行函数
 * @note    在PWM中断中调用，频率10kHz
 *          - 读取ADC采样值，获取A/B相电流
 *          - 根据基尔霍夫定律计算C相电流(Ia + Ib + Ic = 0)
 *          - 速度环控制，计算速度PID输出
 *          - 根据控制模式选择开环/闭环控制
 *          - 执行FOC算法，计算SVPWM占空比
 *          - 更新PWM占空比
 */
void motor_run(void)
{
   
		uvw_current_and_vbus_get(&Vbus, &Ia, &Ib, &Ic); // 获取母线电压，相电流
	
		// 选择电机运行方式 霍尔 开环加闭环 无感EKF
    if(speed_close_loop_flag==0)         // 启动阶段，逐渐增加电流
    {                                    // 电流从零开始，避免启动冲击
        if((Iq_ref<MOTOR_STARTUP_CURRENT)) // 未达到启动电流
        {
            Iq_ref += 0.00004f;              // 缓慢增加电流
        }
        else
        {
            speed_close_loop_flag=1;
        }
    }
    else
    {
        if(speed_close_loop_flag==1)
        {
            if(Iq_ref>(MOTOR_STARTUP_CURRENT/2.0f))
            {
                Iq_ref -= 0.001f;
            }
            else
            {
                speed_close_loop_flag=2;
            }
        }
    }
    
    // 根据传感器类型选择FOC控制模式  
#ifdef  HALL_FOC_SELECT            // 使用霍尔传感器的FOC控制  
    
//    if((hall_speed*2.0f*PI)>SPEED_LOOP_CLOSE_RAD_S)     // 速度达到闭环阈值
//    {
//        FOC_Input.Id_ref = 0.0f;
//        Speed_Fdk = hall_speed*2.0f*PI;
//        FOC_Input.Iq_ref = Speed_Pid_Out;
//    }
//    else
//    {
//        FOC_Input.Id_ref = 0.0f;
//        FOC_Input.Iq_ref = Iq_ref;
//        Speed_Pid.I_Sum = Iq_ref;
//    }
//    FOC_Input.theta = hall_angle;
//    FOC_Input.speed_fdk = hall_speed*2.0f*PI;
    
#endif
    
#ifdef  SENSORLESS_FOC_SELECT            // 无传感器FOC控制
    
    if(FOC_Output.EKF[2]>SPEED_LOOP_CLOSE_RAD_S)      // 速度达到闭环阈值
    {
        FOC_Input.Id_ref = 0.0f;
        Speed_Fdk = FOC_Output.EKF[2];
        FOC_Input.Iq_ref = Speed_Pid_Out;
    }
    else
    {
        FOC_Input.Id_ref = 0.0f;
        FOC_Input.Iq_ref = Iq_ref;
        Speed_Pid.I_Sum = Iq_ref;
    }
    FOC_Input.theta = FOC_Output.EKF[3];
    FOC_Input.speed_fdk = FOC_Output.EKF[2];
    
#endif
    
    //============================== FOC算法计算 ==============================
    
    FOC_parm_input();
		
    FOC_parm_output();
		if(get_motor()->direction == MOTOR_CW)
			get_motor()->eleangle += 0.01f;  // 使用 float 后缀
		else if(get_motor()->direction == MOTOR_CCW)
			get_motor()->eleangle -= 0.01f;
//		if(speed_close_loop_flag == 2)	
//		{
//			if(get_motor()->direction == MOTOR_CW)
//				get_motor()->eleangle += Hall_Get()->angle_add;  // 使用 float 后缀
//			else if(get_motor()->direction == MOTOR_CCW)
//				get_motor()->eleangle -= Hall_Get()->angle_add;
//		}			
	
		
		if(get_motor()->eleangle >= 2.0f * PI)
		{
				get_motor()->eleangle -= 2.0f * PI;
		}else if(get_motor()->eleangle <= 0.0f * PI)
		{
				get_motor()->eleangle += 2.0f * PI;
		}
		
		get_foc_input()->theta = get_motor()->eleangle;
		get_foc_input()->Id_ref = Id_ref;
		get_foc_input()->Iq_ref = Iq_ref;
		
		motor_foc_openloop_run();
		
    // foc_algorithm_step();       // 执行FOC算法，计算SVPWM占空比
    
    if(motor_start_stop==1)
    {
        PWM_TIM->CCR1 = (u16)(FOC_Output.Tcmp1);     // 更新SVPWM占空比
        PWM_TIM->CCR2 = (u16)(FOC_Output.Tcmp2);
        PWM_TIM->CCR3 = (u16)(FOC_Output.Tcmp3);
    }
    else
    {
        PWM_TIM->CCR1 = PWM_TIM_PULSE>>1;
        PWM_TIM->CCR2 = PWM_TIM_PULSE>>1;
        PWM_TIM->CCR3 = PWM_TIM_PULSE>>1;
    }
    
    //communication_task();
}

#endif

/**
 * @brief   ADC中断处理函数
 * @note    处理ADC注入通道转换完成中断
 *          - 当偏移校准完成后，执行电机运行函数
 *          - 当需要校准偏移时，执行偏移校准函数
 *          为什么VBUS不需要零偏校准？ 开始采样即VBUS已经接在电源上了
 */
void ADC_IRQHandler(void)
{
    // 检查ADC1注入组完成（三个ADC已同步完成）
    if((SAMPLE_ADC1->SR & ADC_FLAG_JEOC) == ADC_FLAG_JEOC)
    {  
        // 处理数据
        if(get_offset_flag == 2)
        {
//            hall_angle += hall_angle_add;
//            if(hall_angle < 0.0f)
//                hall_angle += 2.0f * PI;
//            else if(hall_angle > (2.0f * PI))
//                hall_angle -= 2.0f * PI;
            
            motor_run();

        }
        else if(get_offset_flag == 1) // get_offset_flag = 2 表示偏移已经获取完成
        {
            get_offset(&A_offset, &B_offset, &C_offset);
        }
				  
        // 清除所有三个ADC的JEOC标志
        SAMPLE_ADC1->SR = ~ADC_SR_JEOC;
				SAMPLE_ADC2->SR = ~ADC_SR_JEOC;
        SAMPLE_ADC3->SR = ~ADC_SR_JEOC;
    }
}