標(biāo)題: 57&42步進(jìn)電機(jī)_速度閉環(huán)_位置式PID STM32源程序 [打印本頁]
作者: xw198884    時間: 2019-9-25 09:50
標(biāo)題: 57&42步進(jìn)電機(jī)_速度閉環(huán)_位置式PID STM32源程序
黑石H7開發(fā)板步進(jìn)電機(jī)驅(qū)動  親測可用
  1. #include "stm32h7xx_hal.h"
  2. #include "bsp_led.h"
  3. #include "bsp_key.h"
  4. #include "bsp_debug_usart.h"
  5. #include "bsp_stepmotor.h"
  6. #include "bsp_encoder.h"
  7. #include <string.h>
  8. #include <math.h>
  9. #include <stdbool.h>
  10. #include <stdlib.h>

  12. /* 私有類型定義 --------------------------------------------------------------*/
  13. typedef struct
  14. {
  15.   __IO float    SetPoint;    // 目標(biāo)值  單位:r/m
  16.   __IO int32_t  LastError;   // 前一次誤差   
  17.   __IO int32_t  PrevError;   // 前兩次誤差
  18.   __IO int32_t  SumError;    // 累計誤差
  19.   float    Proportion;       // Kp系數(shù)
  20.   float    Integral;         // Ki系數(shù)
  21.   float    Derivative;       // Kd系數(shù)
  22. }PID_Typedef;

  24. /* 私有宏定義 ----------------------------------------------------------------*/
  25. #define    PID_DEFAULT_P          0.4f  //
  26. #define    PID_DEFAULT_I          0.2f  //
  27. #define    PID_DEFAULT_D          0.0f  //
  28. #define    SPEED                  5.3f  // 移動速度 mm/s

  30. #define    MM_ROUND               5   // 絲桿導(dǎo)程 mm/r
  31. #define    INTERVAL               20  //  采樣和PID計算時間間隔 ms

  33. /* 私有變量 ------------------------------------------------------------------*/

  35. __IO static PID_Typedef vPID; // 速度環(huán)PID結(jié)構(gòu)體

  37. __IO uint16_t time_count = 0; // 時間計數(shù)，每1ms增加一(與滴答定時器頻率有關(guān))
  38. __IO uint8_t Time_Flag   = 0; // 任務(wù)時間標(biāo)記
  39. bool Start_flag = false;      // 啟動/停止

  41. /* 擴(kuò)展變量 ------------------------------------------------------------------*/

  43. /* 私有函數(shù)原形 --------------------------------------------------------------*/

  45. static void SystemClock_Config( void );
  46. static void CPU_CACHE_Enable( void );

  48. /* 函數(shù)體 --------------------------------------------------------------------*/
  49.   
  50. /**
  51.   * 函數(shù)功能：位置式PID速度環(huán)計算
  52.   * 輸入?yún)?shù): @NextPoint    由編碼器得到的計數(shù)值
  53.   *           @TargetVal    目標(biāo)值
  54.   * 返 回 值：經(jīng)過PID運算得到的增量值
  55.   * 說    明：位置式 PID 速度環(huán)控制設(shè)計,計算得到的結(jié)果仍然是速度值
  56.   */
  57. float IncPIDCalc(int NextPoint,float TargetVal)       //臨時變量,期望值
  58. {
  59.   float iError = 0,iIncpid = 0;                       //當(dāng)前誤差
  60.   
  61.   iError = TargetVal - NextPoint;                     // 增量計算
  62.   if((iError < 0.5f)&&(iError>-0.5f))
  63.     iError = 0;                                       // |e| < 0.5,不做調(diào)整
  64.   
  65.   vPID.PrevError = iError - vPID.LastError;
  66.   vPID.SumError += iError;
  67.   iIncpid=(vPID.Proportion * iError)                  // E[k]項
  68.               +(vPID.Integral * vPID.SumError)        // E[k-1]項
  69.               +(vPID.Derivative * vPID.PrevError);    // E[k-2]項
  70.   
  71.   vPID.PrevError = vPID.LastError;                    // 存儲誤差，用于下次計算
  72.   vPID.LastError = iError;
  73.   return(iIncpid);                                    // 返回增量值
  74. }

  76. /**
  77.   * 函數(shù)功能: PID結(jié)構(gòu)體初始化
  78.   * 輸入?yún)?shù): 無
  79.   * 返 回 值: 無
  80.   * 說    明: 初始化PID參數(shù)
  81.   */
  82. void Init_PIDStruct()
  83. {
  84.   vPID.SetPoint   = SPEED;          // 目標(biāo)值  單位:mm/s
  85.   vPID.Proportion = PID_DEFAULT_P;  // Kp系數(shù)
  86.   vPID.Integral   = PID_DEFAULT_I;  // Ki系數(shù)
  87.   vPID.Derivative = PID_DEFAULT_D;  // Kd系數(shù)
  88.   vPID.LastError  = 0;
  89.   vPID.PrevError  = 0;
  90.   vPID.SumError   = 0;
  91. }
  92. /**
  93.   * 函數(shù)功能: 主函數(shù).
  94.   * 輸入?yún)?shù): 無
  95.   * 返 回 值: 無
  96.   * 說    明: 無
  97.   */
  98. int main(void)
  99. {
  100.   float     Exp_Val     = 0;  // PID計算出來的期望值
  101.   float     Vel_Target  = 0;; // 速度目標(biāo)值
  102.   int16_t   MSF = 0;          // 電機(jī)反饋速度
  103.   int32_t   Enc_Cap     = 0;  // 編碼器輸入捕獲數(shù)
  104.   int32_t   lastEnc_Cap = 0;  // 編碼器上一次捕獲值
  105.   
  106.   CPU_CACHE_Enable();
  107.   /* 復(fù)位所有外設(shè)，初始化Flash接口和系統(tǒng)滴答定時器 */
  108.   HAL_Init();
  109.   /* 配置系統(tǒng)時鐘 */
  110.   SystemClock_Config();
  111.   /* 板載LED,按鍵初始化 */
  112.   LED_GPIO_Init();
  113.   KEY_GPIO_Init();
  114.   /* 初始化PID參數(shù)結(jié)構(gòu)體 */
  115.   Init_PIDStruct();
  116.   /* 調(diào)試串口初始化 */
  117.   MX_DEBUG_UART_Init();

  119.   /* 編碼器定時器初始化并配置輸入捕獲功能 */
  120.   ENCODER_TIMx_Init();
  121.   /* 啟動編碼器接口 */
  122.   HAL_TIM_Encoder_Start(&htimx_Encoder, TIM_CHANNEL_ALL);
  123.   
  124.   HAL_Delay(10);
  125.   
  126.   /* 步進(jìn)電機(jī)定時器初始化*/
  127.   SMOTOR1_TIMx_Init();
  128.   
  129.   /* 首先禁止步進(jìn)電機(jī)動作*/
  130.   SMotor_Set_State(MOTOR_DISABLE);
  131.   /* 比較輸出和中斷 */
  132.   HAL_TIM_OC_Stop_IT(&htimx_SMotor1,TIM_CHANNEL_1);
  133.   
  134.   /* 移動速度轉(zhuǎn)換為PID的目標(biāo)值
  135.    * 目標(biāo)值是每個采樣周期的編碼器計數(shù)值,SetPoint是移動速度(mm/s),INTERVAL是采樣周期間隔
  136.    * 1mm/s對應(yīng)的編碼器計數(shù)值是 ENCODER_RESOLUTION/MM_ROUND -> 2400/5 = 480 Hz
  137.    * 每個采樣周期之間的編碼器計數(shù)值就是 480/(1000/INTERVAL) = 9.6f
  138.    * SetPoint*9.6 就是 SetPoint mm/s 對應(yīng)的采樣周期間隔編碼器計數(shù)值
  139.    */
  140.   Vel_Target = round(fabs(vPID.SetPoint) * (ENCODER_RESOLUTION/MM_ROUND)/(1000/INTERVAL)  );//每單位采樣周期內(nèi)的脈沖數(shù)(頻率)
  141.   
  142.   __HAL_DBGMCU_FREEZE_TIM8();// debug 的時候停止定時器時鐘
  143.   __HAL_DBGMCU_FREEZE_TIM2();// debug 的時候停止定時器時鐘
  144.   while (1)
  145.   {
  146.    
  147.     /* KEY1啟動電機(jī),并且使能PID計算 */
  148.     if(KEY1_StateRead() == KEY_DOWN)
  149.     {
  150.       SMotor_Set_State(MOTOR_ENABLE);
  151.       Exp_Val = 0;
  152.       Init_PIDStruct();
  153.       if(vPID.SetPoint < 0 )
  154.         SMotor_Set_Dir( MOTOR_DIR_CCW );
  155.       else
  156.         SMotor_Set_Dir( MOTOR_DIR_CW );
  157.       SMotor_Set_Speed(0.01); // 低速啟動
  158.       HAL_TIM_OC_Start_IT(&htimx_SMotor1,TIM_CHANNEL_1);
  159.       Start_flag = true;           // 啟動/停止
  160.       printf("啟動PID計算\n");
  161.       printf("設(shè)置的目標(biāo)值是 %f mm/s \n",vPID.SetPoint);//
  162.       printf("對應(yīng)的編碼器計數(shù)值是 %d pulse/s \n",(int32_t)Vel_Target*(1000/INTERVAL));//
  163.     }
  164.     /* KEY2停止電機(jī),并且停止PID計算 */
  165.     if(KEY2_StateRead() == KEY_DOWN)
  166.     {
  167.       SMotor_Set_State(MOTOR_DISABLE);
  168.       HAL_TIM_OC_Stop_IT(&htimx_SMotor1,TIM_CHANNEL_1);
  169.       Start_flag = false;           // 啟動/停止
  170.     }
  171.     /* KEY3/KEY4用于調(diào)控電機(jī)位置 */
  172.     if(KEY3_StateRead() == KEY_DOWN)
  173.     {
  174.       Start_flag = false;          // 啟動/停止
  175.       SMotor_Set_State( MOTOR_ENABLE );
  176.       SMotor_Set_Dir( MOTOR_DIR_CW );
  177.       SMotor_Set_Speed( 160 );     //設(shè)置低速啟動
  178.       HAL_TIM_OC_Start_IT(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
  179.     }
  180.     if(KEY4_StateRead() == KEY_DOWN)
  181.     {
  182.       Start_flag = false;          // 啟動/停止
  183.       SMotor_Set_State( MOTOR_ENABLE );
  184.       SMotor_Set_Dir( MOTOR_DIR_CCW );
  185.       SMotor_Set_Speed( 160 );     // 設(shè)置低速啟動
  186.       HAL_TIM_OC_Start_IT(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
  187.     }
  188.     // 采樣和控制周期為20ms
  189.     if(Time_Flag & 0x01)
  190.     {
  191.       //獲得編碼器的脈沖值
  192.       Enc_Cap  = YS_Encoder_GetCounting();

  194.       //M法 測速度
  195.       MSF = Enc_Cap - lastEnc_Cap;
  196.       lastEnc_Cap = Enc_Cap;
  197.       MSF = abs(MSF);
  198.       
  199.       Exp_Val    = IncPIDCalc(MSF, Vel_Target);
  200.       Exp_Val    = fabs(Exp_Val);

  202.       SMotor_Set_Speed(Exp_Val);
  203.       Time_Flag &= ~0x01;
  204.     }
  205.    
  206.     // 數(shù)據(jù)發(fā)送周期為1s, 顯示轉(zhuǎn)速
  207.     if(Time_Flag & 0x02)
  208.     {
  209.       int32_t pulse = YS_Encoder_GetCounting();
  210.       static int32_t tmp = 0;
  211.       float speed = (float)( pulse - tmp ) / ENCODER_RESOLUTION ;
  212.       
  213.       printf("1s內(nèi)的編碼器計數(shù)值:  %d\n", pulse - tmp);
  214.       printf("電機(jī)轉(zhuǎn)速:  %.2f r/s  \n", speed );
  215.       printf("移動速度:  %.1f mm/s \n", speed*5.0f);// 1r就是5mm
  216.       tmp = pulse ;
  217.       Time_Flag &= ~0x02;
  218.     }
  219.   }
  220. }
  221. /**
  222.   * 函數(shù)功能: 系統(tǒng)滴答定時器中斷回調(diào)函數(shù)
  223.   * 輸入?yún)?shù): 無
  224.   * 返 回 值: 無
  225.   * 說    明: 每發(fā)生一次滴答定時器中斷進(jìn)入該回調(diào)函數(shù)一次
  226.   */
  227. void HAL_SYSTICK_Callback(void)
  228. {
  229.   // 每1ms自動增一
  230.   time_count++;         
  231.   if( (time_count % INTERVAL) == 0)// 20ms讀取一次編碼器數(shù)值
  232.   {
  233.     if(Start_flag )
  234.       Time_Flag |= 0x01;
  235.   }
  236.   else if(time_count >= 1000)      // 1s發(fā)送一次數(shù)據(jù)
  237.   {
  238.     Time_Flag |= 0x02;
  239.     time_count = 0;
  240.   }
  241. }


  244. /**
  245.   * 函數(shù)功能: 定時器比較輸出中斷回調(diào)函數(shù)
  246.   * 輸入?yún)?shù): htim：定時器句柄指針
  247.   * 返 回 值: 無
  248.   * 說    明: 控制脈沖輸出
  249.   */
  250. void HAL_TIM_OC_DelayElapsedCallback( TIM_HandleTypeDef * htim)
  251. {
  252.   uint32_t Toggle = 0;
  253.   
  254.   if(htim == &htimx_SMotor1)
  255.   {
  256.     /* 設(shè)置下一次中斷的間隔 */
  257.     Toggle = __HAL_TIM_GET_COMPARE(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
  258.     Toggle += SMotor1.PulseWidth; // 計算實際的比較值
  259.     __HAL_TIM_SET_COMPARE(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx, (uint16_t)Toggle);
  260.   }
  261. }
  262. /**
  263.   * 函數(shù)功能: 系統(tǒng)時鐘配置
  264.   * 輸入?yún)?shù): 無
  265.   * 返 回 值: 無
  266.   * 說    明: 無
  267.   */
  268. static void SystemClock_Config(void)
  269. {
  270.   RCC_OscInitTypeDef RCC_OscInitStruct={0};
  271.   RCC_ClkInitTypeDef RCC_ClkInitStruct={0};

  273.   /* 使能PWR配置更新 */
  274.   MODIFY_REG(PWR->CR3, PWR_CR3_SCUEN, 0);

  276.   /* 設(shè)置調(diào)壓器輸出電壓級別1 */
  277.   __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  279.   /* 等待D3區(qū)域VOS 輸出準(zhǔn)備就緒 */
  280.   while ((PWR->D3CR & (PWR_D3CR_VOSRDY)) != PWR_D3CR_VOSRDY) // 等待內(nèi)核電源就緒
  281.   {
  282.    
  283.   }
  284.   /* RCC 內(nèi)部/外部晶振配置 ,用于CPU，AHB，APH總線時鐘 */
  285.   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;// 外部晶振
  286.   RCC_OscInitStruct.HSEState = RCC_HSE_ON;//選擇外部時鐘晶振 HSE 25MHz
  287.   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;//使能PLL
  288.   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;//PLL時鐘源選擇HSE
  289.   RCC_OscInitStruct.PLL.PLLM = 5;  // 外部時鐘2分頻,得到5MHz 作為PLL時鐘源
  290.   RCC_OscInitStruct.PLL.PLLN = 160;// PLL 160倍頻,得到800MHz PLLCLK
  291.   RCC_OscInitStruct.PLL.PLLP = 2;  // PLLCLK 2分頻 得到400MHz SYSCLK
  292.   RCC_OscInitStruct.PLL.PLLQ = 2;  // PLLCLK 2分頻 得到400MHz用于部分外設(shè)的時鐘
  293.   RCC_OscInitStruct.PLL.PLLR = 2;  // PLLCLK 2分頻 得到400MHz用于部分外設(shè)的時鐘
  294.   RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2;// PLL時鐘輸入范圍4-8MHz
  295.   RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;// PLL時鐘輸出范圍192-836MHz
  296.   RCC_OscInitStruct.PLL.PLLFRACN = 0;  // 此參數(shù)用于微調(diào) PLL1 VCO 范圍0~2^13-1
  297.   HAL_RCC_OscConfig(&RCC_OscInitStruct);
  298.   
  299.   /* RCC CPU，AHB，APH總線時鐘配置*/
  300.   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
  301.                               |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
  302.                               |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
  303.   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;//SYSCLK 時鐘源400MHz
  304.   RCC_ClkInitStruct.SYSCLKDivider  = RCC_SYSCLK_DIV1;// SYSCLK 1分頻 HCLK=400MHz
  305.   RCC_ClkInitStruct.AHBCLKDivider  = RCC_HCLK_DIV2;  // HCLK 2分頻，AHB = 200Mhz
  306.   RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; // APB3 2分頻，APB3 = 100MHz
  307.   RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; // APB1 2分頻，APB1 = 100MHz
  308.   RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; // APB2 2分頻，APB2 = 100MHz
  309.   RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; // APB4 2分頻，APB4 = 100MHz
  310.   HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
  311.   

  313.   /* 使能時鐘安全機(jī)制 */
  314.   HAL_RCC_EnableCSS();

  316.   /* 滴答定時器配置1ms */
  317.   // SystemCoreClock/1000    1ms中斷一次
  318.         // SystemCoreClock/100000         10us中斷一次
  319.         // SystemCoreClock/1000000 1us中斷一次
  320.   HAL_SYSTICK_Config(SystemCoreClock/(1000));

  322.   /* 系統(tǒng)滴答定時器時鐘源 */
  323.   HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  325.   /* 系統(tǒng)滴答定時器中斷優(yōu)先級配置 */
  326.   HAL_NVIC_SetPriority(SysTick_IRQn, 1, 1);
  327. }

  329. /**
  330.   * 函數(shù)功能: 使能CPU L1-Cache
  331.   * 輸入?yún)?shù): 無
  332.   * 返 回 值: 無
  333.   * 說    明: 無
  334.   */
  335. static void CPU_CACHE_Enable(void)
  336. {
  337.   /* 使能 I-Cache */
  338.   SCB_EnableICache();

  340.   /* 使能 D-Cache */
  341.   SCB_EnableDCache();
  342. }

  344. /********** (C) COPYRIGHT 2019-2030 硬石嵌入式開發(fā)團(tuán)隊 *******END OF FILE******/
復(fù)制代碼


YS-H7Multi_HAL_MOTOR-236. 57&amp;42步進(jìn)電機(jī)_速度閉環(huán)_位置式PID.7z

7.37 MB, 下載次數(shù): 78, 下載積分: 黑幣 -5

黑石H7步進(jìn)電機(jī)驅(qū)動



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