找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開始

搜索
查看: 5842|回復(fù): 2
打印 上一主題 下一主題
收起左側(cè)

STM32的PID算法調(diào)節(jié)電機(jī)角度與調(diào)角程序

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:407610 發(fā)表于 2019-3-12 22:07 | 只看該作者 回帖獎(jiǎng)勵(lì) |倒序?yàn)g覽 |閱讀模式
不足之處歡迎指出

單片機(jī)源程序如下:
  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端口時(shí)鐘
  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);                                         //根據(jù)設(shè)定參數(shù)初始化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計(jì)算
  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))//計(jì)算周期未到
  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計(jì)算
  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))//計(jì)算周期未到
  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外設(shè)和AFIO復(fù)用功能模塊時(shí)鐘
  196.        
  197.         GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE); //Timer3部分重映射  TIM3_CH2->PB5
  198.        
  199.    //設(shè)置該引腳為復(fù)用輸出功能,輸出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;  //復(fù)用推挽輸出
  202.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  203.         GPIO_Init(GPIOB, &GPIO_InitStructure);
  204.        
  205.         TIM_TimeBaseStructure.TIM_Period = arr; //設(shè)置在下一個(gè)更新事件裝入活動(dòng)的自動(dòng)重裝載寄存器周期的值         
  206.         TIM_TimeBaseStructure.TIM_Prescaler =psc; //設(shè)置用來作為TIMx時(shí)鐘頻率除數(shù)的預(yù)分頻值  不分頻
  207.         TIM_TimeBaseStructure.TIM_ClockDivision =TIM_CKD_DIV1 ; //設(shè)置時(shí)鐘分割:TDTS = Tck_tim
  208.         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上計(jì)數(shù)模式
  209.         TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時(shí)間基數(shù)單位


  212.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //選擇定時(shí)器模式:TIM脈沖寬度調(diào)制模式1
  213.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比較輸出使能
  214.         TIM_OCInitStructure.TIM_Pulse =DIY_Pulse;                           //設(shè)置待裝入捕獲比較寄存器的脈沖值
  215.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;     //輸出極性:TIM輸出比較極性高
  216.        
  217.         TIM_OC2Init(TIM3, &TIM_OCInitStructure);  //根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx
  218.        

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

  222.        
  223.         TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);  //CH2預(yù)裝載使能         
  224.        
  225.         TIM_ARRPreloadConfig(TIM3, ENABLE); //使能TIMx在ARR上的預(yù)裝載寄存器
  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時(shí)鐘
  244.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能GPIO端口時(shí)鐘
  245.        
  246.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;                    //端口配置
  247.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;         //上拉輸入
  248.         GPIO_Init(GPIOA, &GPIO_InitStructure);                                              //根據(jù)設(shè)定參數(shù)初始化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;                                                /*上升沿觸發(fā)*/
  264.           EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  265.           EXTI_Init(&EXTI_InitStructure);                 /*根據(jù)EXTI_InitStruct中指定的參數(shù)初始化外設(shè)EXTI寄存器*/

  267.     NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
  268.           NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;                        /*搶占優(yōu)先級(jí)2， */
  269.           NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;                                                 /*子優(yōu)先級(jí)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. }
復(fù)制代碼
  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端口時(shí)鐘
  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);                                         //根據(jù)設(shè)定參數(shù)初始化GPIOB.5
  30. GPIO_SetBits(GPIOB,GPIO_Pin_5);                                                 //PB.5 輸出高
  31. }

  33. int main(void)
  34. {
  35.         delay_init();                     //延時(shí)函數(shù)初始化          

  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參數(shù)：");
  124.   ILI9341_DispStringLine_EN_CH(LINE(1),"分辨率：240x320 px");
  125.   ILI9341_DispStringLine_EN_CH(LINE(2),"ILI9341液晶驅(qū)動(dòng)");
  126.   
  127.         /********顯示變量示例*******/
  128.         LCD_SetFont(&Font16x24);
  129.         LCD_SetTextColor(GREEN);

  131.         /*使用c標(biāo)準(zhǔn)庫(kù)把變量轉(zhuǎn)化成字符串*/
  132.         sprintf(sv,"設(shè)定速度: %f ",pid.sv);
  133.         sprintf(pv,"實(shí)際速度 : %f ",pid.pv);
  134.         sprintf(OUT,"pid輸出: %f ",pid.OUT);
  135.         sprintf(jiao,"設(shè)定角度: %f ",pid.jiao);
  136.   sprintf(jpv,"實(shí)際角度: %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. }

復(fù)制代碼

所有資料51hei提供下載:
PID算法.7z (227.61 KB, 下載次數(shù): 138)


分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏4 分享淘帖 頂頂 踩踩
回復(fù)

使用道具 舉報(bào)

沙發(fā)
ID:1 發(fā)表于 2019-3-12 22:25 | 只看該作者
本帖需要重新編輯補(bǔ)全電路原理圖,源碼,詳細(xì)說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)
回復(fù)

使用道具 舉報(bào)

板凳
ID:593177 發(fā)表于 2019-7-31 18:36 | 只看該作者
感謝分享
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表