【CW32F003ExPx StartKit开发板】超强增量伺服编码器测试工具

1万 · 2024-6-7 17:04

本帖最后由呐咯密密于 2024-6-11 16:24 编辑

子标题:基于CW32F003的OLED+串口助手双显示伺服电机增量式编码器调试工具
前言

有幸参与芯源的开发板测评活动，此次旨在选用官方的开发板完成一次DEMO设计。本人此次的主题是设计一款可以获取增量式编码器的位置和电机旋转速度的测试设备，便于从业人员判断编码器的好坏和编码器的调零工作。获取的编码器数据通过OLED显示屏和串口助手打印，通过串口助手的绘图，更可直观展现位置波动和速度波动。

一、开发要求

1.完成MCU获取编码器脉冲数据。

2.完成开发板和串口助手的通信。

3.完成OLED的显示驱动。

4.完成按键切换速度显示和位置显示模式。

5.数据滤波处理。

二、系统架构

简单的系统架构如上图所示，输入设备为开发板自带的两个按键，用于切换速度模式和位置模式，单片机通过定时器的编码器接口获取编码器的脉冲信号，OLED显示编码器位置和电机旋转速度，数据同步传输至上位机，可实时显示位置及速度。

三、开发环境
1.CW32F003ExPx StartKit开发板

2.澜宭LME2500FE磁编码器（4对级，2500线）

3.0.96OLED显示模块

4.创芯工坊PowerWtriter X1烧录器
5.电机旋转对拖台（包括整套的伺服驱动器加伺服电机）

6.纸飞机串口助手，用于图形化显示数据
四、详细流程

如上图所示，系统上电后完成MCU系统和外设的初始化，KEY1和KEY2亿外部中断的形式判断按键是否被按下。当KEY按下，OLED显示“Position mode”，表示已经进入位置模式，此时启动ATIM的编码器功能，通过编码器A+ B+ 的脉冲信号的数量和相位关系计算编码器位置，因为是4对级2500线，所以旋转一圈是10000个脉冲。该数据会在OLED和串口助手上显示。如果KEY2被按下，则进入速度模式，OLED显示“Speed mode”，此时除了ATIM外，还需启动BTIM1（用于产生高频时钟脉冲）和GTIM（周期获取高频脉冲和编码器脉冲），通过M/T算法计算电机旋转速度。计算后的速度会显示在OLED上，同时进**尔曼滤波后同时将原始速度和滤波后的速度打印在纸飞机串口助手上，便于观察速度波动。

六、代码实现
1.USART

复制

 //UARTx

#define  DEBUG_USARTx                   CW_UART1

#define  DEBUG_USART_CLK                RCC_APB2_PERIPH_UART1

#define  DEBUG_USART_APBClkENx          RCC_APBPeriphClk_Enable2

#define  DEBUG_USART_BaudRate           9600

#define  DEBUG_USART_UclkFreq           48000000



//UARTx GPIO

#define  DEBUG_USART_TX_GPIO_CLK        RCC_AHB_PERIPH_GPIOB

#define  DEBUG_USART_RX_GPIO_CLK        RCC_AHB_PERIPH_GPIOB

#define  DEBUG_USART_TX_GPIO_PORT       CW_GPIOB   

#define  DEBUG_USART_TX_GPIO_PIN        GPIO_PIN_1

#define  DEBUG_USART_RX_GPIO_PORT       CW_GPIOB

#define  DEBUG_USART_RX_GPIO_PIN        GPIO_PIN_0



//GPIO AF

#define  DEBUG_USART_AFTX               PB01_AFx_UART1TXD()

#define  DEBUG_USART_AFRX               PB00_AFx_UART1RXD()

void USART_GPIO_Configuration(void)

{

  GPIO_InitTypeDef GPIO_InitStructure;



  //UART TX RX 复用

  DEBUG_USART_AFTX;                     

  DEBUG_USART_AFRX;                     



  GPIO_InitStructure.Pins = DEBUG_USART_TX_GPIO_PIN;

  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;

  GPIO_Init(DEBUG_USART_TX_GPIO_PORT, &GPIO_InitStructure);

    

  GPIO_InitStructure.Pins = DEBUG_USART_RX_GPIO_PIN;

  GPIO_InitStructure.Mode = GPIO_MODE_INPUT_PULLUP;

  GPIO_Init(DEBUG_USART_RX_GPIO_PORT, &GPIO_InitStructure);

}



