你的浏览器版本过低,可能导致网站不能正常访问!
为了你能正常使用网站功能,请使用这些浏览器。

基于STM32的自动跟踪小车

[复制链接]
STMCU小助手 发布时间:2021-8-5 11:01
概述
小车外形:
1.png


功能简介
利用摄像头识别前车尾部的AprilTag,得到前车位置,传回stm32主控板处理,使两车在行驶时保持恒定距离,实现自动跟车。




openMV4摄像头
1.1 Apriltag识别与串口传输


AprilTag是一个视觉基准库,在AR,机器人,相机校准领域广泛使用。通过特定的标志(与二维码相似,但是降低了复杂度以满足实时性要求),可以快速地检测标志,并计算相对位置。


Apriltag示例:
2.png
通过识别Apriltag,可以得到x,y,z三个方向的距离以及偏移角度。这里只需要三维的距离即可,通过串口传回stm32.
  1. <font face="微软雅黑" size="3">
  2. import sensor, image, time, math,pyb
  3. from pyb import UART

  4. sensor.reset()
  5. sensor.set_pixformat(sensor.RGB565)
  6. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
  7. sensor.skip_frames(time = 2000)
  8. sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
  9. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
  10. clock = time.clock()
  11. uart = UART(3, 115200)#串口波特率

  12. f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
  13. f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
  14. c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
  15. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)

  16. def degrees(radians):
  17.     return (180 * radians) / math.pi

  18. while(True):
  19.     clock.tick()
  20.     img = sensor.snapshot()
  21.     for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
  22.         img.draw_rectangle(tag.rect(), color = (255, 0, 0))
  23.         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
  24.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
  25.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
  26.         # Translation units are unknown. Rotation units are in degrees.
  27.        # print("Tx %f, Ty %f, Tz %f" % print_args)
  28.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式,以便于stm32分割取得数据
  29.         #pyb.delay(500)
  30.    # print(clock.fps())</font>
复制代码
STM32主控板(型号为F407)


2.1 时钟与中断配置


附上stm32时钟示意图:
3.png
定时器示意图:
4.png
定时器分配:
5.png
所有时钟初始化的函数:(每个函数的详细内容在后面)
  1. <font face="微软雅黑" size="3">
  2. TIM8_PWM_Init(400-1,20-1);  //用于控制电机,168M/20=8.4Mhz的计数频率,重装载值400,所以PWM频率为 8.4M/400=21Khz.
  3.   TIM3_PWM_Init(200-1,8400-1);//用于控制舵机,50HZ
  4.   TIM2_Int_Init(400-1,20-1);//定时中断,21KHZ
  5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数,20HZ,50ms中断一次
  6.   uart_init(115200);  //初始化串口1波特率为115200
  7.   Encoder_Init_TIM4();//编码器接口初始化</font>
复制代码
2.2 串口收发与数据处理


串口中断:USART1,USART2
串口初始化函数(以USART1为例):
  1. <font face="微软雅黑" size="3">
  2. void uart_init(u32 bound){
  3.    //GPIO端口设置
  4.   GPIO_InitTypeDef GPIO_InitStructure;
  5.   USART_InitTypeDef USART_InitStructure;
  6.   NVIC_InitTypeDef NVIC_InitStructure;
  7.   
  8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
  9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟

  10.   //串口1对应引脚复用映射
  11.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
  12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
  13.   
  14.   //USART1端口配置
  15.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
  16.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
  17.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
  18.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
  19.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
  20.   GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9,PA10

  21.    //USART1 初始化设置
  22.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
  23.   USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
  24.   USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
  25.   USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
  26.   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
  27.   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;  //收发模式
  28.   USART_Init(USART1, &USART_InitStructure); //初始化串口1
  29.   
  30.   USART_Cmd(USART1, ENABLE);  //使能串口1
  31.   
  32.   USART_ClearFlag(USART1, USART_FLAG_TC);
  33.   
  34. #if EN_USART1_RX  
  35.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断

  36.   //Usart1 NVIC 配置
  37.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
  38.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
  39.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
  40.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
  41.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、

  42. #endif
  43.   
  44. }</font>
