第27章 FOC（7-8）

为了进一步提升无刷电机的可控性，引入了FOC控制方式。本章主要以PMSM为例带领大家一起学习FOC的控制方式。

本章分为如下几个小节：

27.7 FOC例程分析

27.8 FOC位置闭环控制

27.7 FOC例程分析

以foc_pmsm_m1_encoder_os为例，即电机接口1使用编码器反馈速度、位置在FreeRTOS下实现FOC。

FOC实现在系统下一共创建了两个任务，startMediumFrequencyTask和StartSafetyTask。

 /* Create the thread(s) */
  /* definition and creation of mediumFrequency */
  osThreadDef(mediumFrequency, startMediumFrequencyTask, osPriorityNormal, 0, 
  128);
  mediumFrequencyHandle = osThreadCreate(osThread(mediumFrequency), NULL);
 
  /* definition and creation of safety */
  osThreadDef(safety, StartSafetyTask, osPriorityAboveNormal, 0, 128);
  safetyHandle = osThreadCreate(osThread(safety), NULL);

可能第一步跳转的是main函数的函数实现，这也是由于编译器的查找优先顺序决定的，不过main函数中的函数实现是弱定义的，真正的实现在mc_task.c中。

/* startMediumFrequencyTask function */
void startMediumFrequencyTask(void const * argument)
{
  /* USER CODE BEGIN MF task 1 */
  /* Infinite loop */
  for(;;)
  {
    /* delay of 500us */
    vTaskDelay(1);
    MC_RunMotorControlTasks();
  }
  /* USER CODE END MF task 1 */
}

中频任务startMediumFrequencyTask的内容为系统演示调用MC_RunMotorControlTasks函数。而MC_RunMotorControlTasks函数负责循环调度一些任务，主要是电机控制调度和用户接口调度。

__weak void MC_RunMotorControlTasks(void)
{
  if ( bMCBootCompleted ) {
    /* ** Medium Frequency Tasks ** */
    MC_Scheduler();
 
    /* ** User Interface Task ** */
    UI_Scheduler();
  }
}

其中MC_Scheduler主要由一个变量计数器来实现真正的任务调度，真正负责电机控制调度的实现是TSK_MediumFrequencyTaskM1函数。

