STM32如何同时读取ABZ编码器获取角度、速度与位置信息

        ABZ编码器的原理、信号波形以及如何用cubemx配置在网上有很多介绍，本文以STM32G431为例重点介绍如何利用freeRTOS同时实现角度、速度和位置的读取。

1. cubemx配置编码器

        我使用的ABZ编码器的分辨率是1024，配置四倍频就是4096，所以自动重载设置成4095，这样编码器旋转一周刚好对应0~4095。

以上配置读取的是编码器的AB相，还需要通过外部中断的方式读取零点脉冲即Z相：

生成代码如下：

tim.c:

/* TIM3 init function */
void MX_TIM3_Init(void)
{

  /* USER CODE BEGIN TIM3_Init 0 */

  /* USER CODE END TIM3_Init 0 */

  TIM_Encoder_InitTypeDef sConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM3_Init 1 */

  /* USER CODE END TIM3_Init 1 */
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 0;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 4095;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
  sConfig.IC1Polarity = TIM_ICPOLARITY_FALLING;
  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC1Filter = 0;
  sConfig.IC2Polarity = TIM_ICPOLARITY_FALLING;
  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC2Filter = 0;
  if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM3_Init 2 */

  /* USER CODE END TIM3_Init 2 */

}

void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(tim_encoderHandle->Instance==TIM3)
  {
  /* USER CODE BEGIN TIM3_MspInit 0 */

  /* USER CODE END TIM3_MspInit 0 */
    /* TIM3 clock enable */
    __HAL_RCC_TIM3_CLK_ENABLE();

    __HAL_RCC_GPIOC_CLK_ENABLE();
    /**TIM3 GPIO Configuration
    PC6     ------> TIM3_CH1
    PC7     ------> TIM3_CH2
    */
    GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

  /* USER CODE BEGIN TIM3_MspInit 1 */

  /* USER CODE END TIM3_MspInit 1 */
  }
}

void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle)
{

  if(tim_encoderHandle->Instance==TIM3)
  {
  /* USER CODE BEGIN TIM3_MspDeInit 0 */

  /* USER CODE END TIM3_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_TIM3_CLK_DISABLE();

    /**TIM3 GPIO Configuration
    PC6     ------> TIM3_CH1
    PC7     ------> TIM3_CH2
    */
    HAL_GPIO_DeInit(GPIOC, GPIO_PIN_6|GPIO_PIN_7);

  /* USER CODE BEGIN TIM3_MspDeInit 1 */

  /* USER CODE END TIM3_MspDeInit 1 */
  }
}

在适当时机执行HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);即可启动编码器

2. 读取角度

        上面配置完成之后，通过__HAL_TIM_GET_COUNTER(&htim3)获取的就可以被认为是角度，因为编码器经过4倍频的分辨率是0~4095，寄存器重载的值也是4095，只是每次启动的时候零点会变，而FOC的计算需要的是绝对不变的角度，需要在Z相中断回调函数里面把编码器的寄存器置零：

/* USER CODE BEGIN 2 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if (GPIO_Pin == encode_Z_Pin)
  {
    // 重置编码器计数器
    Set_Encoder_Zero();

    // printf("Encoder Z interrupt: Counter reset\n");
  }
}

void Set_Encoder_Zero()
{
  __HAL_TIM_SET_COUNTER(&htim3, 0);
  encoder_angle_valid = 1;
}

        每次Z相中断都会校准编码器的零点位置，再次读出来的角度就是绝对角度：

// 计算传感器角度
void CalSensorAngle()
{
  uint16_t encode = Read_Encode(); // 0 ~ 4095
  // printf("encode: %d\n", encode);
  sensor_angle = ((float)encode / 4096.0f) * 2.0f * PI;
}

uint16_t Read_Encode()
{
  return __HAL_TIM_GET_COUNTER(&htim3);
  // __HAL_TIM_SET_COUNTER(&htim1, 0);
  // printf("encoder: %d\n", encoder);

}

3. 读取速度和位置

        读取速度和位置目的是让其参与PID的计算，我这里是让PID任务以固定1000Hz的频率计算：

xTaskCreate(pid_task, "PidTask", 256, NULL, osPriorityHigh, &pidTaskHandle);

void pid_task(void *params)
{
  TickType_t xLastWakeTime;
  const TickType_t xFrequency = pdMS_TO_TICKS(1);

  xLastWakeTime = xTaskGetTickCount();

  /**
   * PID初始化代码
   */

  for (;;)
  {
    
      /**
       * 计算速度、位置
       */

      /**
       * PID代码
       */

    vTaskDelayUntil(&xLastWakeTime, xFrequency);
  } 
}

计算速度和位置的方法如下：

float Cal_Encoder_Speed()
{
  // 计算编码器差异
  int encoder_value = Read_Encode();
  int delta_encoder = encoder_value - last_encoder_value;

  // 处理编码器回绕情况
  if (delta_encoder > 2048)
  {
    delta_encoder -= 4096;
  } else if (delta_encoder < -2048)
  {
    delta_encoder += 4096;
  }
  
  // 计算速度，单位是弧度每秒 (rad/s)
  float speed = (float)delta_encoder / 4096.0f * 2.0f * PI * 1000.0f;

  // 计算位置单位是弧度 (rad)
  position_encoder += (float)delta_encoder / 4096.0f * 2.0f * PI;
  last_encoder_value = encoder_value;
  return speed;
}

作者：不再犹豫123