复制代码
串口中断处理函数:
  1. <font face="微软雅黑" size="3">
  2. void USART1_IRQHandler(void)                  //串口1中断服务程序
  3. {
  4.   u8 Res;
  5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
  6.   OSIntEnter();   
  7. #endif
  8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
  9.   {
  10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
  11.    
  12.     if((USART_RX_STA&0x8000)==0)//接收未完成
  13.     {
  14.       if(USART_RX_STA&0x4000)//接收到了0x0d
  15.       {
  16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
  17.         else USART_RX_STA|=0x8000;  //接收完成了
  18.       }
  19.       else //还没收到0X0D
  20.       {  
  21.         if(Res==0x0d)USART_RX_STA|=0x4000;
  22.         else
  23.         {
  24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
  25.           USART_RX_STA++;
  26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
  27.         }     
  28.       }
  29.     }        
  30.   }
  31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
  32.   OSIntExit();                        
  33. #endif
  34. }
  35. #endif  </font>
复制代码
字符串接收与处理(从openMV接收到的数据):
  1. <font face="微软雅黑" size="3">/*涉及到的全局变量
  2. float data[3];//x,y,z方向的距离,浮点数形式
  3. unsigned char data_string[3][7];//x,y,z方向的距离,字符串形式
  4. */
  5. if(USART_RX_STA&0x8000)
  6.     {  
  7.       //清空字符串
  8.       for(i=0;i<2;i++)
  9.       {
  10.         for(j=0;j<6;j++)
  11.         {
  12.           data_string[i][j]=' ';
  13.         }
  14.       }
  15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
  16.       for(t=0,j=0;t<len;t++)
  17.       {
  18.         j=0;
  19.         if(USART_RX_BUF[t]=='A')
  20.         {
  21.           t++;//去掉首字母
  22.           while(USART_RX_BUF[t]!=',')
  23.           {
  24.             data_string[0][j]=USART_RX_BUF[t];
  25.             t++;
  26.             j++;
  27.           }
  28.         }
  29.         if(USART_RX_BUF[t]=='B')
  30.         {
  31.           t++;//去掉首字母
  32.           while(USART_RX_BUF[t]!=',')
  33.           {
  34.             data_string[1][j]=USART_RX_BUF[t];
  35.             t++;
  36.             j++;
  37.           }
  38.         }
  39.         if(USART_RX_BUF[t]=='C')
  40.         {
  41.           t++;//去掉首字母
  42.           while(USART_RX_BUF[t]!=',')
  43.           {
  44.             data_string[2][j]=USART_RX_BUF[t];
  45.             t++;
  46.             j++;
  47.           }
  48.         }
  49.       }
  50.       //把字符串转化为浮点数
  51.       data[0]=myatof(data_string[0])/100.0;//x
  52.       data[1]=myatof(data_string[1])/100.0;//y
  53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z,输入是负数,转换为正
  54.         //USART2发送数据
  55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
  56.         
  57.       //LCD更新显示
  58.       //显示xyz
  59. //        CLEAR(10);
  60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
  61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
  62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
  63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
  64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
  65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
  66.         
  67.         USART_RX_STA=0;//清除标志位
  68.     }
  69. }
  70. </font>
复制代码
字符串转化为两位小数浮点数(用于后续PID控制):

  1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数,返回的是乘100后的int值,因float返回有错误
  2. {
  3.   int flag = 1;//表示正数
  4.   int res =0;
  5.   u8 i=1; //小数点后两位
  6.   while(*str != '\0')
  7.     {
  8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
  9.       {
  10.         str++;
  11.         continue;
  12.       }
  13.       if(*(str-1) == '-')
  14.       {
  15.         flag=-1;//表示是一个负数
  16.       }

  17.     while(*str >= '0' && *str <= '9')
  18.     {
  19.       res = res *10 + (*str - '0');
  20.       str++;
  21.     }
  22.     if(*str == '.')
  23.     {
  24.       str++;
  25.       res = res *10 + (*str - '0');
  26.       str++;
  27.       res = res *10 + (*str - '0');//保留两位,故加两次
  28.       return res*flag;
  29.     }
  30.     }
  31. }


  32. 2.3 LCD显示模块

  33. LCD模块用于调试时观察数据,调试完成可以删去,因为显示屏很耗时,使处理速度变慢
  34. 驱动函数总览:



  35. void LCD_GPIO_Init(void);
  36. void Lcd_WriteIndex(u8 Index);
  37. void Lcd_WriteData(u8 Data);
  38. void Lcd_WriteReg(u8 Index,u8 Data);
  39. u16 Lcd_ReadReg(u8 LCD_Reg);
  40. void Lcd_Reset(void);
  41. void Lcd_Init(void);
  42. void Lcd_Clear(u16 Color);
  43. void Lcd_SetXY(u16 x,u16 y);
  44. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
  45. unsigned int Lcd_ReadPoint(u16 x,u16 y);
  46. void Lcd_SetRegion(u16 x_start,u16 y_start,u16 x_end,u16 y_end);
  47. void LCD_WriteData_16Bit(u16 Data);</font>