/**

 * [url=home.php?mod=space&uid=247401]@brief[/url] 配置UART

 * 

 */

void UART_Configuration(void)

{

  USART_InitTypeDef USART_InitStructure;



  USART_InitStructure.USART_BaudRate = DEBUG_USART_BaudRate;

  USART_InitStructure.USART_Over = USART_Over_16;

  USART_InitStructure.USART_Source = USART_Source_PCLK;

  USART_InitStructure.USART_UclkFreq = DEBUG_USART_UclkFreq;

  USART_InitStructure.USART_StartBit = USART_StartBit_FE;

  USART_InitStructure.USART_StopBits = USART_StopBits_1;

  USART_InitStructure.USART_Parity = USART_Parity_No ;

  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;

  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

  USART_Init(DEBUG_USARTx, &USART_InitStructure);         

}

/**

 * [url=home.php?mod=space&uid=247401]@brief[/url] Retargets the C library printf function to the USART.

 * 

 */

PUTCHAR_PROTOTYPE

{

  USART_SendData_8bit(DEBUG_USARTx, (uint8_t)ch);



  while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);



  return ch;

}

板载的串口转USB在硬件上是无电气连接的，此处需要自行跳线处理，此处使用PB0和PB1作为串口通信。波特率使用9600。2.OLED模块

OLED使用软件模拟SPI的方式驱动，硬件连接如下：

SCLK-->PB0DIN-->PB1
RES-->PB3
DC-->PB4
CS-->GND
OLED.C

复制

#include "oled.h"

#include "stdlib.h"

#include "oledfont.h"           

#include "cw32f003_gpio.h"



//OLED的显存

//存放格式如下.

//[0]0 1 2 3 ... 127 

//[1]0 1 2 3 ... 127 

//[2]0 1 2 3 ... 127 

//[3]0 1 2 3 ... 127 

//[4]0 1 2 3 ... 127 

//[5]0 1 2 3 ... 127 

//[6]0 1 2 3 ... 127 

//[7]0 1 2 3 ... 127       



#if OLED_MODE==1

//向SSD1106写入一个字节。

//dat:要写入的数据/命令

//cmd:数据/命令标志 0,表示命令;1,表示数据;

void OLED_WR_Byte(u8 dat,u8 cmd)

{

 DATAOUT(dat);     

 if(cmd)

   OLED_DC_Set();

 else 

   OLED_DC_Clr();     

 OLED_CS_Clr();

 OLED_WR_Clr();  

 OLED_WR_Set();

 OLED_CS_Set();   

 OLED_DC_Set();  

}           

#else

//向SSD1106写入一个字节。

//dat:要写入的数据/命令

//cmd:数据/命令标志 0,表示命令;1,表示数据;

void OLED_WR_Byte(uint8_t dat,uint8_t cmd)

{ 

 uint8_t i;     

 if(cmd)

   OLED_DC_Set();

 else 

   OLED_DC_Clr();    

 OLED_CS_Clr();

 for(i=0;i<8;i++)

 {     

  OLED_SCLK_Clr();

  if(dat&0x80)

     OLED_SDIN_Set();

  else 

     OLED_SDIN_Clr();

  OLED_SCLK_Set();

  dat<<=1;   

 }         

 OLED_CS_Set();

 OLED_DC_Set();      

} 

#endif



 void OLED_Set_Pos(unsigned char x, unsigned char y) 

{ 

 OLED_WR_Byte(0xb0+y,OLED_CMD);

 OLED_WR_Byte(((x&0xf0)>>4)|0x10,OLED_CMD);

 OLED_WR_Byte((x&0x0f)|0x01,OLED_CMD); 

}      

//开启OLED显示    

void OLED_Display_On(void)

{

 OLED_WR_Byte(0X8D,OLED_CMD);  //SET DCDC命令

 OLED_WR_Byte(0X14,OLED_CMD);  //DCDC ON

 OLED_WR_Byte(0XAF,OLED_CMD);  //DISPLAY ON

}

