標(biāo)題: STM32編程實(shí)現(xiàn)直流電機(jī)位置速度PID雙閉環(huán)控制 HAL庫(kù)源程序 [打印本頁(yè)]
作者: 嬡悅意    時(shí)間: 2021-7-23 15:44
標(biāo)題: STM32編程實(shí)現(xiàn)直流電機(jī)位置速度PID雙閉環(huán)控制 HAL庫(kù)源程序
雙環(huán)閉環(huán)

單片機(jī)源程序如下:
  1. /**
  2.   ******************************************************************************
  3.   * 文件名程: main.c
  4.   * 作    者: 學(xué)習(xí)小組
  5.   * 功    能: GM37-545_位置速度閉環(huán)控制
  6.   ******************************************************************************
  7.   * 說(shuō)明：
  8.   ******************************************************************************
  9.   */
  10. /* 包含頭文件 ----------------------------------------------------------------*/
  11. #include "stm32f4xx_hal.h"
  12. #include "key.h"
  13. #include "encoder.h"
  14. #include "usartx.h"
  15. #include "BDCMotor.h"
  16. /* 私有類(lèi)型定義 --------------------------------------------------------------*/
  17. typedef struct
  18. {
  19.   __IO int32_t  SetPoint;   //設(shè)定目標(biāo) Desired Value
  20.   __IO float    SumError;   //誤差累計(jì)
  21.   __IO float    Proportion; //比例常數(shù) Proportional Const
  22.   __IO float    Integral;   //積分常數(shù) Integral Const
  23.   __IO float    Derivative; //微分常數(shù) Derivative Const
  24.   __IO int      LastError;  //Error[-1]
  25.   __IO int      PrevError;  //Error[-2]
  26. }PID_TypeDef;

  28. /* 私有宏定義 ----------------------------------------------------------------*/
  29. #define SPEEDRATIO    270
  30. #define ENCODER_RESOLUTION    11
  31. #define PPR           ((SPEEDRATIO*ENCODER_RESOLUTION)*4) // Pulse/Round 每圈可捕獲到的脈沖數(shù)

  33. /*************************************/
  34. // 定義PID相關(guān)宏
  35. // 這三個(gè)參數(shù)設(shè)定對(duì)電機(jī)運(yùn)行影響非常大
  36. // PID參數(shù)跟采樣時(shí)間息息相關(guān)
  37. /*************************************/
  38. #define  SPD_P_DATA     50.0f        // P參數(shù)
  39. #define  SPD_I_DATA     8.5f         // I參數(shù)
  40. #define  SPD_D_DATA     0.0f         // D參數(shù)
  41. #define  TARGET_SPEED   10.0f        // 目標(biāo)速度    10r/m

  43. #define  LOC_P_DATA     0.01f       // P參數(shù)
  44. #define  LOC_I_DATA     0.0f       // D參數(shù)
  45. #define  LOC_D_DATA     0.08f       // D參數(shù)
  46. #define  TARGET_LOC     (3*PPR)    // 目標(biāo)位置    11880 Pulse = 1r

  48. /* 私有變量 ------------------------------------------------------------------*/
  49. __IO uint8_t  Start_flag = 0;       // PID 開(kāi)始標(biāo)志
  50. uint32_t Motor_Dir = CW;             // 電機(jī)方向

  52. __IO int32_t tmpPWM_DutySpd = 0;
  53. __IO int32_t tmpPWM_Duty = 0;

  55. __IO int32_t Sample_Pulse;           // 編碼器捕獲值 Pulse
  56. __IO int32_t LastSample_Pulse;       // 編碼器捕獲值 Pulse
  57. __IO int32_t Spd_PPS;                // 速度值 Pulse/Sample
  58. __IO float Spd_RPM;                  // 速度值 r/m

  60. /* 擴(kuò)展變量 ------------------------------------------------------------------ */
  61. extern __IO uint32_t uwTick;

  63. /* PID結(jié)構(gòu)體 */
  64. PID_TypeDef  sPID,lPID;               // PID參數(shù)結(jié)構(gòu)體

  66. /* 擴(kuò)展變量 ------------------------------------------------------------------*/
  67. /* 私有函數(shù)原形 --------------------------------------------------------------*/
  68. void PID_ParamInit(void) ;
  69. int32_t SpdPIDCalc(float NextPoint);
  70. int32_t LocPIDCalc(int32_t NextPoint);
  71. /* 函數(shù)體 --------------------------------------------------------------------*/
  72. /**
  73.   * 函數(shù)功能: 系統(tǒng)時(shí)鐘配置
  74.   * 輸入?yún)?shù): 無(wú)
  75.   * 返 回 值: 無(wú)
  76.   * 說(shuō)    明: 無(wú)
  77.   */
  78. void SystemClock_Config(void)
  79. {
  80.   RCC_OscInitTypeDef RCC_OscInitStruct;
  81.   RCC_ClkInitTypeDef RCC_ClkInitStruct;

  83.   __HAL_RCC_PWR_CLK_ENABLE();                                     // 使能PWR時(shí)鐘

  85.   __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);  // 設(shè)置調(diào)壓器輸出電壓級(jí)別1

  87.   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;      // 外部晶振，8MHz
  88.   RCC_OscInitStruct.HSEState = RCC_HSE_ON;                        // 打開(kāi)HSE
  89.   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;                    // 打開(kāi)PLL
  90.   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;            // PLL時(shí)鐘源選擇HSE
  91.   RCC_OscInitStruct.PLL.PLLM = 8;                                 // 8分頻MHz
  92.   RCC_OscInitStruct.PLL.PLLN = 336;                               // 336倍頻
  93.   RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;                     // 2分頻，得到168MHz主時(shí)鐘
  94.   RCC_OscInitStruct.PLL.PLLQ = 7;                                 // USB/SDIO/隨機(jī)數(shù)產(chǎn)生器等的主PLL分頻系數(shù)
  95.   HAL_RCC_OscConfig(&RCC_OscInitStruct);

  97.   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
  98.                               |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  99.   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;       // 系統(tǒng)時(shí)鐘：168MHz
  100.   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;              // AHB時(shí)鐘： 168MHz
  101.   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;               // APB1時(shí)鐘：42MHz
  102.   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;               // APB2時(shí)鐘：84MHz
  103.   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);

  105.   HAL_RCC_EnableCSS();                                            // 使能CSS功能，優(yōu)先使用外部晶振，內(nèi)部時(shí)鐘源為備用

  107.          // HAL_RCC_GetHCLKFreq()/1000    1ms中斷一次
  108.         // HAL_RCC_GetHCLKFreq()/100000         10us中斷一次
  109.         // HAL_RCC_GetHCLKFreq()/1000000 1us中斷一次
  110.   HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);                 // 配置并啟動(dòng)系統(tǒng)滴答定時(shí)器
  111.   /* 系統(tǒng)滴答定時(shí)器時(shí)鐘源 */
  112.   HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  114.   /* 系統(tǒng)滴答定時(shí)器中斷優(yōu)先級(jí)配置 */
  115.   HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
  116. }

  118. /**
  119.   * 函數(shù)功能: 主函數(shù).
  120.   * 輸入?yún)?shù): 無(wú)
  121.   * 返 回 值: 無(wú)
  122.   * 說(shuō)    明: 無(wú)
  123.   */
  124. int main(void)
  125. {
  126.   /* 復(fù)位所有外設(shè)，初始化Flash接口和系統(tǒng)滴答定時(shí)器 */
  127.   HAL_Init();
  128.   /* 配置系統(tǒng)時(shí)鐘 */
  129.   SystemClock_Config();
  130.   /* 串口初始化 */
  131.   MX_USARTx_Init();
  132.   /* 按鍵初始化 */
  133.   KEY_GPIO_Init();
  134.   /* 編碼器初始化及使能編碼器模式 */
  135.   ENCODER_TIMx_Init();

  137.         /* 高級(jí)控制定時(shí)器初始化并配置PWM輸出功能 */
  138.   BDCMOTOR_TIMx_Init();

  140.   /* 啟動(dòng)定時(shí)器通道和互補(bǔ)通道PWM輸出 */
  141.   PWM_Duty = 0;
  142.   __HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty);  // 0%

  144.   /* PID 參數(shù)初始化 */
  145.   PID_ParamInit();
  146.   /* 無(wú)限循環(huán) */
  147.   while (1)
  148.   {
  149.     /* 停止按鈕 */
  150.     if(KEY1_StateRead()==KEY_DOWN)
  151.     {
  152.       if( lPID.Proportion < 0 )
  153.       {
  154.         Motor_Dir = CW;
  155.         BDDCMOTOR_DIR_CW();
  156.         PWM_Duty = -PWM_Duty;
  157.       }
  158.       else
  159.       {
  160.         Motor_Dir = CCW;
  161.         BDDCMOTOR_DIR_CCW();
  162.       }
  163.       __HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,0);  // 0%
  164.       Start_flag = 1;
  165.     }
  166.     if(KEY2_StateRead()==KEY_DOWN)
  167.     {
  168.       SHUTDOWN_MOTOR();
  169.       HAL_TIM_PWM_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
  170.       HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);         // 停止輸出
  171.     }
  172.     if(KEY3_StateRead()==KEY_DOWN)    // 圈數(shù)+1
  173.     {
  174.       lPID.SetPoint += PPR;
  175.     }
  176.     if(KEY4_StateRead()==KEY_DOWN)    // 圈數(shù)-1
  177.     {
  178.       lPID.SetPoint -= PPR;
  179.     }
  180.   }
  181. }

  183. /**
  184.   * 函數(shù)功能: 系統(tǒng)滴答定時(shí)器中斷回調(diào)函數(shù)
  185.   * 輸入?yún)?shù): 無(wú)
  186.   * 返 回 值: 無(wú)
  187.   * 說(shuō)    明: 每隔一定的時(shí)間就執(zhí)行pid算法
  188.   */
  189. void HAL_SYSTICK_Callback(void)
  190. {
  191.   __IO int32_t ADC_Resul= 0;
  192.   __IO float Volt_Result = 0;

  194.   /* 位置環(huán)周期100ms */
  195.   if(uwTick % 100 == 0)
  196.   {
  197.     /* 獲取當(dāng)前位置值,編碼器4倍頻之后的數(shù)值 */
  198.     Sample_Pulse = (OverflowCount*CNT_MAX) + (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);

  200.     /* 計(jì)算PID結(jié)果 */
  201.     if(Start_flag == 1)
  202.     {
  203.       tmpPWM_DutySpd = LocPIDCalc(Sample_Pulse);

  205.       /* 設(shè)定速度環(huán)的目標(biāo)值 */
  206.       if(tmpPWM_DutySpd >= TARGET_SPEED)
  207.         tmpPWM_DutySpd = TARGET_SPEED;
  208.       if(tmpPWM_DutySpd <= -TARGET_SPEED)
  209.         tmpPWM_DutySpd = -TARGET_SPEED;
  210.     }
  211.   }
  212.   /* 速度環(huán)周期50ms */
  213.   if(uwTick % 50 == 0)
  214.   {
  215.     /* 獲得當(dāng)前速度 */
  216.     Sample_Pulse = (OverflowCount*CNT_MAX) + \
  217.                     (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);
  218.     Spd_PPS = Sample_Pulse - LastSample_Pulse;
  219.     LastSample_Pulse = Sample_Pulse ;

  221.     /* 11線(xiàn)編碼器,270減速比,一圈脈沖信號(hào)是11*270*4 PPR */
  222.     Spd_RPM = ((((float)Spd_PPS/(float)PPR)*20.0f)*(float)60);//單位是rpm

  224.     /* 計(jì)算PID結(jié)果 */
  225.     if(Start_flag == 1)
  226.     {
  227.       sPID.SetPoint = tmpPWM_DutySpd;
  228.       PWM_Duty = SpdPIDCalc(Spd_RPM);
  229.       if(PWM_Duty < 0)
  230.       {
  231.         Motor_Dir = CW;
  232.         BDDCMOTOR_DIR_CW();
  233.         PWM_Duty = -PWM_Duty;
  234.       }
  235.       else
  236.       {
  237.         Motor_Dir = CCW;
  238.         BDDCMOTOR_DIR_CCW();
  239.       }

  241.       __HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty );

  243.     }
  244.     printf("LOC:%d Sped: %2.2f r/m \n",Sample_Pulse,
  245.            Spd_RPM );
  246.   }
  247. }

  249. /******************** PID 控制設(shè)計(jì) ***************************/
  250. /**
  251.   * 函數(shù)功能: PID參數(shù)初始化
  252.   * 輸入?yún)?shù): 無(wú)
  253.   * 返 回 值: 無(wú)
  254.   * 說(shuō)    明: 無(wú)
  255.   */
  256. void PID_ParamInit()
  257. {
  258.     sPID.LastError = 0;           // Error[-1]
  259.     sPID.PrevError = 0;           // Error[-2]
  260.     sPID.Proportion = SPD_P_DATA; // 比例常數(shù) Proportional Const
  261.     sPID.Integral = SPD_I_DATA;   // 積分常數(shù)  Integral Const
  262.     sPID.Derivative = SPD_D_DATA; // 微分常數(shù) Derivative Const
  263.     sPID.SetPoint = TARGET_SPEED; // 設(shè)定目標(biāo)Desired Value

  265.     lPID.LastError = 0;           // Error[-1]
  266.     lPID.PrevError = 0;           // Error[-2]
  267.     lPID.Proportion = LOC_P_DATA; // 比例常數(shù) Proportional Const
  268.     lPID.Integral = LOC_I_DATA;   // 積分常數(shù)  Integral Const
  269.     lPID.Derivative = LOC_D_DATA; // 微分常數(shù) Derivative Const
  270.     lPID.SetPoint = TARGET_LOC;   // 設(shè)定目標(biāo)Desired Value
  271. }


  274. /**
  275.   * 函數(shù)名稱(chēng)：速度閉環(huán)PID控制設(shè)計(jì)
  276.   * 輸入?yún)?shù)：當(dāng)前控制量
  277.   * 返 回 值：目標(biāo)控制量
  278.   * 說(shuō)    明：無(wú)
  279.   */
  280. int32_t SpdPIDCalc(float NextPoint)
  281. {
  282.   float iError,dError;
  283.   iError = sPID.SetPoint - NextPoint; //偏差

  285.   if((iError<0.2f )&& (iError>-0.2f))
  286.     iError = 0.0f;

  288.   sPID.SumError += iError; //積分
  289.     /* 設(shè)定積分上限 */
  290.   if(lPID.SumError >= TARGET_SPEED*10)
  291.     lPID.SumError = TARGET_SPEED*10;
  292.   if(lPID.SumError <= -TARGET_SPEED*10)
  293.     lPID.SumError = -TARGET_SPEED*10;

  295.   dError = iError - sPID.LastError; //微分
  296.   sPID.LastError = iError;
  297.   return (int32_t)(sPID.Proportion * (float)iError //比例項(xiàng)
  298.   + sPID.Integral * (float)sPID.SumError //積分項(xiàng)
  299.   + sPID.Derivative * (float)dError); //微分項(xiàng)
  300. }

  302. /**
  303.   * 函數(shù)名稱(chēng)：位置閉環(huán)PID控制設(shè)計(jì)
  304.   * 輸入?yún)?shù)：當(dāng)前控制量
  305.   * 返 回 值：目標(biāo)控制量
  306.   * 說(shuō)    明：無(wú)
  307.   */
  308. int32_t LocPIDCalc(int32_t NextPoint)
  309. {
  310.   int32_t iError,dError;
  311.   iError = lPID.SetPoint - NextPoint; //偏差
  312.   /* 設(shè)定閉環(huán)死區(qū) */
  313.   if((iError >= -50) && (iError <= 50))
  314.   {
  315.     iError = 0;
  316.     lPID.SumError = 0;
  317.   }

  319.   /* 積分分離 */
  320.   if((iError >= -1000) && (iError <= 1000))
  321.   {
  322.     lPID.SumError += iError; //積分

  324.     /* 設(shè)定積分上限 */
  325.     if(lPID.SumError >= 1000)
  326.       lPID.SumError = 1000;
  327.     if(lPID.SumError <= -1000)
  328.       lPID.SumError = -1000;
  329.   }

  331.   dError = iError - lPID.LastError; //微分
  332.   lPID.LastError = iError;

  334.   return (int32_t)(lPID.Proportion * (float)iError //比例項(xiàng)
  335.   + lPID.Integral * (float)lPID.SumError //積分項(xiàng)
  336.   + lPID.Derivative * (float)dError);    //微分項(xiàng)
  337. }

  339. /**********************************END OF FILE****************************/
復(fù)制代碼

所有代碼51hei附件下載:
STM32編程實(shí)現(xiàn)_位置速度PID雙閉環(huán)控制.7z (1.53 MB, 下載次數(shù): 103)





歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1