STM32控制編碼器直流有刷電機(jī)位置式PID速度閉環(huán)實(shí)現(xiàn)
/**
? ******************************************************************************
? * 文件名程: main.c?
?
? */
/* 包含頭文件 ----------------------------------------------------------------*/
#include "stm32f1xx_hal.h"
#include "DCMotor/bsp_L298N.h"
#include "key/bsp_key.h"
#include "usart/bsp_debug_usart.h"
#include "DCMotor/bsp_encoder.h"
/* 私有類型定義 --------------------------------------------------------------*/
//定義PID結(jié)構(gòu)體
typedef struct?
{
? __IO int? ? ? SetPoint;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//設(shè)定目標(biāo) Desired Value
? __IO long? ? ?SumError;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//誤差累計(jì)
? __IO double? ?Proportion;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//比例常數(shù) Proportional Const
? __IO double? ?Integral;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//積分常數(shù) Integral Const
? __IO double? ?Derivative;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//微分常數(shù) Derivative Const
? __IO int? ? ? LastError;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //Error[-1]
? __IO int? ? ? PrevError;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //Error[-2]
}PID;
/* 私有宏定義 ----------------------------------------------------------------*/
/*************************************/
//定義PID相關(guān)宏
// 這三個(gè)參數(shù)設(shè)定對電機(jī)運(yùn)行影響非常大
/*************************************/
#define? P_DATA? ? ? 3.2? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//P參數(shù)
#define? I_DATA? ? ? 1.1? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //I參數(shù)
#define? D_DATA? ? ? -0.15? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //D參數(shù)
/* 私有變量 ------------------------------------------------------------------*/
__IO uint16_t time_count=0;? ? ? ? // 時(shí)間計(jì)數(shù)，每1ms增加一(與滴定時(shí)器頻率有關(guān))
__IO uint32_t CaptureNumber=0;? ? ?// 輸入捕獲數(shù)
__IO uint8_t? start_flag=0;
__IO double encoder_speed=0;
static PID sPID;
static PID *sptr = &sPID;
/* 擴(kuò)展變量 ------------------------------------------------------------------*/
/* 私有函數(shù)原形 --------------------------------------------------------------*/
void IncPIDInit(void);
int IncPIDCalc(int NextPoint);
unsigned int LocPIDCalc(int NextPoint);
/* 函數(shù)體 --------------------------------------------------------------------*/
/**
? * 函數(shù)功能: 系統(tǒng)時(shí)鐘配置
? * 輸入?yún)?shù): 無
? * 返 回 值: 無
? * 說? ? 明: 無
? */
void SystemClock_Config(void)
{
? RCC_OscInitTypeDef RCC_OscInitStruct;
? RCC_ClkInitTypeDef RCC_ClkInitStruct;
? RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;? // 外部晶振，8MHz
? RCC_OscInitStruct.HSEState = RCC_HSE_ON;
? RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
? RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
? RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
? RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;? // 9倍頻，得到72MHz主時(shí)鐘
? HAL_RCC_OscConfig(&RCC_OscInitStruct);
? RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
? RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;? ? ? ?// 系統(tǒng)時(shí)鐘：72MHz
? RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;? ? ? ? ? ? ? // AHB時(shí)鐘：72MHz
? RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;? ? ? ? ? ? ? ?// APB1時(shí)鐘：36MHz
? RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;? ? ? ? ? ? ? ?// APB2時(shí)鐘：72MHz
? HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
? // HAL_RCC_GetHCLKFreq()/1000? ? 1ms中斷一次
// HAL_RCC_GetHCLKFreq()/100000 10us中斷一次
// HAL_RCC_GetHCLKFreq()/1000000 1us中斷一次
? HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);? // 配置并啟動(dòng)系統(tǒng)滴答定時(shí)器
? /* 系統(tǒng)滴答定時(shí)器時(shí)鐘源 */
? HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
? /* 系統(tǒng)滴答定時(shí)器中斷優(yōu)先級(jí)配置 */
? HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}
/**
? * 函數(shù)功能: 主函數(shù).
? * 輸入?yún)?shù): 無
? * 返 回 值: 無
? * 說? ? 明: 無
? */
int main(void)
{?
? /* 復(fù)位所有外設(shè)，初始化Flash接口和系統(tǒng)滴答定時(shí)器 */
? HAL_Init();
? /* 配置系統(tǒng)時(shí)鐘 */
? SystemClock_Config();
? KEY_GPIO_Init();
? MX_DEBUG_USART_Init();
??
? IncPIDInit();
??
? ENCODER_TIMx_Init();?
? HAL_TIM_Base_Start(&htimx_ENCODER);??
??
? /* 高級(jí)控制定時(shí)器初始化并配置PWM輸出功能 */
? L298N_TIMx_Init();
? /* 啟動(dòng)定時(shí)器 */
? HAL_TIM_Base_Start(&htimx_L298N);
??
? HAL_TIM_IC_Start_IT(&htimx_ENCODER,ENCODER_TIM_CHANNELx);
??
? /* 啟動(dòng)定時(shí)器通道和互補(bǔ)通道PWM輸出 */
? L298N_DCMOTOR_Contrl(1,2,0);
? start_flag=1;?
??
? printf("增量式PID算法控制電機(jī)旋轉(zhuǎn)\n");
? /* 無限循環(huán) */
? while (1)
? {
? ? if(KEY1_StateRead()==KEY_DOWN)? // 增速
? ? {
? ? ? /* 設(shè)置目標(biāo)速度 */
? ? ? sptr->SetPoint =50;? ??
? ? }
? ? if(KEY2_StateRead()==KEY_DOWN)? // 減速
? ? {
? ? ? /* 設(shè)置目標(biāo)速度 */
? ? ? sptr->SetPoint =300;
? ? }? ??
? }
}
/**
? * 函數(shù)功能: 系統(tǒng)滴答定時(shí)器中斷回調(diào)函數(shù)
? * 輸入?yún)?shù): 無
? * 返 回 值: 無
? * 說? ? 明: 每發(fā)生一次滴答定時(shí)器中斷進(jìn)入該回調(diào)函數(shù)一次
? */
void HAL_SYSTICK_Callback(void)
{
? if(start_flag) // 等待脈沖輸出后才開始計(jì)時(shí)
? {
? ? time_count++;? ? ? ? ?// 每1ms自動(dòng)增一
? ? if(time_count==200)
? ? {
? ? ? __IO uint32_t count;
? ? ? __IO double cal;
? ? ??
? ? ? /* 得到編碼器計(jì)數(shù)值，數(shù)值越大說明速度越大 */
? ? ? count=CaptureNumber;?
? ? ? CaptureNumber=0;? ? // 清零，從零開始計(jì)數(shù)
? ? ??
? ? ? /* 計(jì)數(shù)得到位置式PID的位置數(shù)值 */
? ? ? PWM_Duty=LocPIDCalc(count);? ? ??
? ? ? if(PWM_Duty>899)PWM_Duty=899;? ? ? ??
? ? ??
? ? ? // 11：編碼器線數(shù)(轉(zhuǎn)速一圈輸出脈沖數(shù))
? ? ? // 34：電機(jī)減數(shù)比，內(nèi)部電機(jī)轉(zhuǎn)動(dòng)圈數(shù)與電機(jī)輸出軸轉(zhuǎn)動(dòng)圈數(shù)比，即減速齒輪比? ? ??
? ? ? cal=sptr->SetPoint;
? ? ? printf("\n設(shè)定目標(biāo)速度 -> 編碼器在%ds時(shí)間計(jì)數(shù)%d個(gè)脈沖\n",time_count,sptr->SetPoint);? ? ??
? ? ? printf("? ? ? ? ? ? ? ? 相當(dāng)于實(shí)際目標(biāo)速度為：%0.2f圈/s\n",cal*(1000/time_count)/11/34);
? ? ??
? ? ? cal=count;
? ? ? printf("當(dāng)前電機(jī)速度-> 編碼器在%ds時(shí)間計(jì)數(shù)%d個(gè)脈沖\n",time_count,count);
? ? ? printf("? ? ? ? ? ? ? ? 相當(dāng)于當(dāng)前實(shí)際速度為：%0.2f圈/s\n",cal*(1000/time_count)/11/34);
? ? ??
? ? ? printf("設(shè)置新的占空比為：%d\n",PWM_Duty);
? ? ??
? ? ? L298N_DCMOTOR_Contrl(1,2,PWM_Duty);?
? ? ? time_count=0;? ? ??
? ? }
? }
}
/**
? * 函數(shù)功能: 定時(shí)器輸入捕獲中斷回調(diào)函數(shù)
? * 輸入?yún)?shù): htim：定時(shí)器句柄
? * 返 回 值: 無
? * 說? ? 明: 無
? */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
? CaptureNumber++;
}
/**************PID參數(shù)初始化********************************/
void IncPIDInit(void)?
{
? ? sptr->LastError=0;? ? ? ? ? ? //Error[-1]
? ? sptr->PrevError=0;? ? ? ? ? ? //Error[-2]
? ? sptr->Proportion=P_DATA;? ? ? //比例常數(shù) Proportional Const
? ? sptr->Integral=I_DATA;? ? ? ? //積分常數(shù)? Integral Const
? ? sptr->Derivative=D_DATA;? ? ? //微分常數(shù) Derivative Const
? ? sptr->SetPoint=100;? ? ? ? ? ?//設(shè)定目標(biāo)Desired Value
}
/********************位置式 PID 控制設(shè)計(jì)************************************/
unsigned int LocPIDCalc(int NextPoint)
{
? int iError,dError;
? iError = sptr->SetPoint - NextPoint; //偏差
? sptr->SumError += iError; //積分
? dError = iError - sptr->LastError; //微分
? sptr->LastError = iError;
? return(sptr->Proportion * iError //比例項(xiàng)
? + sptr->Integral * sptr->SumError //積分項(xiàng)
? + sptr->Derivative * dError); //微分項(xiàng)
}