//关闭OLED显示     

void OLED_Display_Off(void)

{

 OLED_WR_Byte(0X8D,OLED_CMD);  //SET DCDC命令

 OLED_WR_Byte(0X10,OLED_CMD);  //DCDC OFF

 OLED_WR_Byte(0XAE,OLED_CMD);  //DISPLAY OFF

}         

//清屏函数,清完屏,整个屏幕是黑色的!和没点亮一样!!!   

void OLED_Clear(void)  

{  

 uint8_t i,n;      

 for(i=0;i<8;i++)  

 {  

  OLED_WR_Byte (0xb0+i,OLED_CMD);    //设置页地址（0~7）

  OLED_WR_Byte (0x00,OLED_CMD);      //设置显示位置—列低地址

  OLED_WR_Byte (0x10,OLED_CMD);      //设置显示位置—列高地址   

  for(n=0;n<128;n++)OLED_WR_Byte(0,OLED_DATA); 

 } //更新显示

}



//在指定位置显示一个字符,包括部分字符

//x:0~127

//y:0~63

//mode:0,反白显示;1,正常显示     

//size:选择字体 16/12 

void OLED_ShowChar(uint8_t x,uint8_t y,uint8_t chr)

{       

 unsigned char c=0,i=0; 

  c=chr-' ';//得到偏移后的值   

  if(x>Max_Column-1){x=0;y=y+2;}

  if(SIZE ==16)

   {

   OLED_Set_Pos(x,y); 

   for(i=0;i<8;i++)

   OLED_WR_Byte(F8X16[c*16+i],OLED_DATA);

   OLED_Set_Pos(x,y+1);

   for(i=0;i<8;i++)

   OLED_WR_Byte(F8X16[c*16+i+8],OLED_DATA);

   }

   else { 

    OLED_Set_Pos(x,y+1);

    for(i=0;i<6;i++)

    OLED_WR_Byte(F6x8[c][i],OLED_DATA);   

   }

}

//m^n函数

uint32_t oled_pow(uint8_t m,uint8_t n)

{

 uint32_t result=1;  

 while(n--)result*=m;    

 return result;

}      

//显示2个数字

//x,y :起点坐标  

//len :数字的位数

//size:字体大小

//mode:模式 0,填充模式;1,叠加模式

//num:数值(0~4294967295);      

void OLED_ShowNum(uint8_t x,uint8_t y,uint32_t num,uint8_t len,uint8_t size)

{          

 uint8_t t,temp;

 uint8_t enshow=0;         

 for(t=0;t<len;t++)

 {

  temp=(num/oled_pow(10,len-t-1))%10;

  if(enshow==0&&t<(len-1))

  {

   if(temp==0)

   {

    OLED_ShowChar(x+(size/2)*t,y,' ');

    continue;

   }else enshow=1; 

     

  }

   OLED_ShowChar(x+(size/2)*t,y,temp+'0'); 

 }

} 

//显示一个字符号串

void OLED_ShowString(uint8_t x,uint8_t y,uint8_t *chr)

{

 unsigned char j=0;

 while (chr[j]!='\0')

 {  OLED_ShowChar(x,y,chr[j]);

   x+=8;

  if(x>120){x=0;y+=2;}

   j++;

 }

}

//显示汉字

void OLED_ShowCHinese(uint8_t x,uint8_t y,uint8_t no)

{             

 uint8_t t,adder=0;

 OLED_Set_Pos(x,y); 

    for(t=0;t<16;t++)

  {

    OLED_WR_Byte(Hzk[2*no][t],OLED_DATA);

    adder+=1;

     } 

  OLED_Set_Pos(x,y+1); 

    for(t=0;t<16;t++)

   { 

    OLED_WR_Byte(Hzk[2*no+1][t],OLED_DATA);

    adder+=1;

      }     

}

/***********功能描述：显示显示BMP图片128×64起始点坐标(x,y),x的范围0～127，y为页的范围0～7*****************/

void OLED_DrawBMP(unsigned char x0, unsigned char y0,unsigned char x1, unsigned char y1,unsigned char BMP[])

