设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › STM32的PID算法调节电机角度与调角程序
返回列表 发新帖
查看: 5712|回复: 2
打印 上一主题 下一主题
收起左侧

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

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

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

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

  7. PID pid;
  8. float DIY_Pulse=0;


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

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

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

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

  137. int myabs(int a)
  138. {                   
  139.           int temp;
  140.                 if(a<0)  temp=-a;  
  141.           else temp=a;
  142.           return temp;
  143. }

  145. //void XCHG_Pid(float Pulse)
  146. //{
  147. //        TIM_OCInitTypeDef  TIM_OCInitStructure;
  148. //       
  149. //        TIM_OCInitStructure.TIM_Pulse = Pulse;
  150. //        TIM_OC2Init(TIM3, &TIM_OCInitStructure);
  151. //}

  153. void PID_out(void)
  154. {   
  155.            static u16 pw;
  156.             pw++;
  157.        
  158. //        PID_Calc();
  159.         if(pw>pid.pwmcycle)
  160.         {
  161.                 pw=0;
  162.         }
  163. //        if(pid.OUT>0)                AIN2=1,                        AIN1=0;
  164. //                        else                 AIN2=0,                        AIN1=1;
  165.         if(pw<pid.OUT)
  166.         {
  167.                 DIY_Pulse=DIY_Pulse-0.1;
  168.                 TIM3->CCR2=DIY_Pulse;
  169.                 //XCHG_Pid(DIY_Pulse);
  170.                 //pid.jiao++;
  171.                     //pwmout_0;  //加?
  172.         //PWM1=1;
  173.         }
  174.         else
  175.         {
  176.                 DIY_Pulse=DIY_Pulse+0.1;
  177.                 TIM3->CCR2=DIY_Pulse;
  178.                 //XCHG_Pid(DIY_Pulse);
  179.                
  180.           // pwmout_1;   //减速
  181.                 //pid.jiao--;
  182.                 //PWM1=0;
  183.         }
  184. }

  186. void MiniBalance_PWM_Init(u16 arr,u16 psc)
  187. {       
  188.         GPIO_InitTypeDef GPIO_InitStructure;
  189.         TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  190.         TIM_OCInitTypeDef  TIM_OCInitStructure;



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


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

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

  222.        
  223.         TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);  //CH2预装载使能         
  224.        
  225.         TIM_ARRPreloadConfig(TIM3, ENABLE); //使能TIMx在ARR上的预装载寄存器
  226. //        TIM_CtrlPWMOutputs(TIM3, ENABLE);
  227.         TIM_Cmd(TIM3, ENABLE);  //使能TIM2

  229. }

  231. //int transfer(int encoder)
  232. //{
  233. //        int temp;
  234. //        temp=encoder;
  235. //        encoder=0;                                                //清零是为了算速度 不清零则是算位置
  236. //        return temp;
  237. //}

  239. void MiniBalance_EXTI_Init(void)
  240. {  
  241.         GPIO_InitTypeDef GPIO_InitStructure;
  242.        
  243.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//外部中断，需要使能AFIO时钟
  244.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能GPIO端口时钟
  245.        
  246.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;                    //端口配置
  247.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;         //上拉输入
  248.         GPIO_Init(GPIOA, &GPIO_InitStructure);                                              //根据设定参数初始化GPIO
  249.        
  250.           Exit_Init();
  251. }


  254. void Exit_Init(void)
  255. {
  256.         EXTI_InitTypeDef EXTI_InitStructure;
  257.         NVIC_InitTypeDef NVIC_InitStructure;
  258.         /*====PA.11====*/
  259.         GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource3);
  260.        
  261.           EXTI_InitStructure.EXTI_Line=EXTI_Line3;
  262.           EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;       
  263.           EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;                                                /*上升沿触发*/
  264.           EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  265.           EXTI_Init(&EXTI_InitStructure);                 /*根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器*/

  267.     NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
  268.           NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;                        /*抢占优先级2， */
  269.           NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;                                                 /*子优先级1*/
  270.           NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                                                   /*使能外部中断通道*/
  271.           NVIC_Init(&NVIC_InitStructure);
  272.        
  273.        
  274.          
  275. }

  277. void EXTI3_IRQHandler()
  278. {
  279.                 if(SET == EXTI_GetITStatus(EXTI_Line3))//PB3 右后A相
  280.                 {
  281.                        
  282. //                        if(RB)
  283. //                        {                               
  284.                                 encoder++;
  285.                          if(pid.OUT1>80) jiao++;
  286.                           else  jiao--;
  287. //                        }
  288. //                        else  encoder--;
  289.                        
  290.                         EXTI_ClearITPendingBit(EXTI_Line3);
  291.                 }
  292. }