__weak void TSK_MediumFrequencyTaskM1(void)
{
  /* USER CODE BEGIN MediumFrequencyTask M1 0 */
 
  /* USER CODE END MediumFrequencyTask M1 0 */
 
  State_t StateM1;
  int16_t wAux = 0;
 
  (void) ENC_CalcAvrgMecSpeedUnit( &ENCODER_M1, &wAux );
  PQD_CalcElMotorPower( pMPM[M1] );
 
  StateM1 = STM_GetState( &STM[M1] );
 
  switch ( StateM1 )
  {
 
  case IDLE:
    if ( EAC_GetRestartState( &EncAlignCtrlM1 ) )
    {
      /* The Encoder Restart State is true: the IDLE state has been entered
       * after Encoder alignment was performed.The motor can now be started */
      EAC_SetRestartState( &EncAlignCtrlM1,false );
 
      /* USER CODE BEGIN MediumFrequencyTask M1 Encoder Restart */
 
      /* USER CODE END MediumFrequencyTask M1 Encoder Restart */
 
      STM_NextState( &STM[M1], IDLE_START );
    }
    break;
 
  case IDLE_START:
…… ……

内容过多这里只截取了部分函数。中频任务函数主要由状态机实现，通过不同的状态来切换控制内容，例如硬件刚刚初始化的时候就是ICLWAIT状态，硬件初始化OK可以运行的时候就是IDLE状态，等等。各状态的定义和注释如下：

typedef enum
{
  ICLWAIT = 12,         /*!< Persistent state, the system is waiting for ICL
                           deactivation. Is not possible to run the motor if
                           ICL is active. Until the ICL is active the state is
                           forced to ICLWAIT, when ICL become inactive the state
                           is moved to IDLE */
  IDLE = 0,             /*!< Persistent state, following state can be IDLE_START
                           if a start motor command has been given or
                           IDLE_ALIGNMENT if a start alignment command has been
                           given */
  IDLE_ALIGNMENT = 1,   /*!< "Pass-through" state containg the code to be
executed
                           only once after encoder alignment command.
                           Next states can be ALIGN_CHARGE_BOOT_CAP or
                           ALIGN_OFFSET_CALIB according the configuration. It
                           can also be ANY_STOP if a stop motor command has been
                           given. */
  ALIGN_CHARGE_BOOT_CAP = 13,/*!< Persistent state where the gate driver boot
                           capacitors will be charged. Next states will be
                           ALIGN_OFFSET_CALIB. It can also be ANY_STOP if a stop
                           motor command has been given. */
  ALIGN_OFFSET_CALIB = 14,/*!< Persistent state where the offset of motor
currents
                           measurements will be calibrated. Next state will be
                           ALIGN_CLEAR. It can also be ANY_STOP if a stop motor
                           command has been given. */
  ALIGN_CLEAR = 15,     /*!< "Pass-through" state in which object is cleared
and
                           set for the startup.
                           Next state will be ALIGNMENT. It can also be ANY_STOP
                           if a stop motor command has been given. */
  … …
} State_t;

状态切换由STM_NextState函数实现。StartSafetyTask函数主要做电机控制过程的保护作用。

硬件初始化程序在main函数中，如下：

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_DAC_Init();
  MX_TIM1_Init();
  MX_TIM3_Init();
  MX_USART1_UART_Init();
  MX_MotorControl_Init();
 
  /* Initialize interrupts */
  MX_NVIC_Init();

以电机控制使用的定时器1为例，初始化硬件程序如下：

void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(htim->Instance==TIM1)
  {
  /* USER CODE BEGIN TIM1_MspPostInit 0 */
 
  /* USER CODE END TIM1_MspPostInit 0 */
 
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    /**TIM1 GPIO Configuration
    PB13     ------> TIM1_CH1N
    PB14     ------> TIM1_CH2N
    PB15     ------> TIM1_CH3N
    PA8     ------> TIM1_CH1
    PA9     ------> TIM1_CH2
    PA10     ------> TIM1_CH3
    */
    GPIO_InitStruct.Pin = M1_PWM_UL_Pin|M1_PWM_VL_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLDOWN;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF6_TIM1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
    GPIO_InitStruct.Pin = M1_PWM_WL_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLDOWN;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF4_TIM1;
    HAL_GPIO_Init(M1_PWM_WL_GPIO_Port, &GPIO_InitStruct);
 
    GPIO_InitStruct.Pin = M1_PWM_UH_Pin|M1_PWM_VH_Pin|M1_PWM_WH_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLDOWN;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF6_TIM1;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
  /* USER CODE BEGIN TIM1_MspPostInit 1 */
 
  /* USER CODE END TIM1_MspPostInit 1 */
  }
 
}

管脚定义在main.h头文件中，通过宏的方式可以较为方便的改动硬件。

#define SHOUDOWN_Pin GPIO_PIN_13
#define SHOUDOWN_GPIO_Port GPIOC
#define M1_TEMPERATURE_Pin GPIO_PIN_0
#define M1_TEMPERATURE_GPIO_Port GPIOC
#define M1_CURR_AMPL_U_Pin GPIO_PIN_1
#define M1_CURR_AMPL_U_GPIO_Port GPIOC
#define M1_CURR_AMPL_V_Pin GPIO_PIN_2
#define M1_CURR_AMPL_V_GPIO_Port GPIOC
#define M1_CURR_AMPL_W_Pin GPIO_PIN_3
#define M1_CURR_AMPL_W_GPIO_Port GPIOC
#define M1_BUS_VOLTAGE_Pin GPIO_PIN_3
#define M1_BUS_VOLTAGE_GPIO_Port GPIOA
#define DBG_DAC_CH1_Pin GPIO_PIN_4
#define DBG_DAC_CH1_GPIO_Port GPIOA
#define DBG_DAC_CH2_Pin GPIO_PIN_5
#define DBG_DAC_CH2_GPIO_Port GPIOA
#define M1_ENCODER_A_Pin GPIO_PIN_6
#define M1_ENCODER_A_GPIO_Port GPIOA
#define M1_ENCODER_B_Pin GPIO_PIN_7
#define M1_ENCODER_B_GPIO_Port GPIOA
#define Start_Stop_Pin GPIO_PIN_14
#define Start_Stop_GPIO_Port GPIOE
#define Start_Stop_EXTI_IRQn EXTI15_10_IRQn
#define M1_PWM_UL_Pin GPIO_PIN_13
#define M1_PWM_UL_GPIO_Port GPIOB
#define M1_PWM_VL_Pin GPIO_PIN_14
#define M1_PWM_VL_GPIO_Port GPIOB
#define M1_PWM_WL_Pin GPIO_PIN_15
#define M1_PWM_WL_GPIO_Port GPIOB
#define M1_PWM_UH_Pin GPIO_PIN_8
#define M1_PWM_UH_GPIO_Port GPIOA
#define M1_PWM_VH_Pin GPIO_PIN_9
#define M1_PWM_VH_GPIO_Port GPIOA
#define M1_PWM_WH_Pin GPIO_PIN_10
#define M1_PWM_WH_GPIO_Port GPIOA
#define M1_OCP_Pin GPIO_PIN_11
#define M1_OCP_GPIO_Port GPIOA
#define TMS_Pin GPIO_PIN_13
#define TMS_GPIO_Port GPIOA
#define TCK_Pin GPIO_PIN_14
#define TCK_GPIO_Port GPIOA
#define UART_TX_Pin GPIO_PIN_6
#define UART_TX_GPIO_Port GPIOB
#define UART_RX_Pin GPIO_PIN_7
#define UART_RX_GPIO_Port GPIOB

为了便于平台生成程序，电机控制所相关的中断全部通过stm32G4xx_mc_it.c实现，函数声明如下：

/* Public prototypes of IRQ handlers called from assembly code ----------*/
void ADC1_2_IRQHandler(void);
void TIMx_UP_M1_IRQHandler(void);
void TIMx_BRK_M1_IRQHandler(void);
void SPD_TIM_M1_IRQHandler(void);
void USART_IRQHandler(void);
void HardFault_Handler(void);
void SysTick_Handler(void);
void EXTI15_10_IRQHandler (void);

以前版本的FOC程序支持显示屏，之后和现在的版本均不支持显示屏控制了，不过还支持按键控制启停的功能。以开发板的按键2(PE14)作为启停开关，启停通过外部中断的方式实现，程序如下：

/**
  * @brief  This function handles Button IRQ on PIN PE14.
  */
void EXTI15_10_IRQHandler (void)
{
 
    /* USER CODE BEGIN START_STOP_BTN */
  if ( LL_EXTI_ReadFlag_0_31(LL_EXTI_LINE_14) )
  {
    LL_EXTI_ClearFlag_0_31 (LL_EXTI_LINE_14);
    UI_HandleStartStopButton_cb ();
  }
}

中断程序首先清除中断标志位，然后调用函数UI_HandleStartStopButton_cb，即用户接口启停按钮回调，程序通过判断电机状态决定是启动电机还是停止电机，函数实现如下：

__weak void UI_HandleStartStopButton_cb (void)
{
/* USER CODE BEGIN START_STOP_BTN */
  if (MC_GetSTMStateMotor1() == IDLE)
  {
    /* Ramp parameters should be tuned for the actual motor */
    MC_StartMotor1();
  }
  else
  {
    MC_StopMotor1();
  }

同UI_HandleStartStopButton_cb一样，很多函数很多情况下都是弱定义的也就是可以由用户覆写以实现更灵活的功能。

串口中断函数主要用于与上位机的通信，在接收到数据的时候通过UFCP_RX_IRQ_Handler函数来将有效数据放置到缓存中。接收到完整的数据帧后通过回调函数MCP_ReceivedFrame来解析数据帧并实现交互。接收数据帧解析赋值函数通过MCP_Init函数来初始化，如下：

__weak void MCP_Init( MCP_Handle_t *pHandle,
               FCP_Handle_t * pFCP,
               FCP_SendFct_t fFcpSend,
               FCP_ReceiveFct_t fFcpReceive,
               FCP_AbortReceiveFct_t fFcpAbortReceive,
               DAC_UI_Handle_t * pDAC,
               const char* s_fwVer )
{
  pHandle->pFCP = pFCP;
  pHandle->pDAC = pDAC;
  pHandle->s_fwVer = s_fwVer;
  FCP_SetClient( pFCP, pHandle,
                 (FCP_SentFrameCallback_t) & MCP_SentFrame,
                 (FCP_ReceivedFrameCallback_t) & MCP_ReceivedFrame,
                 (FCP_RxTimeoutCallback_t) & MCP_OnTimeOut );
  pHandle->fFcpSend = fFcpSend;
  pHandle->fFcpReceive = fFcpReceive;
  pHandle->fFcpAbortReceive = fFcpAbortReceive;
 
  MCP_WaitNextFrame(pHandle);
}

由于例程使用了编码器用于反馈速度和位置，所以有SPD_TIM_M1_IRQHandler函数，也就是TIM3_IRQHandler，用于更新编码器计数溢出次数。

/**********  AUXILIARY ENCODER TIMER MOTOR 1 *************/
#define M1_PULSE_NBR ( (4 * (M1_ENCODER_PPR)) - 1 )
#define M1_ENC_IC_FILTER  9
#define SPD_TIM_M1_IRQHandler TIM3_IRQHandler

TIMx_BRK_M1_IRQHandler中断函数用于刹车，也就是TIM1_BRK_TIM9_IRQHandler，刹车信号有效时调用R3_2_BRK_IRQHandler函数，关断输出信号并设置过流标志。

/**
 * @brief  It contains the Break event interrupt
 * @param  pHandle: handler of the current instance of the PWM component
 * @retval none
 */
__weak void *R3_2_BRK_IRQHandler(PWMC_R3_2_Handle_t *pHandle)
{
 
  if ((pHandle->pParams_str->LowSideOutputs)== ES_GPIO)
  {
     LL_GPIO_ResetOutputPin(pHandle->pParams_str->pwm_en_u_port, 
pHandle->pParams_str->pwm_en_u_pin);
     LL_GPIO_ResetOutputPin(pHandle->pParams_str->pwm_en_v_port, 
pHandle->pParams_str->pwm_en_v_pin);
     LL_GPIO_ResetOutputPin(pHandle->pParams_str->pwm_en_w_port, 
pHandle->pParams_str->pwm_en_w_pin);
  }
  pHandle->OverCurrentFlag = true;
 
  return &(pHandle->_Super.Motor);
}

TIMx_UP_M1_IRQHandler函数用于在定时器更新中断时使用ADC注入通道获取采样电流值用于FOC计算的电流反馈。也就是TIM1_UP_TIM10_IRQHandler。

/*************************  IRQ Handler Mapping  *********************/
#define TIMx_UP_M1_IRQHandler TIM1_UP_TIM10_IRQHandler
#define DMAx_R1_M1_IRQHandler DMA2_Stream4_IRQHandler
#define DMAx_R1_M1_Stream     DMA2_Stream4
#define TIMx_BRK_M1_IRQHandler TIM1_BRK_TIM9_IRQHandler

TIMx_UP_M1_IRQHandler函数首先清除更新中断，然后运行R3_2_TIMx_UP_IRQHandler回调函数，函数实现如下：

/**
  * @brief  It contains the TIMx Update event interrupt
  * @param  pHandle: handler of the current instance of the PWM component
  * @retval none
  */
__weak void * R3_2_TIMx_UP_IRQHandler( PWMC_R3_2_Handle_t * pHandle)
{
  TIM_TypeDef* TIMx = pHandle->pParams_str->TIMx;
  ADC_TypeDef * ADCx_1 = pHandle->pParams_str->ADCx_1;
  ADC_TypeDef * ADCx_2 = pHandle->pParams_str->ADCx_2;
  uint32_t ADCInjFlags;
  
  /* dual drive check */
  ADCInjFlags = ADCx_1->SR & (LL_ADC_FLAG_JSTRT|LL_ADC_FLAG_JEOS);
  if ( ADCInjFlags == LL_ADC_FLAG_JSTRT )
  {
    /* ADC conversion is on going on the second motor */
    do
    {
      /* wait for end of conversion */
      ADCInjFlags = ADCx_1->SR & (LL_ADC_FLAG_JSTRT|LL_ADC_FLAG_JEOS);
    } 
    while ( ADCInjFlags != (LL_ADC_FLAG_JSTRT|LL_ADC_FLAG_JEOS) );
  }
  else if ( ADCInjFlags == 0 )
  {
    /* ADC conversion on the second motor is not yet started */
    while ( ( TIMx->CNT ) < ( pHandle->pParams_str->Tw ) )
    {
      /* wait for a maximum delay */
    }
    
    ADCInjFlags = ADCx_1->SR & (LL_ADC_FLAG_JSTRT|LL_ADC_FLAG_JEOS);
    if ( ADCInjFlags == LL_ADC_FLAG_JSTRT )
    {
      /* ADC conversion is on going on the second motor */
      do
      {
        /* wait for end of conversion */
        ADCInjFlags = ADCx_1->SR & (LL_ADC_FLAG_JSTRT|LL_ADC_FLAG_JEOS);
      }
      while ( ADCInjFlags != (LL_ADC_FLAG_JSTRT|LL_ADC_FLAG_JEOS) );
    }
  }
  else
  {
    /* ADC conversion on the second motor is done */
  }
  
  /* Disabling trigger to avoid unwanted conversion */
  LL_ADC_INJ_StopConversionExtTrig(ADCx_1);
  LL_ADC_INJ_StopConversionExtTrig(ADCx_2);
 
  /* Set next current channel according to sector  */
  ADCx_1->JSQR = pHandle->pParams_str->ADCConfig1[pHandle->_Super.Sector];
  ADCx_2->JSQR = pHandle->pParams_str->ADCConfig2[pHandle->_Super.Sector];
 
  LL_ADC_INJ_SetTriggerSource(ADCx_1,pHandle->ADC_ExternalTriggerInjected);
  LL_ADC_INJ_SetTriggerSource(ADCx_2,pHandle->ADC_ExternalTriggerInjected);
    
  /* enable ADC trigger source */
  LL_TIM_CC_EnableChannel(TIMx, LL_TIM_CHANNEL_CH4);
  LL_ADC_INJ_StartConversionExtTrig(ADCx_1, pHandle->ADCTriggerEdge);
  LL_ADC_INJ_StartConversionExtTrig(ADCx_2, pHandle->ADCTriggerEdge);
 
  /* reset default edge detection trigger */
  pHandle->ADCTriggerEdge = LL_ADC_INJ_TRIG_EXT_RISING;
 
  return &( pHandle->_Super.Motor );
}

ADC_IRQHandler函数用于输出调试变量，通过UI_DACUpdate实现，通过判断TSK_HighFrequencyTask函数返回值判断执行FOC循环的电机id来决定输出的模拟值以适配不同电机的不同变量。因此中断时也执行一次TSK_HighFrequencyTask，即FOC高频任务，也就是FOC循环的主体。函数实现如下：

__weak uint8_t TSK_HighFrequencyTask(void)
{
  /* USER CODE BEGIN HighFrequencyTask 0 */
 
  /* USER CODE END HighFrequencyTask 0 */
 
  uint8_t bMotorNbr = 0;
  uint16_t hFOCreturn;
 
  ENC_CalcAngle(&ENCODER_M1);   /* if not sensorless then 2nd parameter is MC_NULL*/
 
  /* USER CODE BEGIN HighFrequencyTask SINGLEDRIVE_1 */
 
  /* USER CODE END HighFrequencyTask SINGLEDRIVE_1 */
  hFOCreturn = FOC_CurrControllerM1();
  /* USER CODE BEGIN HighFrequencyTask SINGLEDRIVE_2 */
 
  /* USER CODE END HighFrequencyTask SINGLEDRIVE_2 */
  if(hFOCreturn == MC_FOC_DURATION)
  {
    STM_FaultProcessing(&STM[M1], MC_FOC_DURATION, 0);
  }
  else
  {
    /* USER CODE BEGIN HighFrequencyTask SINGLEDRIVE_3 */
 
    /* USER CODE END HighFrequencyTask SINGLEDRIVE_3 */
  }
  /* USER CODE BEGIN HighFrequencyTask 1 */
 
  /* USER CODE END HighFrequencyTask 1 */
 
  return bMotorNbr;
}

FOC循环的过程就是和之前介绍的流程一致，通过ENC_CalcAngle获取转子电角度，通过PWMC_GetPhaseCurrents获取a相以及b相电流，然后进行Clarke变换，实现函数：MCM_Clarke；接着进行Park变换，实现函数：MCM_Park；接着就是对d轴以及q轴进行PI运算，运算后的结果在进行反Park变换，实现函数：MCM_Rev_Park；接着就是进行SVPWM的运算，实现函数：PWMC_SetPhaseVoltage，整个过程都在FOC_CurrControllerM1函数中实现，它的功能主要用于控制电流环，函数实现如下：

inline uint16_t FOC_CurrControllerM1(void)
{
  qd_t Iqd, Vqd;
  ab_t Iab;
  alphabeta_t Ialphabeta, Valphabeta;
 
  int16_t hElAngle;
  uint16_t hCodeError;
  SpeednPosFdbk_Handle_t *speedHandle;
 
  speedHandle = STC_GetSpeedSensor(pSTC[M1]);
  hElAngle = SPD_GetElAngle(speedHandle);
  PWMC_GetPhaseCurrents(pwmcHandle[M1], &Iab);
  Ialphabeta = MCM_Clarke(Iab);
  Iqd = MCM_Park(Ialphabeta, hElAngle);
  Vqd.q = PI_Controller(pPIDIq[M1],
            (int32_t)(FOCVars[M1].Iqdref.q) - Iqd.q);
 
  Vqd.d = PI_Controller(pPIDId[M1],
            (int32_t)(FOCVars[M1].Iqdref.d) - Iqd.d);
 
  Vqd = Circle_Limitation(pCLM[M1], Vqd);
  hElAngle += SPD_GetInstElSpeedDpp(speedHandle)*REV_PARK_ANGLE_COMPENSATION_FACTOR;
  Valphabeta = MCM_Rev_Park(Vqd, hElAngle);
  hCodeError = PWMC_SetPhaseVoltage(pwmcHandle[M1], Valphabeta);
  FOCVars[M1].Vqd = Vqd;
  FOCVars[M1].Iab = Iab;
  FOCVars[M1].Ialphabeta = Ialphabeta;
  FOCVars[M1].Iqd = Iqd;
  FOCVars[M1].Valphabeta = Valphabeta;
  FOCVars[M1].hElAngle = hElAngle;
  return(hCodeError);
}

函数PWMC_SetPhaseVoltage计算过后的结果，通过函数pFctSetADCSampPointSectX设置定时器各通道的比较值大小，进而控制电机。然后又通过R3_2_WriteTIMRegisters函数通过检查TIMx CH4是否启用。如果标志置位则发生更新事件因此FOC速率太高，可能过流。如果过流则执行过流处理，即STM_FaultProcessing函数。

__weak uint16_t PWMC_SetPhaseVoltage( PWMC_Handle_t * pHandle, alphabeta_t Valfa_beta )
{
  int32_t wX, wY, wZ, wUAlpha, wUBeta, wTimePhA, wTimePhB, wTimePhC;
 
  wUAlpha = Valfa_beta.alpha * ( int32_t )pHandle->hT_Sqrt3;
  wUBeta = -( Valfa_beta.beta * ( int32_t )( pHandle->PWMperiod ) ) * 2;
 
  wX = wUBeta;
  wY = ( wUBeta + wUAlpha ) / 2;
  wZ = ( wUBeta - wUAlpha ) / 2;
 
  /* Sector calculation from wX, wY, wZ */
  if ( wY < 0 )
  {
    if ( wZ < 0 )
    {
…… ……
    if ( pHandle->Ic > 0 )
    {
      pHandle->CntPhC += pHandle->DTCompCnt;
    }
    else
    {
      pHandle->CntPhC -= pHandle->DTCompCnt;
    }
  }
 
  return ( pHandle->pFctSetADCSampPointSectX( pHandle ) );
}

其中pFctSetADCSampPointSectX赋值如下：

PWMC_R3_2_Handle_t PWM_Handle_M1=
{
  {
    .pFctGetPhaseCurrents              = &R3_2_GetPhaseCurrents,
    .pFctSwitchOffPwm                  = &R3_2_SwitchOffPWM,
    .pFctSwitchOnPwm                   = &R3_2_SwitchOnPWM,
    .pFctCurrReadingCalib              = &R3_2_CurrentReadingCalibration,
    .pFctTurnOnLowSides                = &R3_2_TurnOnLowSides,
    .pFctSetADCSampPointSectX          = &R3_2_SetADCSampPointSectX,
    .pFctIsOverCurrentOccurred         = &R3_2_IsOverCurrentOccurred,
    .pFctOCPSetReferenceVoltage        = MC_NULL,
    .pFctRLDetectionModeEnable         = &R3_2_RLDetectionModeEnable,
    .pFctRLDetectionModeDisable        = &R3_2_RLDetectionModeDisable,
    .pFctRLDetectionModeSetDuty        = &R3_2_RLDetectionModeSetDuty,
    .hT_Sqrt3 = (PWM_PERIOD_CYCLES*SQRT3FACTOR)/16384u,
…… ……
};

R3_2_SetADCSampPointSectX函数定义如下：

__weak uint16_t R3_2_SetADCSampPointSectX( PWMC_Handle_t * pHdl )
{
  PWMC_R3_2_Handle_t * pHandle = ( PWMC_R3_2_Handle_t * )pHdl;
 
  uint16_t hCntSmp;
  uint16_t hDeltaDuty;
  register uint16_t lowDuty = pHdl->lowDuty;
  register uint16_t midDuty = pHdl->midDuty;
 
  /* Check if sampling AB in the middle of PWM is possible */
  if ( ( uint16_t )( pHandle->Half_PWMPeriod - lowDuty ) > pHandle->pParams_str->hTafter )
  {
    pHandle->_Super.Sector = SECTOR_4;
 
    /* set sampling  point trigger in the middle of PWM period */
    hCntSmp = ( uint32_t )( pHandle->Half_PWMPeriod ) - 1u;
  }
  else
  {
… …
  }
 
  return R3_2_WriteTIMRegisters( &pHandle->_Super, hCntSmp );
}

接着看下返回值R3_2_WriteTIMRegisters的函数定义：

__STATIC_INLINE uint16_t R3_2_WriteTIMRegisters( PWMC_Handle_t * pHdl, uint16_t hCCR4Reg )
{
  PWMC_R3_2_Handle_t * pHandle = (PWMC_R3_2_Handle_t *) pHdl;
  TIM_TypeDef * TIMx = pHandle->pParams_str->TIMx;
  uint16_t hAux;
 
  LL_TIM_OC_SetCompareCH1( TIMx, pHandle->_Super.CntPhA );
  LL_TIM_OC_SetCompareCH2( TIMx, pHandle->_Super.CntPhB );
  LL_TIM_OC_SetCompareCH3( TIMx, pHandle->_Super.CntPhC );
  LL_TIM_OC_SetCompareCH4( TIMx, hCCR4Reg );
 
  /* Limit for update event */
  /* Check the if TIMx CH4 is enabled. If it is set, an update event has occurred
  and thus the FOC rate is too high */
  if (LL_TIM_CC_IsEnabledChannel(TIMx, LL_TIM_CHANNEL_CH4))
  {
    hAux = MC_FOC_DURATION;
  }
  else
  {
    hAux = MC_NO_ERROR;
  }
  if ( pHandle->_Super.SWerror == 1u )
  {
    hAux = MC_FOC_DURATION;
    pHandle->_Super.SWerror = 0u;
  }
  return hAux;
}

MC WorkBench上位机目前仅支持速度控制和扭矩控制模式，主要是通过调节电流的方式实现，主要逻辑就是在FOC_CurrControllerM1函数中实现。

inline uint16_t FOC_CurrControllerM1(void)
{
  qd_t Iqd, Vqd;
  ab_t Iab;
  alphabeta_t Ialphabeta, Valphabeta;
 
  int16_t hElAngle;
  uint16_t hCodeError;
  SpeednPosFdbk_Handle_t *speedHandle;
 
  speedHandle = STC_GetSpeedSensor(pSTC[M1]);
  hElAngle = SPD_GetElAngle(speedHandle);
  PWMC_GetPhaseCurrents(pwmcHandle[M1], &Iab);
  Ialphabeta = MCM_Clarke(Iab);
  Iqd = MCM_Park(Ialphabeta, hElAngle);
  Vqd.q = PI_Controller(pPIDIq[M1],
            (int32_t)(FOCVars[M1].Iqdref.q) - Iqd.q);
 
  Vqd.d = PI_Controller(pPIDId[M1],
            (int32_t)(FOCVars[M1].Iqdref.d) - Iqd.d);
 
  Vqd = Circle_Limitation(pCLM[M1], Vqd);
  hElAngle += SPD_GetInstElSpeedDpp(speedHandle)*REV_PARK_ANGLE_COMPENSATION_FACTOR;
  Valphabeta = MCM_Rev_Park(Vqd, hElAngle);
  hCodeError = PWMC_SetPhaseVoltage(pwmcHandle[M1], Valphabeta);
  FOCVars[M1].Vqd = Vqd;
  FOCVars[M1].Iab = Iab;
  FOCVars[M1].Ialphabeta = Ialphabeta;
  FOCVars[M1].Iqd = Iqd;
  FOCVars[M1].Valphabeta = Valphabeta;
  FOCVars[M1].hElAngle = hElAngle;
  return(hCodeError);
}

首先通过STC_GetSpeedSensor函数获取速度传感器句柄，然后通过SPD_GetElAngle函数和速度传感器句柄获取到转子角度hElAngle，然后通过函数PWMC_GetPhaseCurrents获取定子电流到Iab，实际上是通过电流获取函数指针指向的R3_2_GetPhaseCurrents函数实现的，获取电流之后通过MCM_Clarke函数进行Clarke变换到直角坐标系，然后再经过MCM_Park函数进行Park变换得到DQ轴电流Iqd，例程使用速度控制模式，不过速度环经过PID的输出还是要附加到DQ轴电流上，用于最终达到设定速度。PI控制使用PI_Controller函数实现，速度环PI控制器输出DQ轴电流限制使用Circle_Limitation函数实现。然后经过MCM_Rev_Park进行反Park变换得到直角坐标系电流值，最终通过PWMC_SetPhaseVoltage函数把电流通过SVPWM输出到电机。

输出限制函数Circle_Limitation实现如下：

__weak qd_t Circle_Limitation( CircleLimitation_Handle_t * pHandle, qd_t Vqd )
{
  uint16_t table_element;
  uint32_t uw_temp;
  int32_t  sw_temp;
  qd_t local_vqd = Vqd;
 
  sw_temp = ( int32_t )( Vqd.q ) * Vqd.q +
            ( int32_t )( Vqd.d ) * Vqd.d;
 
  uw_temp = ( uint32_t ) sw_temp;
 
  /* uw_temp min value 0, max value 32767*32767 */
  if ( uw_temp > ( uint32_t )( pHandle->MaxModule ) * pHandle->MaxModule )
  {
 
    uw_temp /= ( uint32_t )( 16777216 );
 
    /* wtemp min value pHandle->Start_index, max value 127 */
    uw_temp -= pHandle->Start_index;
 
    /* uw_temp min value 0, max value 127 - pHandle->Start_index */
    table_element = pHandle->Circle_limit_table[( uint8_t )uw_temp];
 
    sw_temp = Vqd.q * ( int32_t )table_element;
    local_vqd.q = ( int16_t )( sw_temp / 32768 );
 
    sw_temp = Vqd.d * ( int32_t )( table_element );
    local_vqd.d = ( int16_t )( sw_temp / 32768 );
  }
 
  return ( local_vqd );
}

程序通过计算DQ轴电流的平方和和最大限制输出占比的平方做比较，判断是否需要进行限制。最大限制输出占比初始赋值如下：

CircleLimitation_Handle_t CircleLimitationM1 =
{
  .MaxModule          = MAX_MODULE,
  .MaxVd              = (uint16_t)(MAX_MODULE * 950 / 1000),
  .Circle_limit_table = MMITABLE,
  .Start_index        = START_INDEX,
};

其最大输出占比宏定义如下。

/* MMI Table Motor 1 MAX_MODULATION_89_PER_CENT */
#define START_INDEX 50
#define MAX_MODULE 29162

程序限制在89%输出，即32767*0.89=29162.63≈29162。计算结果除以65536除以256即16777216将数值限制在0~255，然后减去50定位到限制数组中，结果乘以原始DQ轴电流再除以32768以得到最终限制输出电流值。

PWMC_SetPhaseVoltage用以将直角坐标系电流值赋值到定时器以输出SVPWM，将Valfa_beta乘以hT_Sqrt3转换成三相坐标系wX, wY, wZ。hT_Sqrt3定义为0xDDB4即56756，即/2*65536≈56,755.840862。

接着看下开方快速计算因子赋值：

PWMC_R3_2_Handle_t PWM_Handle_M1=
{
  {
    .pFctGetPhaseCurrents              = &R3_2_GetPhaseCurrents,
    .pFctSwitchOffPwm                  = &R3_2_SwitchOffPWM,
    .pFctSwitchOnPwm                   = &R3_2_SwitchOnPWM,
    .pFctCurrReadingCalib              = &R3_2_CurrentReadingCalibration,
    .pFctTurnOnLowSides                = &R3_2_TurnOnLowSides,
    .pFctSetADCSampPointSectX          = &R3_2_SetADCSampPointSectX,
    .pFctIsOverCurrentOccurred         = &R3_2_IsOverCurrentOccurred,
    .pFctOCPSetReferenceVoltage        = MC_NULL,
    .pFctRLDetectionModeEnable         = &R3_2_RLDetectionModeEnable,
    .pFctRLDetectionModeDisable        = &R3_2_RLDetectionModeDisable,
    .pFctRLDetectionModeSetDuty        = &R3_2_RLDetectionModeSetDuty,
    .hT_Sqrt3 = (PWM_PERIOD_CYCLES*SQRT3FACTOR)/16384u,
… …
};

其中计算因子宏定义为：

#define SECTOR_1  0u
#define SECTOR_2  1u
#define SECTOR_3  2u
#define SECTOR_4  3u
#define SECTOR_5  4u
#define SECTOR_6  5u
#define SQRT3FACTOR (uint16_t) 0xDDB4 /* = (16384 * 1.732051 * 2)*/

通过比较其大小判断所处扇区。然后独立计算三相定时器的比较值大小，设置定时器各通道的比较值，这样就可以实现，控制三相逆变电路的开关了，从而实现FOC控制。最后再附加死区控制完成完整的SVPWM。

27.8 FOC位置闭环控制

本小节我们将使用电机开发板通过FOC控制PMSM电机旋转到指定位置！

27.8.1 硬件设计

1、例程功能

1、本实验以电机开发板的直流有刷/无刷电机驱动接口1为例，基于《FOC_速度模式控制_编码器驱动（接口1）》实验，修改为FOC位置闭环控制，对电机转轴的旋转位置进行闭环控制。

2、当按键KEY0按下，PMSM电机转轴旋转0.5圈；当按键KEY1按下，PMSM电机转轴旋转1圈。按下按键KEY2 ，PMSM电机转轴旋转10圈。

2、硬件资源

1）独立按键

KEY0 – PE12

KEY1 – PE13

KEY2 – PE14

2）定时器：

TIM1_CH1：PA8

TIM1_CH2：PA9

TIM1_CH3：PA10

TIM1_CH1N：PB13

TIM1_CH2N：PB14

TIM1_CH3N：PB15

TIM1_BKIN2：PA11

TIM3_CH1：PA6

TIM3_CH2：PA7

TIM3_CH3：PE4

3）IO：

使能IO：SHDN: PC13

4）PMSM电机

5）无刷电机驱动板

6）12-60V的DC电源（建议使用电机额定电压24V）

7）ADC：

ADC12_CH6（温度）：PC0

ADC1_CH4（电源电压）：PA3

ADC12_CH7（U相电流）：PC1

ADC12_CH8（V相电流）：PC2

ADC12_CH9（W相电流）：PC3

3、原理图

图27.8.1.1 无刷电机接口1原理图

接口涉及的IO如下：

表27.8.1.1 无刷相关IO口说明

27.8.2 FOC位置闭环配置步骤

本小节的配置与前面的创建例程大部分相同，所以我们这里只讲解不同部分的设置，首先，修改控制模式，参数，方法如下：

图27.8.2.2 位置控制模式

将默认的参数内容设置为如下，这些参数是针对实验所用的PMSM电机专门适应的参数，由工程师根据效果调节得来。

图27.8.2.3 PID参数设置

由于本实验是位置控制，所以建议使用编码器反馈位置信息，会更加精确，所以传感器一栏选择编码器，如下图：

图27.8.2.4 位置传感器选择编码器

再MCworkbench上的设置区别就是这部分了，其他与上一节完全一致，接着我们重新生成MDK工程，方法如下：

图27.8.2.5 更新工程

生成代码后，第一步，同样需要将cubemx中的外部晶振修改为8M，并将系统时钟配置为170MHz；第二步，配置PC11，推挽输出，并初始化为上拉，即默认使能半桥芯片，修改完成后，更新代码，然后打开MDK工程，将函数HAL_PWREx_DisableUCPDDeadBattery屏蔽掉，不然会报错！这些修改好之后添加我们的按键驱动代码key.c到工程里，并添加如下代码：

图27.8.2.6 添加相应代码

代码会在后续内容中介绍。先看下添加按键驱动后需要修改的部分。由于按键驱动使用到了KEY2，所以需要将之前配置的KEY2启停电机的中断屏蔽掉，不然会导致按键功能不正常，如下：

图27.8.2.7屏蔽KEY2的外部中断

软件修改部分到这里就全部修改完成了，然后硬件整体连接和测试方法同之前一致，重新编译下载程序到开发板，连接电机及驱动板，注意上电顺序。

测试时切记要注意安全。

测试方法为：当按键KEY0按下，PMSM电机转轴旋转0.5圈；当按键KEY1按下，PMSM电机转轴旋转1圈。按下按键KEY2 ，PMSM电机转轴旋转10圈。

下面我们来介绍下，本实验中我们所使用到的API函数的作用，MCSDK的API 函数：MC_ProgramPositionCommandMotor1用于修改定位位置，函数原型如下（已省略部分注释）：

/**
  * @brief Programs a position command for Motor 1 for later or immediate execution.
  *
  * @param  fTargetPosition Target mechanical angle reference at the end of the movement.
  *         This value represents the final position expressed in radian.
  * @param  fDuration Duration of the movement expressed in seconds.
  *         It is possible to set 0 to perform an instantaneous angular change (follow mode).
  */
__weak void MC_ProgramPositionCommandMotor1( float fTargetPosition, float fDuration )
{
    MCI_ExecPositionCommand( pMCI[M1], fTargetPosition, fDuration );
}

该API函数相对比较简单，它只有两个入口参数，其中fTargetPosition为目标位置，而fDuration为时间。例如ProgramPositionCommandMotor1( 3.1415 , 1.0 )即在1秒内，电机旋转半圈时定住位置。用户可以自行添加相应的应用程序以修改位置闭环。例如，我们添加了按键的驱动代码，并在mian.c里边添加了如下代码：

int main(void)
{
  /* USER CODE BEGIN 1 */
    uint8_t key = 0;
    float step = 0;
  /* USER CODE END 1 */
 
  /* MCU Configuration---------------------------------------*/
 
  /*Reset of all peripherals,Initializes the Flash interface and the Systick*/
  HAL_Init();
 
  /* USER CODE BEGIN Init */
 
  /* USER CODE END Init */
 
  /* Configure the system clock */
  SystemClock_Config();
 
  /* USER CODE BEGIN SysInit */
 
  /* USER CODE END SysInit */
 
  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_CORDIC_Init();
  MX_DAC1_Init();
  MX_TIM1_Init();
  MX_TIM3_Init();
  MX_USART1_UART_Init();
  MX_MotorControl_Init();
 
  /* Initialize interrupts */
  MX_NVIC_Init();
  /* USER CODE BEGIN 2 */
  key_init();
  MC_StartMotor1();
  MC_ProgramPositionCommandMotor1( PI * 0, 0.5 );    /* 设置起始位置 */
  HAL_Delay(1000);                                 /* 等待稳定后再去控制*/
  /* USER CODE END 2 */
 
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    key = key_scan(0);
    if(MC_GetSTMStateMotor1() != IDLE)
    {
        if(key == KEY0_PRES)
        {
            step += 1;                          /* 加半圈位置 */
            MC_ProgramPositionCommandMotor1(PI * step, 2); /* 目标位置加0.5圈 */
            
        }else if(key == KEY1_PRES)
        {
            step += 2;                          /* 加1圈位置 */
            MC_ProgramPositionCommandMotor1(PI * step, 2); /* 目标位置加1圈 */
        }
        else if (key == KEY2_PRES)
        {
            step += 20;                         /* 加10圈位置 */
            MC_ProgramPositionCommandMotor1(PI * step, 2); /* 目标位置加10圈 */
        }
    }
    HAL_Delay(10);                                       /* 消抖 */
    /* USER CODE END WHILE */
 
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

main函数主要添加按键的初始化代码，然后启动电机，并使其一启动就定位在某一位置，作为它的起点位置，然后在while循环里边判断是KEY0、KEY1还是KEY2按下。按键判断逻辑：当按键KEY0按下，PMSM电机转轴旋转0.5圈；当按键KEY1按下，PMSM电机转轴旋转1圈。当按下按键KEY2 ，PMSM电机转轴旋转10圈。

27.8.3 下载验证

下载代码后，可以看到电机会先移动到起始位置（相当于定初始位置），当按键KEY0按下，PMSM电机转轴旋转0.5圈；当按键KEY1按下，PMSM电机转轴旋转1圈。当按下按键KEY2 ，PMSM电机转轴旋转10圈。大家可以自己下载程序并点击按键测试效果。

至此，整个DMG474电机开发板的FOC实验内容就讲完了，其中关于FOC的内容理解也参考了不少网上的资料，对此表示衷心的感谢，同时也希望我们这些代码，可以让大家有所受益，能开发出更强更好的产品。

总的来说我们的教程里介绍的电机种类是非常多的，所以关于电机的相关代码也是比较多的，希望大家，慢慢理解，各个攻破，最后祝大家身体健康、学习进步！

ITPub博客

《DMG474电机控制专题教程V1.1 》第27章 FOC（7-8）

最新文章