{  

 unsigned int j=0;

 unsigned char x,y;

  

  if(y1%8==0) y=y1/8;      

  else y=y1/8+1;

 for(y=y0;y<y1;y++)

 {

  OLED_Set_Pos(x0,y);

    for(x=x0;x<x1;x++)

     {      

      OLED_WR_Byte(BMP[j++],OLED_DATA);      

     }

 }

} 



//初始化SSD1306         

void OLED_Init(void)

{    

          GPIO_InitTypeDef GPIO_InitStruct;



        GPIO_InitStruct.IT = GPIO_IT_NONE;

        GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

        GPIO_InitStruct.Pins = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5;

        GPIO_Init(CW_GPIOA, &GPIO_InitStruct); 

  

  OLED_RST_Set();

        FirmwareDelay(500000);

  OLED_RST_Clr();

 FirmwareDelay(200000);

  OLED_RST_Set();        

  

  OLED_WR_Byte(0xAE,OLED_CMD);//--turn off oled panel

  OLED_WR_Byte(0x00,OLED_CMD);//---set low column address

  OLED_WR_Byte(0x10,OLED_CMD);//---set high column address

  OLED_WR_Byte(0x40,OLED_CMD);//--set start line address  Set Mapping RAM     Display Start Line (0x00~0x3F)

 OLED_WR_Byte(0x81,OLED_CMD);//--set contrast control register

 OLED_WR_Byte(0xCF,OLED_CMD); // Set SEG Output Current Brightness

 OLED_WR_Byte(0xA1,OLED_CMD);//--Set SEG/Column Mapping     0xa0左右反置 0xa1正常

 OLED_WR_Byte(0xC8,OLED_CMD);//Set COM/Row Scan Direction   0xc0上下反置 0xc8正常

 OLED_WR_Byte(0xA6,OLED_CMD);//--set normal display

 OLED_WR_Byte(0xA8,OLED_CMD);//--set multiplex ratio(1 to 64)

 OLED_WR_Byte(0x3f,OLED_CMD);//--1/64 duty

 OLED_WR_Byte(0xD3,OLED_CMD);//-set display offset Shift Mapping RAM Counter (0x00~0x3F)

 OLED_WR_Byte(0x00,OLED_CMD);//-not offset

 OLED_WR_Byte(0xd5,OLED_CMD);//--set display clock divide ratio/oscillator frequency

 OLED_WR_Byte(0x80,OLED_CMD);//--set divide ratio, Set Clock as 100 Frames/Sec

 OLED_WR_Byte(0xD9,OLED_CMD);//--set pre-charge period

 OLED_WR_Byte(0xF1,OLED_CMD);//Set Pre-Charge as 15 Clocks & Discharge as 1 Clock

 OLED_WR_Byte(0xDA,OLED_CMD);//--set com pins hardware configuration

 OLED_WR_Byte(0x12,OLED_CMD);

 OLED_WR_Byte(0xDB,OLED_CMD);//--set vcomh

 OLED_WR_Byte(0x40,OLED_CMD);//Set VCOM Deselect Level

 OLED_WR_Byte(0x20,OLED_CMD);//-Set Page Addressing Mode (0x00/0x01/0x02)

 OLED_WR_Byte(0x02,OLED_CMD);//

 OLED_WR_Byte(0x8D,OLED_CMD);//--set Charge Pump enable/disable

 OLED_WR_Byte(0x14,OLED_CMD);//--set(0x10) disable

 OLED_WR_Byte(0xA4,OLED_CMD);// Disable Entire Display On (0xa4/0xa5)

 OLED_WR_Byte(0xA6,OLED_CMD);// Disable Inverse Display On (0xa6/a7) 

 OLED_WR_Byte(0xAF,OLED_CMD);//--turn on oled panel

 OLED_WR_Byte(0xAF,OLED_CMD); /*display ON*/ 

 OLED_Clear();

 OLED_Set_Pos(0,0);  

}

OLED.H

复制

#ifndef __OLED_H

#define __OLED_H

#include "cw32f003.h"







//OLED模式设置

//0:4线串行模式

//1:并行8080模式

