找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 4897|回复: 2
打印 上一主题 下一主题
收起左侧

STM32的PID算法调节电机角度与调角程序

[复制链接]
跳转到指定楼层
楼主
ID:407610 发表于 2019-3-12 22:07 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
不足之处欢迎指出

单片机源程序如下:
  1. #include "pid.h"
  2. #include "stm32f10x.h"

  3. #define pwmout_0 GPIO_SetBits(GPIOB,GPIO_Pin_5)
  4. #define pwmout_1 GPIO_ResetBits(GPIOB,GPIO_Pin_5)

  5. PID pid;
  6. float DIY_Pulse=0;


  7. void PID_Init()
  8. {
  9.         pid.sv=60;
  10.         pid.Kp=20;
  11.         pid.T=1;
  12.         pid.Ti=50;
  13.         pid.Td=0;
  14.         pid.pwmcycle=1000;//pwm周期
  15.         pid.OUT0=1;
  16.        
  17.        
  18.         pid.T2=100;
  19.         pid.jiao=100;
  20.         pid.Kp2=20;
  21.         pid.Ti2=0;
  22.         pid.Td2=0;
  23. }

  24. void PIDOUT_Init()   
  25. {
  26. GPIO_InitTypeDef  GPIO_InitStructure;       
  27. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);         //使能PB,PE端口时钟
  28. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;                                 //LED0-->PB.5 端口配置
  29. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                  //推挽输出
  30. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                 //IO口速度为50MHz
  31. GPIO_Init(GPIOB, &GPIO_InitStructure);                                         //根据设定参数初始化GPIOB.5
  32. GPIO_ResetBits(GPIOB,GPIO_Pin_6);                                                 //PB.5 输出高
  33.        
  34.        
  35. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;                             //LED1-->PE.5 端口配置, 推挽输出
  36. GPIO_Init(GPIOB, &GPIO_InitStructure);                                           //推挽输出 ,IO口速度为50MHz
  37. GPIO_SetBits(GPIOB,GPIO_Pin_7);                                                  //PE.5 输出高
  38.        
  39. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;                             //LED1-->PE.5 端口配置, 推挽输出
  40. GPIO_Init(GPIOB, &GPIO_InitStructure);                                           //推挽输出 ,IO口速度为50MHz
  41. GPIO_ResetBits(GPIOB,GPIO_Pin_8);                                                  //PE.5 输出高
  42. }

  43. void PID_Calc() //PID计算
  44. {
  45.         float DelEk;
  46.         float ti,ki;
  47.         float Iout;
  48.         float Pout;
  49.         float Dout;
  50.         float td;
  51.         float kd;
  52.        
  53.        
  54.         if(pid.C10ms<(pid.T))//计算周期未到
  55.         {
  56.                 return ;
  57.         }
  58.         pid.Ek=pid.sv-pid.pv;
  59.         Pout=pid.Kp*pid.Ek; //比例输出
  60.         pid.SEK+=pid.Ek; //历史偏差和
  61.         DelEk=pid.Ek-pid.Ek_1;  //相邻两次偏差差值
  62.         ti=pid.T/pid.Ti;  
  63.         ki=ti*pid.Kp;
  64.         Iout=pid.SEK*pid.Kp*ti;   //积分输出
  65.         td=pid.Td/pid.T;
  66.         kd=pid.Kp*td;
  67.         Dout=DelEk*kd; //微分输出
  68.        
  69.         pid.OUT=Pout+Iout+Dout+pid.OUT0;//本次输出脉宽
  70.        
  71.         if(pid.OUT>pid.pwmcycle)
  72.         {
  73.         pid.OUT=pid.pwmcycle;
  74.         }
  75.         if(pid.OUT<0)
  76.         {
  77.                 pid.OUT=0;
  78.         }
  79.         pid.Ek_1=pid.Ek;
  80.        
  81.                 pid.pv=encoder;
  82.             encoder=0;
  83.        
  84.         pid.C10ms=0;
  85. }

  86. void PID_Calc2() //PID计算
  87. {
  88.         float DelEk;
  89.         float ti,ki;
  90.         float Iout;
  91.         float Pout;
  92.         float Dout;
  93.         float td;
  94.         float kd;
  95.        
  96.        
  97.         if(pid.C100ms<(pid.T2))//计算周期未到
  98.         {
  99.                 return ;
  100.         }
  101.         pid.Ek2=pid.jiao-pid.jpv;
  102.         Pout=pid.Kp2*pid.Ek2; //比例输出
  103.         pid.SEK2+=pid.Ek2; //历史偏差和
  104.         DelEk=pid.Ek2-pid.Ek_12;  //相邻两次偏差差值
  105.         ti=pid.T2/pid.Ti2;  
  106.         ki=ti*pid.Kp2;
  107.         Iout=pid.SEK2*pid.Kp2*ti;   //积分输出
  108.         td=pid.Td2/pid.T2;
  109.         kd=pid.Kp2*td;
  110.         Dout=DelEk*kd; //微分输出
  111.        
  112.         pid.OUT1=Pout+Iout+Dout+pid.OUT0;//本次输出脉宽
  113.        
  114.         if(pid.OUT1>pid.pwmcycle)
  115.         {
  116.         pid.OUT1=pid.pwmcycle;
  117.         }
  118.         if(pid.OUT1<0)
  119.         {
  120.                 pid.OUT1=0;
  121.         }
  122.         pid.Ek_12=pid.Ek2;
  123.        
  124.                 pid.jpv=jiao;
  125.           
  126.        
  127.         pid.C100ms=0;
  128. }

  129. int myabs(int a)
  130. {                   
  131.           int temp;
  132.                 if(a<0)  temp=-a;  
  133.           else temp=a;
  134.           return temp;
  135. }

  136. //void XCHG_Pid(float Pulse)
  137. //{
  138. //        TIM_OCInitTypeDef  TIM_OCInitStructure;
  139. //       
  140. //        TIM_OCInitStructure.TIM_Pulse = Pulse;
  141. //        TIM_OC2Init(TIM3, &TIM_OCInitStructure);
  142. //}

  143. void PID_out(void)
  144. {   
  145.            static u16 pw;
  146.             pw++;
  147.        
  148. //        PID_Calc();
  149.         if(pw>pid.pwmcycle)
  150.         {
  151.                 pw=0;
  152.         }
  153. //        if(pid.OUT>0)                AIN2=1,                        AIN1=0;
  154. //                        else                 AIN2=0,                        AIN1=1;
  155.         if(pw<pid.OUT)
  156.         {
  157.                 DIY_Pulse=DIY_Pulse-0.1;
  158.                 TIM3->CCR2=DIY_Pulse;
  159.                 //XCHG_Pid(DIY_Pulse);
  160.                 //pid.jiao++;
  161.                     //pwmout_0;  //加?
  162.         //PWM1=1;
  163.         }
  164.         else
  165.         {
  166.                 DIY_Pulse=DIY_Pulse+0.1;
  167.                 TIM3->CCR2=DIY_Pulse;
  168.                 //XCHG_Pid(DIY_Pulse);
  169.                
  170.           // pwmout_1;   //减速
  171.                 //pid.jiao--;
  172.                 //PWM1=0;
  173.         }
  174. }

  175. void MiniBalance_PWM_Init(u16 arr,u16 psc)
  176. {       
  177.         GPIO_InitTypeDef GPIO_InitStructure;
  178.         TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  179.         TIM_OCInitTypeDef  TIM_OCInitStructure;



  180.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//        
  181.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB  | RCC_APB2Periph_AFIO, ENABLE);  //使能GPIO外设和AFIO复用功能模块时钟
  182.        
  183.         GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE); //Timer3部分重映射  TIM3_CH2->PB5
  184.        
  185.    //设置该引脚为复用输出功能,输出TIM2 CH1~ CH4的PWM脉冲波形
  186.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5; //TIM_CH1 ~TIM_CH4
  187.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
  188.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  189.         GPIO_Init(GPIOB, &GPIO_InitStructure);
  190.        
  191.         TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值         
  192.         TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值  不分频
  193.         TIM_TimeBaseStructure.TIM_ClockDivision =TIM_CKD_DIV1 ; //设置时钟分割:TDTS = Tck_tim
  194.         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
  195.         TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位


  196.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1
  197.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
  198.         TIM_OCInitStructure.TIM_Pulse =DIY_Pulse;                           //设置待装入捕获比较寄存器的脉冲值
  199.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;     //输出极性:TIM输出比较极性高
  200.        
  201.         TIM_OC2Init(TIM3, &TIM_OCInitStructure);  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
  202.        

  203. //                TIM_CtrlPWMOutputs(TIM2,ENABLE);        //MOE 主输出使能       

  204.        
  205.         TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);  //CH2预装载使能         
  206.        
  207.         TIM_ARRPreloadConfig(TIM3, ENABLE); //使能TIMx在ARR上的预装载寄存器
  208. //        TIM_CtrlPWMOutputs(TIM3, ENABLE);
  209.         TIM_Cmd(TIM3, ENABLE);  //使能TIM2

  210. }

  211. //int transfer(int encoder)
  212. //{
  213. //        int temp;
  214. //        temp=encoder;
  215. //        encoder=0;                                                //清零是为了算速度 不清零则是算位置
  216. //        return temp;
  217. //}

  218. void MiniBalance_EXTI_Init(void)
  219. {  
  220.         GPIO_InitTypeDef GPIO_InitStructure;
  221.        
  222.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//外部中断,需要使能AFIO时钟
  223.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能GPIO端口时钟
  224.        
  225.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;                    //端口配置
  226.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;         //上拉输入
  227.         GPIO_Init(GPIOA, &GPIO_InitStructure);                                              //根据设定参数初始化GPIO
  228.        
  229.           Exit_Init();
  230. }


  231. void Exit_Init(void)
  232. {
  233.         EXTI_InitTypeDef EXTI_InitStructure;
  234.         NVIC_InitTypeDef NVIC_InitStructure;
  235.         /*====PA.11====*/
  236.         GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource3);
  237.        
  238.           EXTI_InitStructure.EXTI_Line=EXTI_Line3;
  239.           EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;       
  240.           EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;                                                /*上升沿触发*/
  241.           EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  242.           EXTI_Init(&EXTI_InitStructure);                 /*根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器*/

  243.     NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
  244.           NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;                        /*抢占优先级2, */
  245.           NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;                                                 /*子优先级1*/
  246.           NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                                                   /*使能外部中断通道*/
  247.           NVIC_Init(&NVIC_InitStructure);
  248.        
  249.        
  250.          
  251. }

  252. void EXTI3_IRQHandler()
  253. {
  254.                 if(SET == EXTI_GetITStatus(EXTI_Line3))//PB3 右后A相
  255.                 {
  256.                        
  257. //                        if(RB)
  258. //                        {                               
  259.                                 encoder++;
  260.                          if(pid.OUT1>80) jiao++;
  261.                           else  jiao--;
  262. //                        }
  263. //                        else  encoder--;
  264.                        
  265.                         EXTI_ClearITPendingBit(EXTI_Line3);
  266.                 }
  267. }
