手把手移植 simpleFOC (三)：编码器篇

文章目录

前言

一、延时函数

二、修改encoder外中断接口

1.中断调用接口

2.嫁接回调函数

3、新增digitalRead函数

三、添加编译项

四、编译，调试

 总结

---

前言

今天移植的主要内容是simpleFoc的encoder，目标是转到电机，读出对应的角度及角度率。

一、延时函数

阅读encoder.cpp后，发现里面需要几个延时函数。由于stm32cubeMX生成工程后，sysTick默认为产生1ms中断，并且HAL_GetTick()函数记录了从开机起到当前的ms数。利用该接口编写millis（）、micros（）、delay（）供 simpleFoc使用

掏出arduino里wiring.c文件，进行修改。

#include "Arduino.h"
#include "stm32f1xx_hal.h"

static void __empty() {
	// Empty
}
void yield(void) __attribute__ ((weak, alias("__empty")));

#ifdef __cplusplus
extern "C" {
#endif

uint32_t millis(void)
{
// todo: ensure no interrupts
    return HAL_GetTick();
}

// Interrupt-compatible version of micros
// Theory: repeatedly take readings of SysTick counter, millis counter and SysTick interrupt pending flag.
// When it appears that millis counter and pending is stable and SysTick hasn't rolled over, use these 
// values to calculate micros. If there is a pending SysTick, add one to the millis counter in the calculation.
uint32_t micros(void)
{
    uint32_t ticks, ticks2;
    uint32_t pend, pend2;
    uint32_t count, count2;

    ticks2  = SysTick->VAL;
    pend2   = !!((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk)||((SCB->SHCSR & SCB_SHCSR_SYSTICKACT_Msk)))  ;
    count2  = HAL_GetTick();

    do {
        ticks=ticks2;
        pend=pend2;
        count=count2;
        ticks2  = SysTick->VAL;
        pend2   = !!((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk)||((SCB->SHCSR & SCB_SHCSR_SYSTICKACT_Msk)))  ;
        count2  = HAL_GetTick();
    } while ((pend != pend2) || (count != count2) || (ticks < ticks2));

    return ((count+pend) * 1000) + (((SysTick->LOAD  - ticks)*(1048576/(SystemCoreClock/1000000)))>>20) ; 
    // this is an optimization to turn a runtime division into two compile-time divisions and 
    // a runtime multiplication and shift, saving a few cycles
}

// original function:
// uint32_t micros( void )
// {
//     uint32_t ticks ;
//     uint32_t count ;
// 
//     SysTick->CTRL;
//     do {
//         ticks = SysTick->VAL;
//         count = GetTickCount();
//     } while (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk);
// 
//     return count * 1000 + (SysTick->LOAD + 1 - ticks) / (SystemCoreClock/1000000) ;
// }


void delay(uint32_t dwMs)
{
    if (dwMs == 0)
        return;
    uint32_t start = HAL_GetTick();
    do {
        yield();
    } while (HAL_GetTick() - start < dwMs);
}

#ifdef __cplusplus
}
#endif

二、修改encoder外中断接口

1.中断调用接口

一般编码器 doA doB会赋予的，所以在中断里尽量节约执行时间，并没有判断回调函数是否为空，而doIndex项，看例程里有时并没有赋予，所以在执行时判断了 if(functionZ) 再执行，否则stm32会异常，该处踩坑了。

typedef void (*pf_callbakck)(void);

static pf_callbakck functionA=nullptr;
static pf_callbakck functionB=nullptr;
static pf_callbakck functionZ=nullptr;


void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  switch(GPIO_Pin)
  {
    case M0_ENC_A_Pin:{
      functionA();
    }break;
    case M0_ENC_B_Pin:{
      functionB();
    }break;
    case M0_ENC_Z_Pin:{
      if(functionZ)
        functionZ();
    }break;
    default:break;
  }
  
}

2.嫁接回调函数

代码如下：

void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)())
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(doA != nullptr) functionA=doA;
  if(doB != nullptr) functionB=doB;

  GPIO_InitStruct.Pin = M0_ENC_A_Pin|M0_ENC_B_Pin;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  switch(quadrature){
    case Quadrature::ON:      
      /*Configure GPIO pins : PBPin PBPin */
      
      GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
      
      break;
    case Quadrature::OFF:
      // A callback and B callback

      GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;

      break;
  }
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = M0_ENC_Z_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(M0_ENC_Z_GPIO_Port, &GPIO_InitStruct);

  // if index used initialize the index interrupt
  if(hasIndex() && doIndex != nullptr) 
    functionZ=doIndex;

  HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
}

3、新增digitalRead函数

__inline__  bool digitalRead(int pin)
{
  bool tmp=0;
  switch(pin)
  {
    case M0_ENC_A_Pin:tmp=HAL_GPIO_ReadPin(M0_ENC_A_GPIO_Port, M0_ENC_A_Pin);break;
    case M0_ENC_B_Pin:tmp=HAL_GPIO_ReadPin(M0_ENC_B_GPIO_Port, M0_ENC_B_Pin);break;
    case M0_ENC_Z_Pin:tmp=HAL_GPIO_ReadPin(M0_ENC_Z_GPIO_Port, M0_ENC_Z_Pin);break;
    default:break;
  }  
  return tmp;
}

三、添加编译项

如图所示：

四、编译，调试

测试代码如下：

#define __MAIN_CPP__

#include "simpleFoc_main.h"
#include "Print.h"
#include "hwSerial.h"
#include <SimpleFOC.h>
#include "main.h"
extern HardwareSerial  Serial2;
#define Serial Serial2


extern "C" {


Encoder encoder = Encoder(M0_ENC_A_Pin, M0_ENC_B_Pin, 1000);
// interrupt routine intialisation
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

void setup() {  

  // enable/disable quadrature mode
  encoder.quadrature = Quadrature::ON;

  // check if you need internal pullups
  encoder.pullup = Pullup::USE_EXTERN;
  
  // initialise encoder hardware
  encoder.init();
  // hardware interrupt enable
  encoder.enableInterrupts(doA, doB);

  Serial.println("Encoder ready");
  _delay(1000);
}

void loop() {
  // iterative function updating the sensor internal variables
  // it is usually called in motor.loopFOC()
  // not doing much for the encoder though
  // encoder.update();
  // display the angle and the angular velocity to the terminal
  Serial.print(encoder.getAngle());
  Serial.print("\t");
  Serial.println(encoder.getVelocity());
  _delay(100);
}



}

编译下载，转动电机，运行结果如下：

---

 总结

 运行结果，编码器程序移植工作正常。只是simpleFoc采用的是外部中断方式进行采集，看了stm32定时器章节，里面有编码器接口功能，后续有必要的情况下会对该部分以定时器接口进行实现。