复制代码


TFT屏幕初始化:

  1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
  2. {
  3.   Lcd_Clear(WHITE);
  4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
  5.   delay_ms(1000);
  6.   Lcd_Clear(WHITE);
  7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
  8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
  9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
  10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
  11. }</font>
复制代码
字符串显示函数;
  1. <font face="微软雅黑" size="3">
  2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
  3. {
  4.   unsigned char i,j;
  5.   unsigned short k,x0;
  6.   x0=x;

  7.   while(*s)
  8.   {  
  9.     if((*s) < 128)
  10.     {
  11.       k=*s;
  12.       if (k==13)
  13.       {
  14.         x=x0;
  15.         y+=16;
  16.       }
  17.       else
  18.       {
  19.         if (k>32) k-=32; else k=0;
  20.   
  21.           for(i=0;i<16;i++)
  22.         for(j=0;j<8;j++)
  23.           {
  24.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
  25.             else
  26.             {
  27.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
  28.             }
  29.           }
  30.         x+=8;
  31.       }
  32.       s++;
  33.     }
  34.       
  35.     else
  36.     {
  37.    

  38.       for (k=0;k<hz16_num;k++)
  39.       {
  40.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
  41.         {
  42.             for(i=0;i<16;i++)
  43.             {
  44.             for(j=0;j<8;j++)
  45.               {
  46.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
  47.                 else {
  48.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
  49.                 }
  50.               }
  51.             for(j=0;j<8;j++)
  52.               {
  53.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
  54.                 else
  55.                 {
  56.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
  57.                 }
  58.               }
  59.             }
  60.         }
  61.         }
  62.       s+=2;x+=16;
  63.     }
  64.    
  65.   }
  66. }
  67. </font>
复制代码
2.4 电机、舵机与编码器


定时中断:TIM2,用于修改电机和舵机的PWM占空比
初始化函数:

  1. <font face="微软雅黑" size="3">
  2. //通用定时器2中断初始化
  3. //arr:自动重装值。
  4. //psc:时钟预分频数
  5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
  6. //Ft=定时器工作频率,单位:Mhz
  7. //这里使用的是定时器2!
  8. void TIM2_Int_Init(u16 arr,u16 psc)
  9. {
  10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
  11.   NVIC_InitTypeDef NVIC_InitStructure;
  12.   
  13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
  14.   
  15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
  16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
  17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
  18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
  19.   
  20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
  21.   
  22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
  23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
  24.   
  25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
  26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
  27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
  28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
  29.   NVIC_Init(&NVIC_InitStructure);

  30. }</font>
复制代码
TIM2中断处理函数:
  1. <font face="微软雅黑" size="3">
  2. void TIM2_IRQHandler(void)
  3. {  
  4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
  5.   {
  6.     if(motor_flag==1)//反转
  7.     {
  8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC6
  9.       TIM_SetCompare2(TIM8,0);
  10.     }
  11.     if(motor_flag==0)//正转
  12.     {
  13.       TIM_SetCompare1(TIM8,0);
  14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC7
  15.     }
  16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度,引脚:PA6
  17.   }
  18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
  19. }
  20. </font>