复制代码
  1. #include "stm32f10x.h"  
  2. #include "pid.h"
  3. #include "timer.h"
  4. #include "bsp_key.h"
  5. #include "bsp_ili9341_lcd.h"
  6. #include "bsp_spi_flash.h"
  7. #include "bsp_usart.h"
  8. #include "delay.h"
  9. #include <stdio.h>


  10. extern PID pid;
  11. static void LCD_Test(void);
  12. static void Delay ( __IO uint32_t nCount );
  13. void Printf_Charater(void)   ;
  14. u32 encoder;
  15. u32 jiao;

  16. void LED_Init(void)
  17. {

  18. GPIO_InitTypeDef  GPIO_InitStructure;
  19.        
  20. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOE, ENABLE);         //使能PB,PE端口时钟
  21.        
  22. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;                                 //LED0-->PB.5 端口配置
  23. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                  //推挽输出
  24. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                 //IO口速度为50MHz
  25. GPIO_Init(GPIOB, &GPIO_InitStructure);                                         //根据设定参数初始化GPIOB.5
  26. GPIO_SetBits(GPIOB,GPIO_Pin_5);                                                 //PB.5 输出高
  27. }

  28. int main(void)
  29. {
  30.         delay_init();                     //延时函数初始化          

  31.         LED_Init();                             //LED端口初始化
  32.         PID_Init();
  33.   PIDOUT_Init();
  34.         ILI9341_Init ();         //LCD 初始化
  35.         Key_GPIO_Config();
  36.        
  37.         TIM4_Int_Init(7199,0);  //
  38.         MiniBalance_PWM_Init(7199,0);  //输出PWM波
  39.         MiniBalance_EXTI_Init();
  40.         ILI9341_GramScan ( 6 );
  41.         while(1)
  42.         {
  43.                 LCD_Test();
  44.                 PID_Calc();
  45.                
  46.                         if( Key_Scan(KEY1_GPIO_PORT,KEY1_GPIO_PIN) == KEY_ON  )
  47.                 {
  48.                            pid.sv=pid.sv+10;
  49.                 }
  50.                 if( Key_Scan(KEY2_GPIO_PORT,KEY2_GPIO_PIN) == KEY_ON  )
  51.                 {
  52.                         pid.sv=pid.sv-10;
  53.                 }
  54.                 if( KEY3==0 )
  55.                 {
  56.                           AIN1=1,AIN2=0;  
  57.                
  58.                         while(!KEY3);
  59.                 }
  60.                 if( KEY4==0 )
  61.                 {
  62.                           AIN1=0,AIN2=1;
  63.                         while(!KEY4);
  64.                 }
  65.                 if( KEY5==0 )
  66.                 {
  67.                           AIN1=0,AIN2=0;

  68.                         while(!KEY5);
  69.                 }
  70.                
  71.                         if( KEY6==0 )
  72.                 {
  73.                         jiao=0;
  74.                         while(1)
  75.                         {
  76.                                 pid.OUT=0;
  77.                                 pid.sv=10;
  78.                         PID_Calc();       
  79.                         PID_Calc2();
  80.                         LCD_Test();
  81.                         if(pid.OUT1>80)
  82.                         {
  83.                                 AIN1=1,AIN2=0;
  84.                         }
  85.                         else
  86.                                 AIN1=0,AIN2=1;
  87.                   
  88.       if(pid.jpv==pid.jiao)
  89.                                 {
  90.                                         AIN1=0,AIN2=0;
  91.                                         break;
  92.                         }                               
  93.                 }
  94.                 }


  95.         }
  96. }

  97. void LCD_Test(void)
  98. {
  99.         /*演示显示变量*/
  100.         char sv[100];
  101.         char pv[100];
  102.         char OUT[100];
  103.         char jiao[100];
  104.         char jpv[100];
  105.   char OUT1[100];
  106.         char disbuff[100];
  107.        
  108.         LCD_SetFont(&Font8x16);
  109.         LCD_SetColors(RED,BLACK);

  110.   ILI9341_Clear(0,0,LCD_X_LENGTH,LCD_Y_LENGTH);        /* 清屏,显示全黑 */
  111.         /********显示字符串示例*******/
  112.   ILI9341_DispStringLine_EN_CH(LINE(0),"野火3.2寸LCD参数:");
  113.   ILI9341_DispStringLine_EN_CH(LINE(1),"分辨率:240x320 px");
  114.   ILI9341_DispStringLine_EN_CH(LINE(2),"ILI9341液晶驱动");
  115.   
  116.         /********显示变量示例*******/
  117.         LCD_SetFont(&Font16x24);
  118.         LCD_SetTextColor(GREEN);

  119.         /*使用c标准库把变量转化成字符串*/
  120.         sprintf(sv,"设定速度: %f ",pid.sv);
  121.         sprintf(pv,"实际速度 : %f ",pid.pv);
  122.         sprintf(OUT,"pid输出: %f ",pid.OUT);
  123.         sprintf(jiao,"设定角度: %f ",pid.jiao);
  124.   sprintf(jpv,"实际角度: %f ",pid.jpv);
  125.         sprintf(OUT1,"pid输出: %f ",pid.OUT1);
  126.        
  127.   LCD_ClearLine(LINE(4));        /* 清除单行文字 */
  128.         LCD_ClearLine(LINE(5));        /* 清除单行文字 */
  129.         LCD_ClearLine(LINE(6));        /* 清除单行文字 */
  130.   LCD_ClearLine(LINE(7));        /* 清除单行文字 */
  131.   LCD_ClearLine(LINE(8));        /* 清除单行文字 */
  132.         LCD_ClearLine(LINE(9));        /* 清除单行文字 */
  133.        
  134.         /*然后显示该字符串即可,其它变量也是这样处理*/
  135.         ILI9341_DispStringLine_EN_CH(LINE(4),sv);
  136.         ILI9341_DispStringLine_EN_CH(LINE(5),pv);
  137.         ILI9341_DispStringLine_EN_CH(LINE(6),OUT);
  138.         ILI9341_DispStringLine_EN_CH(LINE(7),jiao);
  139.   ILI9341_DispStringLine_EN_CH(LINE(8),jpv);
  140.         ILI9341_DispStringLine_EN_CH(LINE(9),OUT1);

  141. }

复制代码

所有资料51hei提供下载:
PID算法.7z (227.61 KB, 下载次数: 135)


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏4 分享淘帖 顶 踩
回复

使用道具 举报

沙发
ID:1 发表于 2019-3-12 22:25 | 只看该作者
本帖需要重新编辑补全电路原理图,源码,详细说明与图片即可获得100+黑币(帖子下方有编辑按钮)
回复

使用道具 举报

板凳
ID:593177 发表于 2019-7-31 18:36 | 只看该作者
感谢分享
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表