#define OLED_MODE 0

#define SIZE 16

#define XLevelL  0x00

#define XLevelH  0x10

#define Max_Column 128

#define Max_Row  64

#define Brightness 0xFF 

#define X_WIDTH  128

#define Y_WIDTH  64    



//-----------------OLED端口定义----------------          

#define OLED_SCLK_Clr() (CW_GPIOA->BRR=bv0)//CLK

#define OLED_SCLK_Set() (CW_GPIOA->BSRR=bv0)



#define OLED_SDIN_Clr() (CW_GPIOA->BRR=bv1)//DIN

#define OLED_SDIN_Set() (CW_GPIOA->BSRR=bv1)



#define OLED_RST_Clr() (CW_GPIOA->BRR=bv3)//RES

#define OLED_RST_Set() (CW_GPIOA->BSRR=bv3)



#define OLED_DC_Clr() (CW_GPIOA->BRR=bv4)//DC

#define OLED_DC_Set() (CW_GPIOA->BSRR=bv4)      



#define OLED_CS_Clr()  (CW_GPIOA->BRR=bv5)//CS

#define OLED_CS_Set()  (CW_GPIOA->BSRR=bv5)



#define OLED_CMD  0 //写命令

#define OLED_DATA 1 //写数据



//OLED控制用函数

void OLED_WR_Byte(uint8_t dat,uint8_t cmd);     

void OLED_Display_On(void);

void OLED_Display_Off(void);                    

void OLED_Init(void);

void OLED_Clear(void);

void OLED_DrawPoint(uint8_t x,uint8_t y,uint8_t t);

void OLED_Fill(uint8_t x1,uint8_t y1,uint8_t x2,uint8_t y2,uint8_t dot);

void OLED_ShowChar(uint8_t x,uint8_t y,uint8_t chr);

void OLED_ShowNum(uint8_t x,uint8_t y,uint32_t num,uint8_t len,uint8_t size);

void OLED_ShowString(uint8_t x,uint8_t y, uint8_t *p);  

void OLED_Set_Pos(unsigned char x, unsigned char y);

void OLED_ShowCHinese(uint8_t x,uint8_t y,uint8_t no);

void OLED_DrawBMP(unsigned char x0, unsigned char y0,unsigned char x1, unsigned char y1,unsigned char BMP[]);



#endif /* __MAIN_H */

3.按键驱动
按键直接采用了板载的按键，分别是KEY1-->PB5,KEY2-->PB6，采用中断的方式获取按键状态，用于切换速度和位置模式。

复制

void GPIOB_IRQHandlerCallback(void)

{

    if (CW_GPIOB->ISR_f.PIN5)

    {

        GPIOB_INTFLAG_CLR(bv5);

                //KEY1按下，进入位置模式

                Key_Flag = 1;

                CW_ATIM->CNT = 0;

                CW_GTIM->CNT = 0;

                CW_BTIM1->CNT = 0;

                ATIM_Cmd(DISABLE);

                GTIM_Cmd(DISABLE);

                BTIM_Cmd(CW_BTIM1, DISABLE);

                ATIM_configuration(9999);

                OLED_Clear();

                OLED_ShowString(0,0,"Position mode");        

    }



    if (CW_GPIOB->ISR_f.PIN6)

    {

        GPIOB_INTFLAG_CLR(bv6);

                //KEY2按下，进入速度模式

                Key_Flag = 2;

                CW_ATIM->CNT = 0;

                CW_GTIM->CNT = 0;

                CW_BTIM1->CNT = 0;

                BTIM_configuration();

                ATIM_configuration(65535);

                GTIM_configuration();

                OLED_Clear();

                OLED_ShowString(0,0,"Speed mode");                

    }

}

4.定时器
定时器部分为核心功能，速度和位置的数据来源均在此获取。
M/T法测速原理
以采样周期为基准，在采样时间T内，同时计算编码器输出的脉冲数量M1和高频脉冲数量M2，同时尽量保持两个计数时间的的严格同步，最大限度减小误差。

设电机旋转一圈编码器输出脉冲数为p,高频脉冲频率为fc，T时间内，高频脉冲计数为M2，编码器输出脉冲数为M1，则此时T时间内电机旋转圈数：