复制代码
  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>


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

  19. void LED_Init(void)
  20. {

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

  33. int main(void)
  34. {
  35.         delay_init();                     //延时函数初始化          

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

  75.                         while(!KEY5);
  76.                 }
  77.                
  78.                         if( KEY6==0 )
  79.                 {
  80.                         jiao=0;
  81.                         while(1)
  82.                         {
  83.                                 pid.OUT=0;
  84.                                 pid.sv=10;
  85.                         PID_Calc();       
  86.                         PID_Calc2();
  87.                         LCD_Test();
  88.                         if(pid.OUT1>80)
  89.                         {
  90.                                 AIN1=1,AIN2=0;
  91.                         }
  92.                         else
  93.                                 AIN1=0,AIN2=1;
  94.                   
  95.       if(pid.jpv==pid.jiao)
  96.                                 {
  97.                                         AIN1=0,AIN2=0;
  98.                                         break;
  99.                         }                               
  100.                 }
  101.                 }


  104.         }
  105. }

  107. void LCD_Test(void)
  108. {
  109.         /*演示显示变量*/
  110.         char sv[100];
  111.         char pv[100];
  112.         char OUT[100];
  113.         char jiao[100];
  114.         char jpv[100];
  115.   char OUT1[100];
  116.         char disbuff[100];
  117.        
  118.         LCD_SetFont(&Font8x16);
  119.         LCD_SetColors(RED,BLACK);

  121.   ILI9341_Clear(0,0,LCD_X_LENGTH,LCD_Y_LENGTH);        /* 清屏，显示全黑 */
  122.         /********显示字符串示例*******/
  123.   ILI9341_DispStringLine_EN_CH(LINE(0),"野火3.2寸LCD参数：");
  124.   ILI9341_DispStringLine_EN_CH(LINE(1),"分辨率：240x320 px");
  125.   ILI9341_DispStringLine_EN_CH(LINE(2),"ILI9341液晶驱动");
  126.   
  127.         /********显示变量示例*******/
  128.         LCD_SetFont(&Font16x24);
  129.         LCD_SetTextColor(GREEN);

  131.         /*使用c标准库把变量转化成字符串*/
  132.         sprintf(sv,"设定速度: %f ",pid.sv);
  133.         sprintf(pv,"实际速度 : %f ",pid.pv);
  134.         sprintf(OUT,"pid输出: %f ",pid.OUT);
  135.         sprintf(jiao,"设定角度: %f ",pid.jiao);
  136.   sprintf(jpv,"实际角度: %f ",pid.jpv);
  137.         sprintf(OUT1,"pid输出: %f ",pid.OUT1);
  138.        
  139.   LCD_ClearLine(LINE(4));        /* 清除单行文字 */
  140.         LCD_ClearLine(LINE(5));        /* 清除单行文字 */
  141.         LCD_ClearLine(LINE(6));        /* 清除单行文字 */
  142.   LCD_ClearLine(LINE(7));        /* 清除单行文字 */
  143.   LCD_ClearLine(LINE(8));        /* 清除单行文字 */
  144.         LCD_ClearLine(LINE(9));        /* 清除单行文字 */
  145.        
  146.         /*然后显示该字符串即可，其它变量也是这样处理*/
  147.         ILI9341_DispStringLine_EN_CH(LINE(4),sv);
  148.         ILI9341_DispStringLine_EN_CH(LINE(5),pv);
  149.         ILI9341_DispStringLine_EN_CH(LINE(6),OUT);
  150.         ILI9341_DispStringLine_EN_CH(LINE(7),jiao);
  151.   ILI9341_DispStringLine_EN_CH(LINE(8),jpv);
  152.         ILI9341_DispStringLine_EN_CH(LINE(9),OUT1);

  154. }

复制代码

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


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

使用道具 举报

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

使用道具 举报

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

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

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