【直线电机】_直流直线电机

项目场景：

病例切片扫描仪中需要XYZ三轴电机，由于XY轴的运动速度稳定性及定位精度要求需要伺服电机。传统解决方案是步进电机加丝杆，性能没有直线电机优越。本文开发一套适用于扫描仪领域的直线电机平台。

原理图

`采样下桥臂三电阻采样方案，STM32主控，FOC算法，光栅尺编码器。

原因分析：

FOC代码

#include "main.h" #include "ST_FOC.h" #include "stdlib.h" #include "i2c.h" #include "cmsis_os.h" #define PWM2_MODE 0 #define PWM1_MODE 1 /************overcurrent protection written by xiaozhirui 2019/3/6****************/ Curr_Components Stat_Curr_a_b; /*Stator currents Ia,Ib*/ Curr_Components Stat_Curr_alfa_beta; /*Ialpha & Ibeta, Clarke's transformations of Ia & Ib */ Curr_Components Stat_Curr_q_d; /*Iq & Id, Parke's transformations of Ialpha & Ibeta */ Volt_Components Stat_Volt_a_b; /*Stator voltages Va, Vb*/ Volt_Components Stat_Volt_q_d; /*Vq & Vd, voltages on a reference frame synchronous with the rotor flux*/ Volt_Components Stat_Volt_alfa_beta; /*Valpha & Vbeta, RevPark transformations of Vq & Vd*/ int32_t Origin_Location = 0; uint16_t EXTI3_IRQ_FLAG = 0; uint32_t wTimebase=0; static Trig_Components Vector_Components; const s16 hSin_Cos_Table[256] = SIN_COS_TABLE; static Curr_Components Stat_Curr_q_d_ref_ref; uint16_t hPhaseAOffset=0; uint16_t hPhaseBOffset=0; uint16_t hPhaseCOffset=0; int32_t hTorque_Reference, hFlux_Reference , hSpeed_Reference, hLocation_Reference; uint8_t PWM4Direction = PWM2_MODE; uint32_t piout; uint64_t iout; uint16_t Motor_Init_Flag = 0; PID_Struct_t PID_Torque_Init, PID_Flux_Init,PID_Speed_Init,PID_Location_Init; Volt_Components Stat_Volt_q_d; /*Vq & Vd, voltages on a reference frame synchronous with the rotor flux*/ Curr_Components SVPWM_3ShuntGetPhaseCurrentValues(void) { 
    Curr_Components Local_Stator_Currents; int32_t wAux; uint16_t AD_Channel_A_Value; uint16_t AD_Channel_B_Value; uint16_t AD_Channel_C_Value; AD_Channel_A_Value = (ADC1->JDR1)<<1; AD_Channel_B_Value = (ADC1->JDR2)<<1; AD_Channel_C_Value = (ADC1->JDR3)<<1; // printf("\r\n A_Value:%d B_Value:%d C_Value:%d \r\n",AD_Channel_A_Value,AD_Channel_B_Value,AD_Channel_C_Value); //current protection code if(AD_Channel_A_Value> AD_MAXSAMPLE_VALUE || AD_Channel_B_Value> AD_MAXSAMPLE_VALUE || AD_Channel_C_Value>AD_MAXSAMPLE_VALUE \ || AD_Channel_A_Value< AD_MINSAMPLE_VALUE || AD_Channel_B_Value< AD_MINSAMPLE_VALUE || AD_Channel_C_Value< AD_MINSAMPLE_VALUE) { 
   //overcurrent  TIM1_PWM_Disable(); STATE = FAULT; Local_Stator_Currents.qI_Component1 = 0; Local_Stator_Currents.qI_Component2 = 0; return Local_Stator_Currents; } // switch (SECTOR_NUMBER) // { 
    // case 4: // case 5: //Current on Phase C not accessible  // //Ia = (hPhaseAOffset)-(ADC Channel 11 value)  // wAux = (int32_t)(hPhaseAOffset) - AD_Channel_A_Value; // if (wAux < S16_MIN) // { 
    // Local_Stator_Currents.qI_Component1= S16_MIN; // }  // else if (wAux > S16_MAX) // {  // Local_Stator_Currents.qI_Component1= S16_MAX; // } // else // { 
    // Local_Stator_Currents.qI_Component1= wAux; // } // wAux = (int32_t)(hPhaseBOffset)-AD_Channel_B_Value; // // Saturation of Ib // if (wAux < S16_MIN) // { 
    // Local_Stator_Currents.qI_Component2= S16_MIN; // }  // else if (wAux > S16_MAX) // {  // Local_Stator_Currents.qI_Component2= S16_MAX; // } // else // { 
    // Local_Stator_Currents.qI_Component2= wAux; // } // break; //  // case 6: // case 1: //Current on Phase A not accessible  // // Ib = (hPhaseBOffset)-(ADC Channel 12 value) // wAux = (int32_t)(hPhaseBOffset) - AD_Channel_B_Value; // //Saturation of Ib  // if (wAux < S16_MIN) // { 
    // Local_Stator_Currents.qI_Component2= S16_MIN; // }  // else if (wAux > S16_MAX) // {  // Local_Stator_Currents.qI_Component2= S16_MAX; // } // else // { 
    // Local_Stator_Currents.qI_Component2= wAux; // } // // Ia = -Ic -Ib  // wAux = AD_Channel_C_Value - hPhaseCOffset - Local_Stator_Currents.qI_Component2; // //Saturation of Ia // if (wAux> S16_MAX) // { 
    // Local_Stator_Currents.qI_Component1 = S16_MAX; // } // else if (wAux <S16_MIN) // { 
    // Local_Stator_Currents.qI_Component1 = S16_MIN; // } // else // {  // Local_Stator_Currents.qI_Component1 = wAux; // } // break; //  // case 2: // case 3: // Current on Phase B not accessible // // Ia = (hPhaseAOffset)-(ADC Channel 11 value)  // wAux = (int32_t)(hPhaseAOffset) - AD_Channel_A_Value; // //Saturation of Ia  // if (wAux < S16_MIN) // { 
    // Local_Stator_Currents.qI_Component1= S16_MIN; // }  // else if (wAux > S16_MAX) // {  // Local_Stator_Currents.qI_Component1= S16_MAX; // } // else // { 
    // Local_Stator_Currents.qI_Component1= wAux; // } // // Ib = -Ic-Ia; // wAux = AD_Channel_C_Value - hPhaseCOffset - Local_Stator_Currents.qI_Component1; // // Saturation of Ib // if (wAux> S16_MAX) // { 
    // Local_Stator_Currents.qI_Component2=S16_MAX; // } // else if (wAux <S16_MIN) // {  // Local_Stator_Currents.qI_Component2 = S16_MIN; // } // else  // { 
    // Local_Stator_Currents.qI_Component2 = wAux; // }  // break; // default: // break; // } wAux = (int32_t)(hPhaseAOffset) - AD_Channel_A_Value; if (wAux < S16_MIN) { 
    Local_Stator_Currents.qI_Component1= S16_MIN; } else if (wAux > S16_MAX) { 
    Local_Stator_Currents.qI_Component1= S16_MAX; } else { 
    Local_Stator_Currents.qI_Component1= wAux; } wAux = (int32_t)(hPhaseBOffset) - AD_Channel_B_Value; // Saturation of Ib if (wAux < S16_MIN) { 
    Local_Stator_Currents.qI_Component2= S16_MIN; } else if (wAux > S16_MAX) { 
    Local_Stator_Currents.qI_Component2= S16_MAX; } else { 
    Local_Stator_Currents.qI_Component2= wAux; } return(Local_Stator_Currents); } /******************************************************************************* * Function Name : SVPWM_3ShuntCurrentReadingCalibration * Description : Store zero current converted values for current reading network offset compensation in case of 3 shunt resistors *******************************************************************************/ void SVPWM_3ShuntCurrentReadingCalibration(void) { 
    uint16_t bIndex; for(bIndex=0; bIndex <16; bIndex++) { 
    while(!__HAL_ADC_GET_IT_SOURCE(&hadc1,ADC_FLAG_JEOC)) { 
    } hPhaseAOffset += (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1))>>3; hPhaseBOffset += (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2))>>3; hPhaseCOffset += (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3))>>3; /* Clear the ADC1 JEOC pending flag */ __HAL_ADC_CLEAR_FLAG(&hadc1,ADC_FLAG_JEOC); HAL_ADCEx_InjectedStart_IT(&hadc1); } } /******************************************************************************* * Function Name : Clarke Transformation * Description : This function transforms stator currents qIas and * qIbs (which are directed along axes each displaced by * 120 degrees) into currents qIalpha and qIbeta in a * stationary qd reference frame. * qIalpha = qIas * qIbeta = -(2*qIbs+qIas)/sqrt(3) * Input : Stat_Curr_a_b * Output : Stat_Curr_alfa_beta * Return : none. *******************************************************************************/ Curr_Components Clarke(Curr_Components Curr_Input) { 
    Curr_Components Curr_Output; int32_t qIa_divSQRT3_tmp,qIb_divSQRT3_tmp; int16_t qIa_divSQRT3,qIb_divSQRT3; Curr_Output.qI_Component1= Curr_Input.qI_Component1; // qIalpha = qIas qIa_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component1; qIa_divSQRT3_tmp /=32768; qIb_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component2; qIb_divSQRT3_tmp /=32768; qIa_divSQRT3=((int16_t)(qIa_divSQRT3_tmp)); qIb_divSQRT3=((int16_t)(qIb_divSQRT3_tmp)); Curr_Output.qI_Component2 = -qIa_divSQRT3 - qIb_divSQRT3 - qIb_divSQRT3; //qIbeta = -(2*qIbs+qIas)/sqrt(3) // Curr_Output.qI_Component2 = qIa_divSQRT3 + qIb_divSQRT3 + qIb_divSQRT3; return(Curr_Output); } /******************************************************************************* * Function Name : Trig_Functions * Description : This function returns Cosine and Sine functions of the input angle. * Input : angle in s16 format * Output : Cosine and Sine in s16 format * Return : none. *******************************************************************************/ Trig_Components Trig_Functions(int16_t hAngle) { 
    u16 hindex; Trig_Components Local_Components; /* 10 bit index computation */ hindex = (u16)(hAngle + 32768); hindex /= 64; switch (hindex & SIN_MASK) { 
    case U0_90: Local_Components.hSin = hSin_Cos_Table[(u8)(hindex)]; Local_Components.hCos = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))]; break; case U90_180: Local_Components.hSin = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))]; Local_Components.hCos = -hSin_Cos_Table[(u8)(hindex)]; break; case U180_270: Local_Components.hSin = -hSin_Cos_Table[(u8)(hindex)]; Local_Components.hCos = -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))]; break; case U270_360: Local_Components.hSin = -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))]; Local_Components.hCos = hSin_Cos_Table[(u8)(hindex)]; break; default: break; } return (Local_Components); } /******************************************************************************* * Function Name : ENC_Get_Electrical_Angle * Description : Returns the absolute electrical Rotor angle * Input : None * Output : None * Return : Rotor electrical angle: 0 -> 0 degrees, * S16_MAX -> 180 degrees, * S16_MIN -> -180 degrees * Mechanical angle can be derived calling this function and * dividing by POLE_PAIR_NUM *******************************************************************************/ s16 ENC_Get_Electrical_Angle(void) { 
    s32 temp; s32 Distance; Distance = Encoder_Timer_Overflow*60000+TIM3->CNT; if(Distance >= 0) { 
    temp = (s32)(Distance%60000) * (s32)(U32_MAX / 60000); temp /= 65536; } else { 
    temp = (s32)(-Distance%60000) * (s32)(U32_MAX / 60000); temp /= 65536; temp *= -1; } // temp = (s32)(TIM3->CNT) * (s32)(U32_MAX / 64000); return((s16)(temp)); // s16 result } /******************************************************************************* * Function Name : Park Transformation * Description : This function transforms stator currents qIalpha and qIbeta, * which belong to a stationary qd reference frame, to a rotor * flux synchronous reference frame (properly oriented), so as * to obtain qIq and qIds. * qId=qIalpha_tmp*sin(theta)+qIbeta_tmp*cos(Theta) * qIq=qIalpha_tmp*cos(Theta)-qIbeta_tmp*sin(Theta) * Input : Stat_Curr_alfa_beta * Output : Stat_Curr_q_d. * Return : none. *******************************************************************************/ Curr_Components Park(Curr_Components Curr_Input, s16 Theta) { 
    Curr_Components Curr_Output; int32_t qId_tmp_1, qId_tmp_2; int32_t qIq_tmp_1, qIq_tmp_2; int16_t qId_1, qId_2; int16_t qIq_1, qIq_2; int16_t Theta_Angle; Vector_Components = Trig_Functions(Theta); //No overflow guaranteed qIq_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hCos; //Vector_Components.hCos qIq_tmp_1 /= 32768; //No overflow guaranteed qIq_tmp_2 = Curr_Input.qI_Component2 * Vector_Components.hSin; qIq_tmp_2 /= 32768; qIq_1 = ((int16_t)(qIq_tmp_1)); qIq_2 = ((int16_t)(qIq_tmp_2)); //Iq component in Q1.15 Format  Curr_Output.qI_Component1 = ((qIq_1)-(qIq_2)); //No overflow guaranteed qId_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hSin; qId_tmp_1 /= 32768; //No overflow guaranteed qId_tmp_2 = Curr_Input.qI_Component2 * Vector_Components.hCos; qId_tmp_2 /= 32768; qId_1 = (int16_t)(qId_tmp_1); qId_2 = (int16_t)(qId_tmp_2); //Id component in Q1.15 Format  Curr_Output.qI_Component2 = ((qId_1)+(qId_2)); return (Curr_Output); } /******************************************************************************* * Function Name : PID_Regulator * Description : Compute the PI(D) output for a PI(D) regulation. * Input : Pointer to the PID settings (*PID_Flux) Speed in s16 format * Output : s16 * Return : None *******************************************************************************/ s16 PID_Cal(s32 hReference, s32 hPresentFeedback, PID_Struct_t *PID_Struct) { 
    s32 wError, wProportional_Term,wIntegral_Term, houtput_32 ,wtemp , wDifferential_Term; s64 dwAux; wError= (s32)(hReference - hPresentFeedback); // error computation wProportional_Term = PID_Struct->hKp_Gain * wError; // Proportional term computation if (PID_Struct->hKi_Gain == 0)// Integral term computation { 
    PID_Struct->wIntegral = 0; } else { 
    // if(abs(wError) < PID_Struct->wErrorIntegralLimit) /*error integral limit*/ wIntegral_Term = PID_Struct->hKi_Gain * wError; // else // wIntegral_Term = 0;  dwAux = PID_Struct->wIntegral + (s64)(wIntegral_Term); iout = dwAux; if (dwAux > PID_Struct->wUpper_Limit_Integral) { 
    PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral; } else if (dwAux < PID_Struct->wLower_Limit_Integral) { 
    PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral; } else { 
    PID_Struct->wIntegral = (s32)(dwAux); } } wtemp = wError - PID_Struct->wPreviousError; //取得上次和这个的误差之差 wDifferential_Term = PID_Struct->hKd_Gain * wtemp; //wD = Kd * delta_d PID_Struct->wPreviousError = wError; // 更新上次误差  houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor+ //输出总的调节量 = 比例调节量/分数因子 + PID_Struct->wIntegral/PID_Struct->hKi_Divisor + // + 积分调节量/分数因子 wDifferential_Term/PID_Struct->hKd_Divisor); // + 微分调节量/分数因子 // houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor + PID_Struct->wIntegral/PID_Struct->hKi_Divisor);  // piout = houtput_32; //for debug // houtput_32 = (s16)houtput_32; if (houtput_32 >= PID_Struct->hUpper_Limit_Output) { 
    return(PID_Struct->hUpper_Limit_Output); } else if (houtput_32 < PID_Struct->hLower_Limit_Output) { 
    return(PID_Struct->hLower_Limit_Output); } else { 
    return((s16)(houtput_32)); } } /******************************************************************************* * Function Name : RevPark_Circle_Limitation * Description : Check if * Stat_Volt_q_d.qV_Component1^2 + Stat_Volt_q_d.qV_Component2^2 <= 32767^2 * Apply limitation if previous condition is not met, * by keeping a constant ratio * Stat_Volt_q_d.qV_Component1/Stat_Volt_q_d.qV_Component2 * Input : None * Output : None * Return : None *******************************************************************************/ void RevPark_Circle_Limitation(void) { 
    s32 temp; temp = Stat_Volt_q_d.qV_Component1 * Stat_Volt_q_d.qV_Component1 + Stat_Volt_q_d.qV_Component2 * Stat_Volt_q_d.qV_Component2; // min value 0, max value 2*32767*32767 if(temp > (u32)(( MAX_MODULE * MAX_MODULE) ) ) // (Vd^2+Vq^2) > MAX_MODULE^2 ? { 
    u16 index; temp /= (u32)(512*32768); // min value START_INDEX, max value 127 temp -= START_INDEX ; // min value 0, max value 127 - START_INDEX index = circle_limit_table[(u8)temp]; temp = (s16)Stat_Volt_q_d.qV_Component1 * (u16)(index); Stat_Volt_q_d.qV_Component1 = (s16)(temp/32768); temp = (s16)Stat_Volt_q_d.qV_Component2 * (u16)(index); Stat_Volt_q_d.qV_Component2 = (s16)(temp/32768); } } /******************************************************************************* * Function Name : Rev_Park Transformation * Description : This function transforms stator voltage qVq and qVd, that * belong to a rotor flux synchronous rotating frame, to a * stationary reference frame, so as to obtain qValpha and qVbeta * qValfa=qVq*Cos(theta)+qVd*Sin(theta) * qVbeta=-qVq*Sin(theta)+qVd*Cos(theta) * Input : Stat_Volt_q_d. * Output : Stat_Volt_a_b * Return : none. *******************************************************************************/ Volt_Components Rev_Park(Volt_Components Volt_Input) { 
    s32 qValpha_tmp1,qValpha_tmp2,qVbeta_tmp1,qVbeta_tmp2; s16 qValpha_1,qValpha_2,qVbeta_1,qVbeta_2; Volt_Components Volt_Output; //No overflow guaranteed qValpha_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hCos; //Vector_Components.hCos qValpha_tmp1 /= 32768; qValpha_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hSin; qValpha_tmp2 /= 32768; qValpha_1 = (s16)(qValpha_tmp1); qValpha_2 = (s16)(qValpha_tmp2); Volt_Output.qV_Component1 = (qValpha_1)+(qValpha_2); qVbeta_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hSin; qVbeta_tmp1 /= 32768; qVbeta_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hCos; qVbeta_tmp2 /= 32768; qVbeta_1 = (s16)(qVbeta_tmp1); qVbeta_2 = (s16)(qVbeta_tmp2); Volt_Output.qV_Component2 = -(qVbeta_1)+(qVbeta_2); return(Volt_Output); } /******************************************************************************* * Function Name : SVPWM_3ShuntCalcDutyCycles * Description : Computes duty cycle values corresponding to the input value and configures the AD converter and TIM1 for next period current reading conversion synchronization * Input : Stat_Volt_alfa_beta * Output : None * Return : None *******************************************************************************/ void SVPWM_3ShuntCalcDutyCycles_Function (Volt_Components Stat_Volt_Input) { 
    s32 wX, wY, wZ, wUAlpha, wUBeta; u16 hTimePhA=0, hTimePhB=0, hTimePhC=0, hTimePhD=0, hDeltaDuty=0; wUAlpha = Stat_Volt_Input.qV_Component1 * T_SQRT3 ; wUBeta = -(Stat_Volt_Input.qV_Component2 * T); wX = wUBeta; wY = (wUBeta + wUAlpha)/2; wZ = (wUBeta - wUAlpha)/2; if (wY<0) { 
    if (wZ<0) { 
    SECTOR_NUMBER = SECTOR_5; } else if (wX<=0)// wZ >= 0 { 
    SECTOR_NUMBER = SECTOR_4; } else // wX > 0 { 
    SECTOR_NUMBER = SECTOR_3; } } else // wY > 0 { 
    if (wZ>=0) { 
    SECTOR_NUMBER = SECTOR_2; } else if (wX<=0)// wZ < 0 { 
    SECTOR_NUMBER = SECTOR_6; } else // wX > 0 { 
    SECTOR_NUMBER = SECTOR_1; } } switch(SECTOR_NUMBER) { 
    case SECTOR_1: hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/); /* = 0x20000 ??**/ hTimePhB = hTimePhA + wZ/; hTimePhC = hTimePhB - wX/; break; case SECTOR_2: hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/); hTimePhB = hTimePhA + wZ/; hTimePhC = hTimePhA - wY/; break; case SECTOR_3: hTimePhA = (T/8) + ((((T - wX) + wY)/2)/); hTimePhC = hTimePhA - wY/; hTimePhB = hTimePhC + wX/; break; case SECTOR_4: hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/); hTimePhB = hTimePhA + wZ/; hTimePhC = hTimePhB - wX/; break; case SECTOR_5: hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/); hTimePhB = hTimePhA + wZ/; hTimePhC = hTimePhA - wY/; break; case SECTOR_6: hTimePhA = (T/8) + ((((T - wX) + wY)/2)/); hTimePhC = hTimePhA - wY/; hTimePhB = hTimePhC + wX/; break; default: break; } /* Load compare registers values */ TIM1->CCR1 = hTimePhA; TIM1->CCR2 = hTimePhB; TIM1->CCR3 = hTimePhC; // TIM1->CCR4 = hTimePhD; // To Syncronyze the ADC } /******************************************************************************* * Function Name : FOC_Function: * Description : FOC algorithm, current ADC, Clark, Park,PI, RevPark_Limitation, RevPark, SVPWM *******************************************************************************/ void FOC_Function(void) { 
    // hTorque_Reference = -500; Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues(); Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b); Stat_Curr_q_d = Park(Stat_Curr_alfa_beta,GET_ELECTRICAL_ANGLE); //GET_ELECTRICAL_ANGLE Stat_Volt_q_d.qV_Component1 = PID_Cal(hTorque_Reference,Stat_Curr_q_d.qI_Component1,&PID_Torque_Init);//hTorque_Reference Stat_Volt_q_d.qV_Component2 = PID_Cal(hFlux_Reference,Stat_Curr_q_d.qI_Component2,&PID_Flux_Init);//Stat_Curr_q_d_ref_ref.qI_Component2  RevPark_Circle_Limitation(); Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d); SVPWM_3ShuntCalcDutyCycles_Function(Stat_Volt_alfa_beta); } /******************************************************************************* * Function Name : PID_Init * Description : Initialize PID coefficients for torque, flux ,speed and loop: read PI data from EEPROM Kp_Gain: proportional coeffcient Ki_Gain: integral coeffcient Kd_Gain: differential coeffcient * Input : Pointer 1 to Torque PI structure, Pointer 2 to Flux PI structure, Pointer 3 to Speed PI structure Pointer 4 to Location PI structure * Output : None * Return : None *******************************************************************************/ void PID_Init_Function (PID_Struct_t *PID_Torque, PID_Struct_t *PID_Flux, PID_Struct_t *PID_Speed, PID_Struct_t *PID_Location) { 
    int32_t temp=0; /************END PID Torque Regulator members*******/ hTorque_Reference = PID_TORQUE_REFERENCE; // temp = I2C_One_Parameter_Read(PID_TORQUE_KP_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Torque->hKp_Gain = temp; // else PID_Torque->hKp_Gain = PID_TORQUE_KP_DEFAULT; osDelay(5); PID_Torque->hKp_Divisor = TF_KPDIV; // temp = I2C_One_Parameter_Read(PID_TORQUE_KI_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Torque->hKi_Gain = temp; // else PID_Torque->hKi_Gain = PID_TORQUE_KI_DEFAULT; osDelay(5); PID_Torque->hKi_Divisor = TF_KIDIV; // temp = I2C_One_Parameter_Read(PID_TORQUE_KD_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Torque->hKd_Gain = temp; // else PID_Torque->hKd_Gain = PID_TORQUE_KD_DEFAULT; osDelay(5); PID_Torque->wPreviousError = 0; PID_Torque->hLower_Limit_Output = S16_MIN; //Lower Limit for Output limitation PID_Torque->hUpper_Limit_Output = S16_MAX; //Upper Limit for Output limitation PID_Torque->wLower_Limit_Integral = S16_MIN * TF_KIDIV; PID_Torque->wUpper_Limit_Integral = S16_MAX * TF_KIDIV; PID_Torque->wIntegral = 0; PID_Torque->wErrorIntegralLimit = PID_TORQUE_ERROR_LIMIT; /************PID Flux Regulator members*************/ hFlux_Reference = PID_FLUX_REFERENCE; // temp = I2C_One_Parameter_Read(PID_FLUX_KP_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Flux->hKp_Gain = temp; // else PID_Flux->hKp_Gain = PID_FLUX_KP_DEFAULT; osDelay(5); PID_Flux->hKp_Divisor = TF_KPDIV; // temp = I2C_One_Parameter_Read(PID_FLUX_KI_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Flux->hKi_Gain = temp; // else PID_Flux->hKi_Gain = PID_FLUX_KI_DEFAULT; osDelay(5); PID_Flux->hKi_Divisor = TF_KIDIV; // temp = I2C_One_Parameter_Read(PID_FLUX_KD_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Flux->hKd_Gain = temp; // else PID_Flux->hKd_Gain = PID_FLUX_KD_DEFAULT; osDelay(5); PID_Flux->hKd_Divisor = TF_KDDIV; PID_Flux->wPreviousError = 0; PID_Flux->hLower_Limit_Output = S16_MIN; //Lower Limit for Output limitation PID_Flux->hUpper_Limit_Output = S16_MAX; //Upper Limit for Output limitation PID_Flux->wLower_Limit_Integral = S16_MIN * TF_KIDIV; PID_Flux->wUpper_Limit_Integral = S16_MAX * TF_KIDIV; PID_Flux->wIntegral = 0; PID_Flux->wErrorIntegralLimit = PID_FLUX_ERROR_LIMIT; /************PID Speed Regulator members*************/ // hSpeed_Reference = 100; // temp = I2C_One_Parameter_Read(PID_SPEED_KP_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Speed->hKp_Gain = temp; // else PID_Speed->hKp_Gain = PID_SPEED_KP_DEFAULT; osDelay(5); PID_Speed->hKp_Divisor = SP_KPDIV; // temp = I2C_One_Parameter_Read(PID_SPEED_KI_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Speed->hKi_Gain = temp; // else PID_Speed->hKi_Gain = PID_SPEED_KI_DEFAULT; osDelay(5); PID_Speed->hKi_Divisor = SP_KIDIV; // temp = I2C_One_Parameter_Read(PID_SPEED_KD_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Speed->hKd_Gain = temp; // else PID_Speed->hKd_Gain = PID_SPEED_KD_DEFAULT; osDelay(5); PID_Speed->hKd_Divisor = SP_KDDIV; PID_Speed->wPreviousError = 0; PID_Speed->hLower_Limit_Output= -IQMAX; //Lower Limit for Output limitation PID_Speed->hUpper_Limit_Output= IQMAX; //Upper Limit for Output limitation PID_Speed->wLower_Limit_Integral = -IQMAX * SP_KIDIV; PID_Speed->wUpper_Limit_Integral = IQMAX * SP_KIDIV; PID_Speed->wIntegral = 0; PID_Speed->wErrorIntegralLimit = PID_SPEED_ERROR_LIMIT; /*******************PID Location Regulator members******************/ // temp = I2C_One_Parameter_Read(PID_LOCATION_KP_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Location->hKp_Gain = temp; // else PID_Location->hKp_Gain = PID_LOCATION_KP_DEFAULT; osDelay(5); PID_Location->hKp_Divisor = LO_KPDIV; // temp = I2C_One_Parameter_Read(PID_LOCATION_KI_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Location->hKi_Gain = temp; // else PID_Location->hKi_Gain = PID_LOCATION_KI_DEFAULT; osDelay(5); PID_Location->hKi_Divisor = LO_KIDIV; // temp = I2C_One_Parameter_Read(PID_LOCATION_KD_ID<<2);  // if(temp != 0xFFFFFFFF) // PID_Location->hKd_Gain = temp; // else PID_Location->hKd_Gain = PID_LOCATION_KD_DEFAULT; osDelay(5); PID_Location->hKd_Divisor = LO_KDDIV; PID_Location->wPreviousError = 0; PID_Location->hLower_Limit_Output= S16_SPEED_MIN; //Lower Limit for Output limitation PID_Location->hUpper_Limit_Output= S16_SPEED_MAX; //Upper Limit for Output limitation PID_Location->wLower_Limit_Integral = S16_MIN * LO_KIDIV; PID_Location->wUpper_Limit_Integral = S16_MAX * LO_KIDIV; PID_Location->wIntegral = 0; PID_Location->wErrorIntegralLimit = PID_LOCATION_ERROR_LIMIT; } /******************************************************************************* * Function Name : ENC_Start_Up: * Description : When STATE is START, to execute finding zero electrical angle progress. After zero electrical angle was found, set motor a constant speed, of which is moving to origin loction. * Input : None * Output : None * Return : None *******************************************************************************/ void ENC_Start_Up(void) { 
    if(Motor_Init_Flag == 0) { 
    wTimebase++; if(wTimebase <= T_ALIGNMENT_PWM_STEPS/4) //T_ALIGNMENT_PWM_STEPSΪ(T_ALIGNMENT * SAMPLING_FREQ)/1000 { 
    hFlux_Reference = I_ALIGNMENT/4+3*I_ALIGNMENT * wTimebase / T_ALIGNMENT_PWM_STEPS; hTorque_Reference = 0; Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues(); Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b); Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, ALIGNMENT_ANGLE_S16); Stat_Volt_q_d.qV_Component1 = PID_Cal(hTorque_Reference, Stat_Curr_q_d.qI_Component1, &PID_Torque_Init); Stat_Volt_q_d.qV_Component2 = PID_Cal(hFlux_Reference, Stat_Curr_q_d.qI_Component2, &PID_Flux_Init); RevPark_Circle_Limitation(); Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d); SVPWM_3ShuntCalcDutyCycles_Function(Stat_Volt_alfa_beta); } else if(wTimebase <= T_ALIGNMENT_PWM_STEPS*3/4) //T_ALIGNMENT_PWM_STEPSΪ(T_ALIGNMENT * SAMPLING_FREQ)/1000 { 
    hFlux_Reference = I_ALIGNMENT; hTorque_Reference = 0; Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues(); Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b); Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, ALIGNMENT_ANGLE_S16); Stat_Volt_q_d.qV_Component1 = PID_Cal(hTorque_Reference,Stat_Curr_q_d.qI_Component1, &PID_Torque_Init); Stat_Volt_q_d.qV_Component2 = PID_Cal(hFlux_Reference,Stat_Curr_q_d.qI_Component2, &PID_Flux_Init); RevPark_Circle_Limitation(); Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d); SVPWM_3ShuntCalcDutyCycles_Function(Stat_Volt_alfa_beta); } else if(wTimebase <= T_ALIGNMENT_PWM_STEPS) //T_ALIGNMENT_PWM_STEPSΪ(T_ALIGNMENT * SAMPLING_FREQ)/1000 { 
    hFlux_Reference = I_ALIGNMENT/2; hTorque_Reference = 0; Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues(); Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b); Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, ALIGNMENT_ANGLE_S16); Stat_Volt_q_d.qV_Component1 = PID_Cal(hTorque_Reference,Stat_Curr_q_d.qI_Component1, &PID_Torque_Init); Stat_Volt_q_d.qV_Component2 = PID_Cal(hFlux_Reference,Stat_Curr_q_d.qI_Component2, &PID_Flux_Init); RevPark_Circle_Limitation(); Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d); SVPWM_3ShuntCalcDutyCycles_Function(Stat_Volt_alfa_beta); } else { 
    Motor_Init_Flag = 1; HAL_TIM_Base_Start_IT(&htim5);//弿启TIM5，并使能TIM5中断 // HAL_TIM_Base_Start_IT(&htim3); //使用定时器需调用这个函数启动定时器，并开始定时器中断,使能TIM3中断  wTimebase = 0; TIM3->CNT = 0 ; Encoder_Timer_Overflow = 0; hFlux_Reference = 0; EXTI3_IRQ_FLAG = 1; __HAL_GPIO_EXTI_CLEAR_FLAG(GPIO_PIN_3); __HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_3); // hSpeed_Reference = -10; for (uint16_t i=0; i<SPEED_BUFFER_SIZE; i++) { 
    speed_Buffer[i] = 0; } // STATE = RUN; } } else if(Motor_Init_Flag == 1) { 
    FOC_Function(); } } 

代码解释：

在永磁同步电机和直流无刷电机的控制中，要得到高精度的控制效果，就会使用FOC算法控制，而FOC算法一般我们都是在中断中去完成，而ST的官方MC SDK生成的也是在中断中完成。而其中ADC的采样，PWM的完成都需要特别注意，下面的几个流程图对FOC的理解很有帮助