一秒电机旋转圈数为：

每分钟的转速为：

T可通过高频脉冲计数原理求得：

所以最终电机转速为：

在此应用中
/****CW_BTIM1  用于产生高频时钟脉冲****/
/****CW_ATIM  用于捕获编码器脉冲数****/
/****CW_GTIM  用于周期获取脉冲数和高频时钟脉冲****/

定时器CW_ATIM使用编码器功能，捕获M1的值，使用定时器CW_BTIM1产生高频脉冲，每当计数器到达就触发中断，获取M2的值，最后在定时器CW_GTIM中定时获取M1和M2的值，在主函数打印。

复制

/****CW_BTIM1  用于产生高频时钟脉冲****/

void BTIM_configuration(void)

{

        BTIM_TimeBaseInitTypeDef BTIM_TimeBaseInitStruct;

        BTIM_TimeBaseInitStruct.BTIM_Mode = BTIM_Mode_TIMER;

        BTIM_TimeBaseInitStruct.BTIM_Period = 239;

        BTIM_TimeBaseInitStruct.BTIM_Prescaler = BTIM_PRS_DIV1;



        BTIM_TimeBaseInit(CW_BTIM1, &BTIM_TimeBaseInitStruct);

        BTIM_ITConfig(CW_BTIM1, BTIM_IT_OV, ENABLE);

        BTIM_Cmd(CW_BTIM1, ENABLE);

}

/****CW_ATIM  用于捕获编码器脉冲数****/

void ATIM_configuration(uint32_t ReloadValue)

{

        ATIM_InitTypeDef ATIM_InitStruct;



        ATIM_InitStruct.BufferState = DISABLE;

        ATIM_InitStruct.ClockSelect = ATIM_CLOCK_PCLK;

        ATIM_InitStruct.CounterAlignedMode = ATIM_COUNT_MODE_EDGE_ALIGN;

        ATIM_InitStruct.CounterDirection = ATIM_COUNTING_UP;

        ATIM_InitStruct.CounterOPMode = ATIM_OP_MODE_REPETITIVE;

        ATIM_InitStruct.OverFlowMask = DISABLE;

        ATIM_InitStruct.Prescaler = ATIM_Prescaler_DIV1;

        ATIM_InitStruct.ReloadValue = ReloadValue;

        ATIM_InitStruct.RepetitionCounter = 0;

        ATIM_InitStruct.UnderFlowMask = DISABLE;



        ATIM_Init(&ATIM_InitStruct);

        ATIM_SlaverModeConfig(ATIM_SLAVER_TYPE_ENCODE3);

        ATIM_TriggerSelect(ATIM_TRIGGER_SOURCE_IAFP);

        ATIM_Cmd(ENABLE);



}

/****CW_GTIM  用于周期获取脉冲数和高频时钟脉冲****/

void GTIM_configuration(void)

{

        GTIM_InitTypeDef GTIM_InitStruct;

        BTIM_TimeBaseInitTypeDef BTIM_InitStruct;



        GTIM_InitStruct.Mode = GTIM_MODE_TIME;

        GTIM_InitStruct.OneShotMode = GTIM_COUNT_CONTINUE;

        GTIM_InitStruct.Prescaler = GTIM_PRESCALER_DIV32;

        GTIM_InitStruct.ReloadValue = 1499;

        GTIM_InitStruct.ToggleOutState = DISABLE;

        GTIM_TimeBaseInit(>IM_InitStruct);



        GTIM_ITConfig(GTIM_IT_OV, ENABLE);



        GTIM_Cmd(ENABLE);

}



void GTIM_IRQHandlerCallBack(void)

{

  if (GTIM_GetITStatus(GTIM_IT_OV))

  {

    GTIM_ClearITPendingBit(GTIM_IT_OV);

//    PA07_TOG();

                m1 = CW_ATIM->CNT;

                if(TIM_NUM<point_num)

                {

                        m1_data[TIM_NUM] = m1;

                        m2_data[TIM_NUM] = m2;

                        TIM_NUM++;

                        m1 = 0;

                        m2 = 0;

                }else

                {

                        ATIM_Cmd(DISABLE);

                        GTIM_Cmd(DISABLE);

                        BTIM_Cmd(CW_BTIM1, DISABLE);

                }          

  }

}