复制代码
PWM输出:TIM3(舵机),TIM8(电机)
初始化函数(以TIM8为例):

  1. <font face="微软雅黑" size="3">
  2. void TIM8_PWM_Init(u32 arr,u32 psc)
  3. {               
  4.   //此部分需手动修改IO口设置
  5.   
  6.   GPIO_InitTypeDef GPIO_InitStructure;
  7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
  9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
  10.   
  11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
  12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
  13.   
  14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
  15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
  16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
  17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
  18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
  19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7

  20.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
  21.   GPIO_Init(GPIOB,&GPIO_InitStructure);
  22.   
  23.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
  24.   GPIO_Init(GPIOC,&GPIO_InitStructure);
  25.   
  26.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
  27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
  28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
  29.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
  30.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
  31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1

  32.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
  33.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
  34.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
  35.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
  36.   
  37.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
  38.   
  39.   //初始化PWM模式   
  40.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  41.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  42.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  43.   TIM_OCInitStructure.TIM_Pulse = 0;
  44.   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  45.   TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
  46.   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
  47.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;

  48.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
  49.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
  50.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);

  51.   TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);
  52.   TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);
  53.   TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);

  54.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
  55.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
  56.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
  57.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
  58.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
  59.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
  60.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
  61.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);

  62.   TIM_Cmd(TIM8,ENABLE);
  63.   TIM_CCPreloadControl(TIM8,ENABLE);
  64.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
  65.                      
  66. }</font>
复制代码
编码器初始化函数:
  1. <font face="微软雅黑" size="3">
  2. void Encoder_Init_TIM4(void)
  3. {
  4.     GPIO_InitTypeDef         GPIO_InitStructure;
  5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
  7.     NVIC_InitTypeDef NVIC_InitStructure;

  8.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
  9.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
  10.   
  11.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
  12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用

  13.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
  14.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
  15.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  16.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
  17.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
  18.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
  19.     GPIO_Init(GPIOB,&GPIO_InitStructure);
  20.    
  21.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
  22.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
  23.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  24.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断,防止计数溢出而没有相应函数,造成卡死
  25.     NVIC_Init(&NVIC_InitStructure);



  26.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
  27.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
  28.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
  29.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
  30.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

  31.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
  32.     TIM_ICStructInit(&TIM_ICInitStructure);
  33.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
  34.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
  35.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
  36.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
  37.     TIM4->CNT = 0;
  38.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
  39. }
  40. </font>
复制代码
编码器返回速度值:
  1. <font face="微软雅黑" size="3">/**************************************************************************
  2. 函数功能:单位时间读取编码器计数
  3. 入口参数:定时器
  4. 返回  值:速度值
  5. **************************************************************************/
  6. float Read_Encoder_Speed(uint8_t TIMX)
  7. {
  8.     int32_t Encoder_TIM;
  9.     float res = 0;
  10.     switch (TIMX)
  11.     {
  12.     case 5:
  13.         Encoder_TIM = TIM_GetCounter(TIM5);
  14.         TIM5->CNT = ENCODER_BASE_COUNT;
  15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
  16.         break;
  17.     case 4:
  18.         Encoder_TIM = TIM_GetCounter(TIM4);
  19.         TIM4->CNT = ENCODER_BASE_COUNT;
  20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
  21.         break;
  22.     default:
  23.         Encoder_TIM = 0;
  24.         res = 0;
  25.     }
  26.     if(res>2048.0f)
  27.         res-=4096.0f;
  28.     return res*360.0f/4096.0f;
  29. }
  30. </font>
复制代码
定时从编码器取数,注意,时间不一样,取回的数值也不一样,取决于实际速度以及编码器线数。这里50ms取一次:
  1. <font face="微软雅黑" size="3">
  2. void TIM7_IRQHandler(void)//频率20HZ,用于编码器计数
  3. {  
  4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
  5.   {
  6.     speed=Read_Encoder_Speed(4);
  7.   }
  8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
  9. }</font>
复制代码
2.5 PID控制


