57&42步進電機_速度閉環_位置式PID STM32源程序

ID:301684 · 發表于 2019-9-25 09:50

黑石H7開發板步進電機驅動親測可用

#include "stm32h7xx_hal.h"
#include "bsp_led.h"
#include "bsp_key.h"
#include "bsp_debug_usart.h"
#include "bsp_stepmotor.h"
#include "bsp_encoder.h"
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <stdlib.h>
/* 私有類型定義 --------------------------------------------------------------*/
typedef struct
{
__IO float SetPoint; // 目標值單位:r/m
__IO int32_t LastError; // 前一次誤差
__IO int32_t PrevError; // 前兩次誤差
__IO int32_t SumError; // 累計誤差
float Proportion; // Kp系數
float Integral; // Ki系數
float Derivative; // Kd系數
}PID_Typedef;
/* 私有宏定義 ----------------------------------------------------------------*/
#define PID_DEFAULT_P 0.4f //
#define PID_DEFAULT_I 0.2f //
#define PID_DEFAULT_D 0.0f //
#define SPEED 5.3f // 移動速度 mm/s
#define MM_ROUND 5 // 絲桿導程 mm/r
#define INTERVAL 20 // 采樣和PID計算時間間隔 ms
/* 私有變量 ------------------------------------------------------------------*/
__IO static PID_Typedef vPID; // 速度環PID結構體
__IO uint16_t time_count = 0; // 時間計數，每1ms增加一(與滴答定時器頻率有關)
__IO uint8_t Time_Flag = 0; // 任務時間標記
bool Start_flag = false; // 啟動/停止
/* 擴展變量 ------------------------------------------------------------------*/
/* 私有函數原形 --------------------------------------------------------------*/
static void SystemClock_Config( void );
static void CPU_CACHE_Enable( void );
/* 函數體 --------------------------------------------------------------------*/
/**
* 函數功能：位置式PID速度環計算
* 輸入參數: @NextPoint 由編碼器得到的計數值
* @TargetVal 目標值
* 返回值：經過PID運算得到的增量值
* 說明：位置式 PID 速度環控制設計,計算得到的結果仍然是速度值
*/
float IncPIDCalc(int NextPoint,float TargetVal) //臨時變量,期望值
{
float iError = 0,iIncpid = 0; //當前誤差
iError = TargetVal - NextPoint; // 增量計算
if((iError < 0.5f)&&(iError>-0.5f))
iError = 0; // |e| < 0.5,不做調整
vPID.PrevError = iError - vPID.LastError;
vPID.SumError += iError;
iIncpid=(vPID.Proportion * iError) // E[k]項
+(vPID.Integral * vPID.SumError) // E[k-1]項
+(vPID.Derivative * vPID.PrevError); // E[k-2]項
vPID.PrevError = vPID.LastError; // 存儲誤差，用于下次計算
vPID.LastError = iError;
return(iIncpid); // 返回增量值
}
/**
* 函數功能: PID結構體初始化
* 輸入參數: 無
* 返回值: 無
* 說明: 初始化PID參數
*/
void Init_PIDStruct()
{
vPID.SetPoint = SPEED; // 目標值單位:mm/s
vPID.Proportion = PID_DEFAULT_P; // Kp系數
vPID.Integral = PID_DEFAULT_I; // Ki系數
vPID.Derivative = PID_DEFAULT_D; // Kd系數
vPID.LastError = 0;
vPID.PrevError = 0;
vPID.SumError = 0;
}
/**
* 函數功能: 主函數.
* 輸入參數: 無
* 返回值: 無
* 說明: 無
*/
int main(void)
{
float Exp_Val = 0; // PID計算出來的期望值
float Vel_Target = 0;; // 速度目標值
int16_t MSF = 0; // 電機反饋速度
int32_t Enc_Cap = 0; // 編碼器輸入捕獲數
int32_t lastEnc_Cap = 0; // 編碼器上一次捕獲值
CPU_CACHE_Enable();
/* 復位所有外設，初始化Flash接口和系統滴答定時器 */
HAL_Init();
/* 配置系統時鐘 */
SystemClock_Config();
/* 板載LED,按鍵初始化 */
LED_GPIO_Init();
KEY_GPIO_Init();
/* 初始化PID參數結構體 */
Init_PIDStruct();
/* 調試串口初始化 */
MX_DEBUG_UART_Init();
/* 編碼器定時器初始化并配置輸入捕獲功能 */
ENCODER_TIMx_Init();
/* 啟動編碼器接口 */
HAL_TIM_Encoder_Start(&htimx_Encoder, TIM_CHANNEL_ALL);
HAL_Delay(10);
/* 步進電機定時器初始化*/
SMOTOR1_TIMx_Init();
/* 首先禁止步進電機動作*/
SMotor_Set_State(MOTOR_DISABLE);
/* 比較輸出和中斷 */
HAL_TIM_OC_Stop_IT(&htimx_SMotor1,TIM_CHANNEL_1);
/* 移動速度轉換為PID的目標值
* 目標值是每個采樣周期的編碼器計數值,SetPoint是移動速度(mm/s),INTERVAL是采樣周期間隔
* 1mm/s對應的編碼器計數值是 ENCODER_RESOLUTION/MM_ROUND -> 2400/5 = 480 Hz
* 每個采樣周期之間的編碼器計數值就是 480/(1000/INTERVAL) = 9.6f
* SetPoint*9.6 就是 SetPoint mm/s 對應的采樣周期間隔編碼器計數值
*/
Vel_Target = round(fabs(vPID.SetPoint) * (ENCODER_RESOLUTION/MM_ROUND)/(1000/INTERVAL) );//每單位采樣周期內的脈沖數(頻率)
__HAL_DBGMCU_FREEZE_TIM8();// debug 的時候停止定時器時鐘
__HAL_DBGMCU_FREEZE_TIM2();// debug 的時候停止定時器時鐘
while (1)
{
/* KEY1啟動電機,并且使能PID計算 */
if(KEY1_StateRead() == KEY_DOWN)
{
SMotor_Set_State(MOTOR_ENABLE);
Exp_Val = 0;
Init_PIDStruct();
if(vPID.SetPoint < 0 )
SMotor_Set_Dir( MOTOR_DIR_CCW );
else
SMotor_Set_Dir( MOTOR_DIR_CW );
SMotor_Set_Speed(0.01); // 低速啟動
HAL_TIM_OC_Start_IT(&htimx_SMotor1,TIM_CHANNEL_1);
Start_flag = true; // 啟動/停止
printf("啟動PID計算\n");
printf("設置的目標值是 %f mm/s \n",vPID.SetPoint);//
printf("對應的編碼器計數值是 %d pulse/s \n",(int32_t)Vel_Target*(1000/INTERVAL));//
}
/* KEY2停止電機,并且停止PID計算 */
if(KEY2_StateRead() == KEY_DOWN)
{
SMotor_Set_State(MOTOR_DISABLE);
HAL_TIM_OC_Stop_IT(&htimx_SMotor1,TIM_CHANNEL_1);
Start_flag = false; // 啟動/停止
}
/* KEY3/KEY4用于調控電機位置 */
if(KEY3_StateRead() == KEY_DOWN)
{
Start_flag = false; // 啟動/停止
SMotor_Set_State( MOTOR_ENABLE );
SMotor_Set_Dir( MOTOR_DIR_CW );
SMotor_Set_Speed( 160 ); //設置低速啟動
HAL_TIM_OC_Start_IT(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
}
if(KEY4_StateRead() == KEY_DOWN)
{
Start_flag = false; // 啟動/停止
SMotor_Set_State( MOTOR_ENABLE );
SMotor_Set_Dir( MOTOR_DIR_CCW );
SMotor_Set_Speed( 160 ); // 設置低速啟動
HAL_TIM_OC_Start_IT(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
}
// 采樣和控制周期為20ms
if(Time_Flag & 0x01)
{
//獲得編碼器的脈沖值
Enc_Cap = YS_Encoder_GetCounting();
//M法測速度
MSF = Enc_Cap - lastEnc_Cap;
lastEnc_Cap = Enc_Cap;
MSF = abs(MSF);
Exp_Val = IncPIDCalc(MSF, Vel_Target);
Exp_Val = fabs(Exp_Val);
SMotor_Set_Speed(Exp_Val);
Time_Flag &= ~0x01;
}
// 數據發送周期為1s, 顯示轉速
if(Time_Flag & 0x02)
{
int32_t pulse = YS_Encoder_GetCounting();
static int32_t tmp = 0;
float speed = (float)( pulse - tmp ) / ENCODER_RESOLUTION ;
printf("1s內的編碼器計數值: %d\n", pulse - tmp);
printf("電機轉速: %.2f r/s \n", speed );
printf("移動速度: %.1f mm/s \n", speed*5.0f);// 1r就是5mm
tmp = pulse ;
Time_Flag &= ~0x02;
}
}
}
/**
* 函數功能: 系統滴答定時器中斷回調函數
* 輸入參數: 無
* 返回值: 無
* 說明: 每發生一次滴答定時器中斷進入該回調函數一次
*/
void HAL_SYSTICK_Callback(void)
{
// 每1ms自動增一
time_count++;
if( (time_count % INTERVAL) == 0)// 20ms讀取一次編碼器數值
{
if(Start_flag )
Time_Flag |= 0x01;
}
else if(time_count >= 1000) // 1s發送一次數據
{
Time_Flag |= 0x02;
time_count = 0;
}
}
/**
* 函數功能: 定時器比較輸出中斷回調函數
* 輸入參數: htim：定時器句柄指針
* 返回值: 無
* 說明: 控制脈沖輸出
*/
void HAL_TIM_OC_DelayElapsedCallback( TIM_HandleTypeDef * htim)
{
uint32_t Toggle = 0;
if(htim == &htimx_SMotor1)
{
/* 設置下一次中斷的間隔 */
Toggle = __HAL_TIM_GET_COMPARE(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
Toggle += SMotor1.PulseWidth; // 計算實際的比較值
__HAL_TIM_SET_COMPARE(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx, (uint16_t)Toggle);
}
}
/**
* 函數功能: 系統時鐘配置
* 輸入參數: 無
* 返回值: 無
* 說明: 無
*/
static void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct={0};
RCC_ClkInitTypeDef RCC_ClkInitStruct={0};
/* 使能PWR配置更新 */
MODIFY_REG(PWR->CR3, PWR_CR3_SCUEN, 0);
/* 設置調壓器輸出電壓級別1 */
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/* 等待D3區域VOS 輸出準備就緒 */
while ((PWR->D3CR & (PWR_D3CR_VOSRDY)) != PWR_D3CR_VOSRDY) // 等待內核電源就緒
{
}
/* RCC 內部/外部晶振配置 ,用于CPU，AHB，APH總線時鐘 */
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;// 外部晶振
RCC_OscInitStruct.HSEState = RCC_HSE_ON;//選擇外部時鐘晶振 HSE 25MHz
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;//使能PLL
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;//PLL時鐘源選擇HSE
RCC_OscInitStruct.PLL.PLLM = 5; // 外部時鐘2分頻,得到5MHz 作為PLL時鐘源
RCC_OscInitStruct.PLL.PLLN = 160;// PLL 160倍頻,得到800MHz PLLCLK
RCC_OscInitStruct.PLL.PLLP = 2; // PLLCLK 2分頻得到400MHz SYSCLK
RCC_OscInitStruct.PLL.PLLQ = 2; // PLLCLK 2分頻得到400MHz用于部分外設的時鐘
RCC_OscInitStruct.PLL.PLLR = 2; // PLLCLK 2分頻得到400MHz用于部分外設的時鐘
RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2;// PLL時鐘輸入范圍4-8MHz
RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;// PLL時鐘輸出范圍192-836MHz
RCC_OscInitStruct.PLL.PLLFRACN = 0; // 此參數用于微調 PLL1 VCO 范圍0~2^13-1
HAL_RCC_OscConfig(&RCC_OscInitStruct);
/* RCC CPU，AHB，APH總線時鐘配置*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
|RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;//SYSCLK 時鐘源400MHz
RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;// SYSCLK 1分頻 HCLK=400MHz
RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; // HCLK 2分頻，AHB = 200Mhz
RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; // APB3 2分頻，APB3 = 100MHz
RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; // APB1 2分頻，APB1 = 100MHz
RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; // APB2 2分頻，APB2 = 100MHz
RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; // APB4 2分頻，APB4 = 100MHz
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
/* 使能時鐘安全機制 */
HAL_RCC_EnableCSS();
/* 滴答定時器配置1ms */
// SystemCoreClock/1000 1ms中斷一次
// SystemCoreClock/100000 10us中斷一次
// SystemCoreClock/1000000 1us中斷一次
HAL_SYSTICK_Config(SystemCoreClock/(1000));
/* 系統滴答定時器時鐘源 */
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* 系統滴答定時器中斷優先級配置 */
HAL_NVIC_SetPriority(SysTick_IRQn, 1, 1);
}
/**
* 函數功能: 使能CPU L1-Cache
* 輸入參數: 無
* 返回值: 無
* 說明: 無
*/
static void CPU_CACHE_Enable(void)
{
/* 使能 I-Cache */
SCB_EnableICache();
/* 使能 D-Cache */
SCB_EnableDCache();
}

復制代碼

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

57&42步進電機_速度閉環_位置式PID STM32源程序