void BTIM1_IRQHandler(void)

{

  /* USER CODE BEGIN */

  if (BTIM_GetITStatus(CW_BTIM1, BTIM_IT_OV))

  {

        BTIM_ClearITPendingBit(CW_BTIM1, BTIM_IT_OV);

        m2++;

  }

  /* USER CODE END */

}

CW_ATIM 作为高级定时器，拥有编码器的功能，我们将其通道ATIM_CH1A和通道ATIM_CH1B接到编码器的A和B相。编码器功能属于定时器的从模式，通过函数ATIM_SlaverModeConfig(ATIM_SLAVER_TYPE_ENCODE3);这是为编码器模式3。

CW_BTIM1 用于产生一个100K的高频时钟脉冲，并使能溢出中断，每产生一个脉冲则将M2的值累加1；

CW_GTIM 用于周期获取脉冲数和高频时钟脉冲数，并使能溢出中断，中断到达后读取CW_ATIM的计数值，该值的变化量与编码器输出脉冲相同，计算两次之间的差值即为每次采样脉冲间隔的编码器脉冲数。中断中将M1和M2的值存入数组，在主函数进行速度计算并输出。

复制

  while(1)

  {



         if(Key_Flag == 2) 

         {

                if(TIM_NUM>point_num-2)

                {                        

                        for(uint16_t p=5;p<point_num-5;p++)

                        {

                                m1calc = m1_data[p]-m1_data[p+1];

                                if(m1_data[p]<m1_data[p+1])

                                {

                                        m1calc = m1calc+65535;                                                

                                }

                                Speed_num[p] = (float)(60*m1calc*20)/(m2_data[p]);

                                kalman_height = kalmanFilter(&KFP_height,Speed_num[p]);

        //                                                printf("{Speed:%d,%d,%d}\n",(int)(Speed_num[p]+0.5),m1calc,m2_data[p]);

                                printf("{Speed:%d,%d}\n",(int)Speed_num[p],(int)(kalman_height+0.5));                                        

                        }

                        OLED_ShowNum(0,3,(int)Speed_num[50],4,16);

                        TIM_NUM = 0;

                        ATIM_Cmd(ENABLE);

                        GTIM_Cmd(ENABLE);

                        BTIM_Cmd(CW_BTIM1, ENABLE);

                }         

         }else if(Key_Flag == 1) 

         {

                Plus_Value = CW_ATIM->CNT;

                 printf("{Angle Value: %d}\n",Plus_Value);

                 if( (GetTick() - cnt) >10000)

                 {

                        cnt = GetTick();

                 }

                 OLED_ShowNum(0,3,Plus_Value,4,16);

         }



  }

}

案例中：m1calc为M1,100000为fc,即编码器输出脉冲数。m2_data[p]为M2,即高频脉冲计数。kalmanFilter（）函数为卡尔曼滤波器。

复制

//1. 结构体类型定义

typedef struct 

{

    float LastP;//上次估算协方差 初始化值为0.02

    float Now_P;//当前估算协方差 初始化值为0

    float out;//卡尔曼滤波器输出 初始化值为0

    float Kg;//卡尔曼增益 初始化值为0

    float Q;//过程噪声协方差 初始化值为0.001

    float R;//观测噪声协方差 初始化值为0.543

}KFP;//Kalman Filter parameter



//2. 以速度为例 定义卡尔曼结构体并初始化参数

KFP KFP_height={0.02,0,3000,0,0.001,0.005};



/**

 *卡尔曼滤波器

 *@param KFP *kfp 卡尔曼结构体参数

 *   float input 需要滤波的参数的测量值（即传感器的采集值）

 *[url=home.php?mod=space&uid=266161]@return[/url] 滤波后的参数（最优值）

 */

 float kalmanFilter(KFP *kfp,float input)

 {

     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差

     kfp->Now_P = kfp->LastP + kfp->Q;

     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）

     kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);

     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）

     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值

     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。

     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;

     return kfp->out;

 }