PID库函数:

  1. <font face="微软雅黑" size="3">
  2. #define N 2        //需要对多少变量进行pid调节

  3. const float KP[N]={1.3,1.0};//这里只用了比例调节
  4. const float KI[N]={0,0};
  5. const float KD[N]={0,0};


  6. struct _pid{
  7.   float SetVol;        //定义设定值
  8.   float ActVol;        //定义实际值
  9.   float Err;          //定义误差
  10.   float  Err_Next;      //定义上一个误差
  11.   float  Err_Last;      //定义上上一个误差
  12.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
  13.   float integral;      //定义积分值
  14.   float actuator;      //定义控制器执行变量
  15. }pid[N];

  16. void PID_Init(void)
  17. {
  18.   for(int i=0;i<N;i++)
  19.   {
  20.     pid[i].SetVol=0.0;
  21.     pid[i].ActVol=0.0;
  22.     pid[i].Err=0.0;
  23.     pid[i].Err_Next=0.0;
  24.     pid[i].Err_Last=0.0;
  25.     pid[i].integral=0.0;
  26.     pid[i].actuator=0.0;
  27.     pid[i].Kp=KP[i];
  28.     pid[i].Ki=KI[i];
  29.     pid[i].Kd=KD[i];
  30.   }
  31. }

  32. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
  33. {
  34.   pid[i].SetVol=set_val;
  35.   pid[i].ActVol=get_val;
  36.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
  37.   float IncVol;    //定义增量
  38.   pid[i].integral+=pid[i].Err;
  39. //  IncVol=pid[i].Kp*(pid[i].Err-pid[i].Err_Next)+pid[i].Ki*pid[i].Err+pid[i].Kd*(pid[i].Err-2*pid[i].Err_Next+pid[i].Err_Last);
  40.   pid[i].actuator=pid[i].Kp* pid[i].Err+pid[i].Ki*pid[i].integral+pid[i].Kd*(pid[i].Err-pid[i].Err_Next);
  41. //  pid[i].actuator=adc_val+IncVol;
  42.   pid[i].ActVol=pid[i].actuator;
  43.   pid[i].Err_Last=pid[i].Err_Next;
  44.   pid[i].Err_Next=pid[i].Err;
  45.   
  46.   return pid[i].actuator;
  47. }</font>
复制代码
主函数中的PID调节:

  1. <font face="微软雅黑" size="3">
  2.     z_get=data[2];
  3.     x_get=data[0];
  4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
  5.     {
  6.       LED1=0;  //调节时灯亮
  7.       PID_val_motor=PID_realize(z_set,z_get,0);
  8.       PID_val_motor=PID_val_motor/10.0;
  9.       if(PID_val_motor<=0)
  10.         motor_flag=0;//motor_flag控制电机正反转,PID_val_motor用于改变占空比,范围0~1
  11.       if(PID_val_motor>0)
  12.         motor_flag=1;
  13.       PID_val_motor=abs_float(PID_val_motor);
  14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远,让车停止
  15.       if(PID_val_motor>1&&PID_val_motor<=2)PID_val_motor=1;
  16.       if(PID_val_motor<0.2)PID_val_motor=0;
  17.     }
  18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
  19.     {
  20.       LED1=0;  
  21.       PID_val_servo=PID_realize(x_set,x_get,1);
  22.       servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射,把PID的值转化为角度35~140的舵机转角
  23.       if(servo_angle<35)servo_angle=35;
  24.       if(servo_angle>140)servo_angle=140;
  25.     }
  26.     LED1=1;</font>
复制代码
定时器TIM2中断里改变占空比:
  1. <font face="微软雅黑" size="3">
  2. void TIM2_IRQHandler(void)
  3. {  
  4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
  5.   {
  6.     if(motor_flag==1)//反转
  7.     {
  8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC6
  9.       TIM_SetCompare2(TIM8,0);
  10.     }
  11.     if(motor_flag==0)//正转
  12.     {
  13.       TIM_SetCompare1(TIM8,0);
  14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC7
  15.     }
  16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度,根据舵机手册得到占空比与转角的关系,引脚:PA6
  17.   }
  18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
  19. }
  20. </font>
复制代码



收藏 1 评论1 发布时间:2021-8-5 11:01

举报

1个回答
sujinfu 回答时间:2021-8-5 15:06:10
优秀

所属标签

相似分享

官网相关资源

关于
我们是谁
投资者关系
意法半导体可持续发展举措
创新与技术
意法半导体官网
联系我们
联系ST分支机构
寻找销售人员和分销渠道
社区
媒体中心
活动与培训
隐私策略
隐私策略
Cookies管理
行使您的权利
官方最新发布
STM32N6 AI生态系统
STM32MCU,MPU高性能GUI
ST ACEPACK电源模块
意法半导体生物传感器
STM32Cube扩展软件包
关注我们
st-img 微信公众号
st-img 手机版