原文地址:http://www.amobbs.com/thread-5575823-1-1.html
硬件部分:
控制系統的控制對象是4個空心杯直流電機,電機帶光電編碼器,可以反饋轉速大小的波形。電機驅動模塊是普通的L298N模塊。
芯片型號,STM32F103ZET6
軟件部分:
PWM輸出:TIM3,可以直接輸出4路不通占空比的PWM波
PWM捕獲:STM32除了TIM6 TIM7其余的都有捕獲功能,使用TIM1 TIM2 TIM4 TIM5四個定時器捕獲四個反饋信號
PID的采樣和處理:使用了基本定時器TIM6,溢出時間就是我的采樣周期,理論上T越小效果會越好,這里我取20ms,依據控制對象吧,如果控制水溫什么的采樣周期會是幾秒幾分鍾什么的。
上面的PWM輸出和捕獲關於定時器的設置都有例程,我這里是這樣的:
TIM3輸出四路PWM,在引腳 C 的 GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9輸出
四路捕獲分別是TIM4 TIM1 TIM2 TIM5 ,對應引腳是: PB7 PE11 PB3 PA1
高級定時器tim1的初始化略不同,它的中斷”名稱“和通用定時器不同,見代碼:
- /*功能名稱IM3_PWM_Init(u16 arr,u16 psc)
- 描述 TIM3產生四路PWM
- */
- void TIM3_PWM_Init(u16 arr,u16 psc)
- {
- GPIO_InitTypeDef GPIO_InitStructure;
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE); //使能GPIO外設和AFIO復用功能模塊時鍾使能
- GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE); //Timer3全映射 GPIOC-> 6,7,8,9 //用於TIM3的CH2輸出的PWM通過該LED顯示
- //設置該引腳為復用輸出功能,輸出TIM3 CH1 CH2 CH3 CH4 的PWM脈沖波形
- GPIO_InitStructure.GPIO_Pin =GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; //初始化GPIO
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //復用推挽輸出
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOC, &GPIO_InitStructure);
- GPIO_ResetBits(GPIOC,GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9);//默認電機使能端狀態:不使能
- TIM_TimeBaseStructure.TIM_Period = arr; //設置在下一個更新事件裝入活動的自動重裝載寄存器周期的值
- TIM_TimeBaseStructure.TIM_Prescaler =psc; //設置用來作為TIMx時鍾頻率除數的預分頻值 這里是72分頻,那么時鍾頻率就是1M
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鍾分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
- TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx的時間基數單位
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //選擇定時器模式:TIM脈沖寬度調制模式1
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比較輸出使能
- TIM_OCInitStructure.TIM_Pulse = 0; //設置待裝入捕獲比較寄存器的脈沖值
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //輸出極性:TIM輸出比較極性高
- TIM_OC1Init(TIM3, &TIM_OCInitStructure); //根據TIM_OCInitStruct中指定的參數初始化外設TIMx
- TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIMx在CCR1上的預裝載寄存器
- TIM_OC2Init(TIM3, &TIM_OCInitStructure); //根據TIM_OCInitStruct中指定的參數初始化外設TIMx
- TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIMx在CCR2上的預裝載寄存器
- TIM_OC3Init(TIM3, &TIM_OCInitStructure); //根據TIM_OCInitStruct中指定的參數初始化外設TIMx
- TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIMx在CCR3上的預裝載寄存器
- TIM_OC4Init(TIM3, &TIM_OCInitStructure); //根據TIM_OCInitStruct中指定的參數初始化外設TIMx
- TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIMx在CCR4上的預裝載寄存器
- TIM_ARRPreloadConfig(TIM3, ENABLE); //使能TIMx在ARR上的預裝載寄存器
- TIM_Cmd(TIM3, ENABLE); //使能TIMx外設
- }
- /*功能名稱TIM4_PWMINPUT_INIT(u16 arr,u16 psc)
- 描述 PWM輸入初始化*/
- void TIM4_PWMINPUT_INIT(u16 arr,u16 psc)
- {
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //TIM的初始化結構體
- NVIC_InitTypeDef NVIC_InitStructure; //中斷配置
- TIM_ICInitTypeDef TIM4_ICInitStructure; //TIM4 PWM配置結構體
- GPIO_InitTypeDef GPIO_InitStructure; //IO口配置結構體
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //Open TIM4 clock
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //open gpioB clock
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; //GPIO 7
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //上拉輸入
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
- TIM_TimeBaseStructure.TIM_Period = arr; //設置在下一個更新事件裝入活動的自動重裝載寄存器周期的值
- TIM_TimeBaseStructure.TIM_Prescaler =psc; //設置用來作為TIMx時鍾頻率除數的預分頻值
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鍾分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
- TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx的時間基數單位
- /*配置中斷優先級*/
- NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- TIM4_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM4_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM4_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM4_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM4_ICInitStructure.TIM_ICFilter = 0x3; //Filter:過濾
- TIM_PWMIConfig(TIM4, &TIM4_ICInitStructure); //PWM輸入配置
- TIM_SelectInputTrigger(TIM4, TIM_TS_TI2FP2); //選擇有效輸入端
- TIM_SelectSlaveMode(TIM4, TIM_SlaveMode_Reset); //配置為主從復位模式
- TIM_SelectMasterSlaveMode(TIM4, TIM_MasterSlaveMode_Enable);//啟動定時器的被動觸發
- TIM_ITConfig(TIM4, TIM_IT_CC2|TIM_IT_Update, ENABLE); //中斷配置
- TIM_ClearITPendingBit(TIM4, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- TIM_Cmd(TIM4, ENABLE);
- }
- void TIM4_IRQHandler(void)
- {
- if (TIM_GetITStatus(TIM4, TIM_IT_CC2) != RESET)//捕獲1發生捕獲事件
- {
- duty_TIM4 = TIM_GetCapture1(TIM4); //采集占空比
- if (TIM_GetCapture2(TIM4)>600) period_TIM4 = TIM_GetCapture2(TIM4);//簡單的處理
- CollectFlag_TIM4 = 0;
- }
- TIM_ClearITPendingBit(TIM4, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- }
- /*功能名稱TIM1_PWMINPUT_INIT(u16 arr,u16 psc)
- 描述 PWM輸入初始化*/
- void TIM1_PWMINPUT_INIT(u16 arr,u16 psc)
- {
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //TIM的初始化結構體
- NVIC_InitTypeDef NVIC_InitStructure; //中斷配置
- TIM_ICInitTypeDef TIM1_ICInitStructure; //PWM配置結構體
- GPIO_InitTypeDef GPIO_InitStructure; //IO口配置結構體
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); //Open TIM1 clock
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOE, ENABLE); //open gpioE clock
- GPIO_PinRemapConfig(GPIO_FullRemap_TIM1, ENABLE); //Timer1完全重映射 TIM1_CH2->PE11
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; //GPIO 11
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //上拉輸入
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOE, &GPIO_InitStructure);
- TIM_TimeBaseStructure.TIM_Period = arr; //設置在下一個更新事件裝入活動的自動重裝載寄存器周期的值
- TIM_TimeBaseStructure.TIM_Prescaler =psc; //設置用來作為TIMx時鍾頻率除數的預分頻值
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鍾分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
- TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx的時間基數單位
- /*配置中斷優先級*/
- NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; //TIM1捕獲中斷
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- TIM1_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM1_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM1_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM1_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM1_ICInitStructure.TIM_ICFilter = 0x03; //Filter:過濾
- TIM_PWMIConfig(TIM1, &TIM1_ICInitStructure); //PWM輸入配置
- TIM_SelectInputTrigger(TIM1, TIM_TS_TI2FP2); //選擇有效輸入端
- TIM_SelectSlaveMode(TIM1, TIM_SlaveMode_Reset); //配置為主從復位模式
- TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);//啟動定時器的被動觸發
- // TIM_ITConfig(TIM1, TIM_IT_CC2|TIM_IT_Update, ENABLE); //中斷配置
- TIM_ITConfig(TIM1, TIM_IT_CC2, ENABLE); //通道2 捕獲中斷打開
- //TIM_ClearITPendingBit(TIM1, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- TIM_Cmd(TIM1, ENABLE);
- }
- void TIM1_CC_IRQHandler(void)
- {
- {
- if (TIM_GetITStatus(TIM1, TIM_IT_CC2) != RESET)//捕獲1發生捕獲事件
- {
- duty_TIM1 = TIM_GetCapture1(TIM1); //采集占空比
- if (TIM_GetCapture2(TIM1)>600) period_TIM1 = TIM_GetCapture2(TIM1);
- CollectFlag_TIM1 = 0;
- }
- }
- TIM_ClearITPendingBit(TIM1, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- }
- /*功能名稱TIM2_PWMINPUT_INIT(u16 arr,u16 psc)
- 描述 PWM輸入初始化*/
- void TIM2_PWMINPUT_INIT(u16 arr,u16 psc)
- {
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //TIM的初始化結構體
- NVIC_InitTypeDef NVIC_InitStructure; //中斷配置
- TIM_ICInitTypeDef TIM2_ICInitStructure; //TIM2 PWM配置結構體
- GPIO_InitTypeDef GPIO_InitStructure; //IO口配置結構體
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //Open TIM2 clock
- // RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //open gpioB clock
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); //使能GPIO外設和AFIO復用功能模塊時鍾
- GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); //關閉JTAG
- GPIO_PinRemapConfig(GPIO_FullRemap_TIM2, ENABLE); //Timer2完全重映射 TIM2_CH2->PB3
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3; //GPIO 3
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //浮空輸入 上拉輸入
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
- TIM_TimeBaseStructure.TIM_Period = arr; //設置在下一個更新事件裝入活動的自動重裝載寄存器周期的值
- TIM_TimeBaseStructure.TIM_Prescaler =psc; //設置用來作為TIMx時鍾頻率除數的預分頻值
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鍾分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
- TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx的時間基數單位
- /*配置中斷優先級*/
- NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- TIM2_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM2_ICInitStructure.TIM_ICFilter = 0x3; //Filter:過濾
- TIM_PWMIConfig(TIM2, &TIM2_ICInitStructure); //PWM輸入配置
- TIM_SelectInputTrigger(TIM2, TIM_TS_TI2FP2); //選擇有效輸入端
- TIM_SelectSlaveMode(TIM2, TIM_SlaveMode_Reset); //配置為主從復位模式
- TIM_SelectMasterSlaveMode(TIM2, TIM_MasterSlaveMode_Enable);//啟動定時器的被動觸發
- TIM_ITConfig(TIM2, TIM_IT_CC2|TIM_IT_Update, ENABLE); //中斷配置
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- TIM_Cmd(TIM2, ENABLE);
- }
- void TIM2_IRQHandler(void)
- {
- {
- if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)//捕獲1發生捕獲事件
- {
- duty_TIM2 = TIM_GetCapture1(TIM2); //采集占空比
- if (TIM_GetCapture2(TIM2)>600) period_TIM2 = TIM_GetCapture2(TIM2);
- CollectFlag_TIM2 = 0;
- }
- }
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- }
- /*功能名稱TIM5_PWMINPUT_INIT(u16 arr,u16 psc)
- 描述 PWM輸入初始化*/
- void TIM5_PWMINPUT_INIT(u16 arr,u16 psc)
- {
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //TIM的初始化結構體
- NVIC_InitTypeDef NVIC_InitStructure; //中斷配置
- TIM_ICInitTypeDef TIM5_ICInitStructure; //TIM4 PWM配置結構體
- GPIO_InitTypeDef GPIO_InitStructure; //IO口配置結構體
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); //Open TIM4 clock
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //open gpioB clock
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; //GPIO 1
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //浮空輸入 上拉輸入
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOA, &GPIO_InitStructure);
- TIM_TimeBaseStructure.TIM_Period = arr; //設置在下一個更新事件裝入活動的自動重裝載寄存器周期的值
- TIM_TimeBaseStructure.TIM_Prescaler =psc; //設置用來作為TIMx時鍾頻率除數的預分頻值
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鍾分割:TDTS = Tck_tim
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
- TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx的時間基數單位
- /*配置中斷優先級*/
- NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- TIM5_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM5_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM5_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM5_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM5_ICInitStructure.TIM_ICFilter = 0x3; //Filter:過濾
- TIM_PWMIConfig(TIM5, &TIM5_ICInitStructure); //PWM輸入配置
- TIM_SelectInputTrigger(TIM5, TIM_TS_TI2FP2); //選擇有效輸入端
- TIM_SelectSlaveMode(TIM5, TIM_SlaveMode_Reset); //配置為主從復位模式
- TIM_SelectMasterSlaveMode(TIM5, TIM_MasterSlaveMode_Enable);//啟動定時器的被動觸發
- TIM_ITConfig(TIM5, TIM_IT_CC2|TIM_IT_Update, ENABLE); //中斷配置
- TIM_ClearITPendingBit(TIM5, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- TIM_Cmd(TIM5, ENABLE);
- }
- void TIM5_IRQHandler(void)
- {
- {
- if (TIM_GetITStatus(TIM5, TIM_IT_CC2) != RESET)//捕獲1發生捕獲事件
- {
- duty_TIM5 = TIM_GetCapture1(TIM5); //采集占空比
- if (TIM_GetCapture2(TIM5)>600) period_TIM5 = TIM_GetCapture2(TIM5);
- CollectFlag_TIM5 = 0;
- }
- }
- TIM_ClearITPendingBit(TIM5, TIM_IT_CC2|TIM_IT_Update); //清除中斷標志位
- }
PID部分:
准備部分:先定義PID結構體:
- typedef struct
- {
- int setpoint;//設定目標
- int sum_error;//誤差累計
- float proportion ;//比例常數
- float integral ;//積分常數
- float derivative;//微分常數
- int last_error;//e[-1]
- int prev_error;//e[-2]
- }PIDtypedef;
這里注意一下成員的數據類型,依據實際需要來定的。
在文件中定義幾個關鍵變量:
- float Kp = 0.32 ; //比例常數
- float Ti = 0.09 ; //積分時間常數
- float Td = 0.0028 ; //微分時間常數
- #define T 0.02 //采樣周期
- #define Ki Kp*(T/Ti) // Kp Ki Kd 三個主要參數
- #define Kd Kp*(Td/T)
C語言好像用#define 什么什么對程序不太好,各位幫忙寫個優化辦法看看呢? 用const?
PID.H里面主要的幾個函數:
- void PIDperiodinit(u16 arr,u16 psc); //PID 采樣定時器設定
- void incPIDinit(void); //初始化,參數清零清零
- int incPIDcalc(PIDtypedef*PIDx,u16 nextpoint); //PID計算
- void PID_setpoint(PIDtypedef*PIDx,u16 setvalue); //設定 PID預期值
- void PID_set(float pp,float ii,float dd);//設定PID kp ki kd三個參數
- void set_speed(float W1,float W2,float W3,float W4);//設定四個電機的目標轉速
PID處理過程:
岔開一下:這里我控制的是電機的轉速w,實際上電機的反饋波形的頻率f、電機轉速w、控制信號PWM的占空比a三者是大致線性的正比的關系,這里強調這個的目的是
因為樓主在前期一直搞不懂我控制的轉速怎么和TIM4輸出的PWM的占空比聯系起來,后來想清楚里面的聯系之后通過公式把各個系數算出來了。
正題:控制流程是這樣的,首先我設定我需要的車速(對應四個輪子的轉速),然后PID就是開始響應了,它先采樣電機轉速,得到偏差值E,帶入PID計算公式,得到調整量也就是最終更改了PWM的占空比,不斷調節,直到轉速在穩態的一個小范圍上下浮動。
上面講到的“得到調整量”就是增量PID的公式:
- int incPIDcalc(PIDtypedef *PIDx,u16 nextpoint)
- {
- int iError,iincpid;
- iError=PIDx->setpoint-nextpoint; //當前誤差
- /*iincpid= //增量計算
- PIDx->proportion*iError //e[k]項
- -PIDx->integral*PIDx->last_error //e[k-1]
- +PIDx->derivative*PIDx->prev_error;//e[k-2]
- */
- iincpid= //增量計算
- PIDx->proportion*(iError-PIDx->last_error)
- +PIDx->integral*iError
- +PIDx->derivative*(iError-2*PIDx->last_error+PIDx->prev_error);
- PIDx->prev_error=PIDx->last_error; //存儲誤差,便於下次計算
- PIDx->last_error=iError;
- return(iincpid) ;
- }
注釋掉的是第一種寫法,沒注釋的是第二種以Kp KI kd為系數的寫法,實際結果是一樣的。
處理過程放在了TIM6,溢出周期時間就是是PID里面采樣周期(區分於反饋信號的采樣,反饋信號采樣是1M的頻率)
相關代碼:
- void TIM6_IRQHandler(void) // 采樣時間到,中斷處理函數
- {
- if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)//更新中斷
- {
- frequency1=1000000/period_TIM4 ; //通過捕獲的波形的周期算出頻率
- frequency2=1000000/period_TIM1 ;
- frequency3=1000000/period_TIM2 ;
- frequency4=1000000/period_TIM5 ;
- /********PID1處理**********/
- PID1.sum_error+=(incPIDcalc(&PID1,frequency1)); //計算增量並累加
- pwm1=PID1.sum_error*4.6875 ; //pwm1 代表將要輸出PWM的占空比
- frequency1=0; //清零
- period_TIM4=0;
- /********PID2處理**********/
- PID2.sum_error+=(incPIDcalc(&PID2,frequency2)); //計算增量並累加 Y=Y+Y'
- pwm2=PID2.sum_error*4.6875 ; //將要輸出PWM的占空比
- frequency2=0;
- period_TIM1=0;
- /********PID3處理**********/
- PID3.sum_error+=(incPIDcalc(&PID3,frequency3)); //常規PID控制
- pwm3=PID3.sum_error*4.6875 ; //將要輸出PWM的占空比
- frequency3=0;
- period_TIM2=0;
- /********PID4處理**********/
- PID4.sum_error+=(incPIDcalc(&PID4,frequency4)); //計算增量並累加
- pwm4=PID4.sum_error*4.6875 ; //將要輸出PWM的占空比
- frequency4=0;
- period_TIM5=0;
- }
- TIM_SetCompare(pwm1,pwm2,pwm3,pwm4); //重新設定PWM值
- TIM_ClearITPendingBit(TIM6, TIM_IT_Update); //清除中斷標志位
- }
上面幾個代碼是PID實現的關鍵部分
整定過程:
辦法有不少,這里用的是先KP,再TI,再TD,在微調。其他的辦法特別是有個尼古拉斯法我發現不適合我這個控制對象。
先Kp,就是消除積分和微分部分的影響,這里我糾結過到底是讓Ti 等於一個很大的值讓Ki=Kp*(T/Ti)里面的KI接近零,還是直接定義KI=0,TI=0.
然后發現前者沒法找到KP使系統震盪的臨界值,第二個辦法可以得到預期的效果:即KP大了會產生震盪,小了會讓系統穩定下來,當然這個時候是有穩態誤差的。
隨后把積分部分加進去,KI=Kp*(T/Ti)這個公式用起來,並且不斷調節TI 。TI太大系統穩定時間比較長。
然后加上Kd =Kp*(Td/T),對於系統響應比較滯后的情況效果好像好一些,我這里的電機反映挺快的,所以Td值很小。
最后就是幾個參數調節一下,讓波形好看一點。這里的波形實際反映的是采集回來的轉速值,用STM32的DAC功能輸出和轉速對應的電壓,用示波器采集的。
關於定時器的一些設置做在了main函數里面,上面沒給出,現在貼出來!
float Kp = 0.32 ; //比例常數
float Ti = 0.09 ; //積分常數
float Td = 0.015 ; //微分常數
#define T 0.02 //采樣周期
//#define Ka Kp*(1+(T/Ti)+(Td/T)) //另一種公式的三個參數
//#define Kb (Kp)*(1+(2*Td/T))
//#define Kc Kp*Td/T
#define Ki Kp*(T/Ti) // Kp Ki Kd 三個主要參數
#define Kd Kp*(Td/T)
u8 start_flag=0;
u16 pwm1=0,pwm2=0,pwm3=0,pwm4=0; //PWM 波形占空量 占空比=PWMx/7200
u8 flag_lcd=0;//液晶屏幕更新標志
u8 flag_bluetooth =0;//藍牙驗證狀態 1:已發出驗證信息 0:未發出驗證信息
u8 status_bluetooth=0;//藍牙連接狀態位 1:已連接 0:未連接
int main(void)
{
u8 len ,t;
SystemInit();
delay_init(72); //延時初始化
NVIC_Configuration(); //中斷配置 中斷分組2:2位搶占優先級,2位響應優先級
init_LCD_IO() ; //初始化LCD控制引腳 PG4 5
uart_init(9600); //串口初始化
lcd_init(); //LCD顯示
LED_GPIO_Config(); // led 初始化
MOTOR_INIT();
Dac1_Init(); //DAC初始化
incPIDinit(); //PID初始化 置零
fuzzy_init() ;
KEY_Init();
EXTIX_Init(); //外部中斷初始化
TIM3_PWM_Init(7200-1,1-1); //參數1*參數2/(72e6)=1/f f:需要的電機頻率
//PWM輸出 頻率:1KHZ pwm周期:1000us 參數: 1000-1 72-1 定時器頻率 1M 特點;電機頻率太低,電機噪音,精度Vmax/1000
//PWM輸出 頻率:10KHZ pwm周期:100us 參數: 100-1 72-1 定時器頻率 1M 特點;頻率合適,控制精度太低
//PWM輸出 頻率:10KHZ 周期:7200 參數: 1000-1 72-1
TIM4_PWMINPUT_INIT(0xffff,72-1); //pwm輸入初始化以1M的頻率捕捉
TIM1_PWMINPUT_INIT(0xffff,72-1); //pwm輸入初始化以1M的頻率捕捉
TIM2_PWMINPUT_INIT(0xffff,72-1); //pwm輸入初始化以1M的頻率捕捉
TIM5_PWMINPUT_INIT(0xffff,72-1); //pwm輸入初始化以1M的頻率捕捉
MOTOR_OUT(1,0,1,0,1,0,1,0);//轉速全為正,速度都是0
PID_set(Kp,Ki,Kd); //初始化 PID參數
printf("Kp=%f\r\n",Kp); //輸出參數:便於調試觀察
printf("Ki=%f\r\n",Ki);
printf("Kd=%f\r\n",Kd);
// PID_setpoint(&PID1,500); //開機就設定輪子轉動,便於調試,可注釋掉
// PID_setpoint(&PID2,200);
// PID_setpoint(&PID3,500);
// PID_setpoint(&PID4,300);
PIDperiodinit(40,36000-1);
//設定PID采樣周期 T=20ms 72000 000/36 000 = 2 KHz 和 T 對應
//set_speed(3,3,3,3);
TIM_Cmd(TIM6, ENABLE); //使能TIMx 開啟PID處理
while(1)
{
// printf("Kp=%f\r\n",PID3.proportion2); //輸出參數:便於調試觀察
if(USART_RX_STA&0x8000) //如果完成一次接收
{
TIM_Cmd(TIM6, DISABLE); // 關閉PID運算
stop(); //PID相關參數清零,並且小車停止運動
len=USART_RX_STA&0x3fff;//得到此次接收到的數據長度,本控制系統應該len==15 或者 2
printf("MCU_GET:");
for(t=0;t<len;t++) //返回所以數值
{
USART_SendData(USART1, USART_RX_BUF[t]);//向串口1發送數據
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)!=SET);//等待發送結束
}
printf("\r\n");//插入換行
if(len==15) //長度15的是速度控制報文
{ LED1=0;// LIGHT ON
//報文格式 :x+正負號+x速度+y+正負號+y速度+w+正負號+w速度
//例如 :x+123y+000w+000,表示只有x方向速度的,其余均為零的
table1[3]=USART_RX_BUF[1];table1[4]=USART_RX_BUF[2]; table1[5]=USART_RX_BUF[3];table1[6]=USART_RX_BUF[4]; //更新到LCD
table2[3]=USART_RX_BUF[6];table2[4]=USART_RX_BUF[7]; table2[5]=USART_RX_BUF[8];table2[6]=USART_RX_BUF[9];
table3[3]=USART_RX_BUF[11];table3[4]=USART_RX_BUF[12]; table3[5]=USART_RX_BUF[13];table3[6]=USART_RX_BUF[14];
lcd_init(); //LCD顯示
X=(USART_RX_BUF[2]-0x30)*100+(USART_RX_BUF[3]-0x30)*10+(USART_RX_BUF[4]-0x30) ; //得到Vx
if(USART_RX_BUF[1]=='-')X=-X; //加上正負號
Y=(USART_RX_BUF[7]-0x30)*100+(USART_RX_BUF[8]-0x30)*10+(USART_RX_BUF[9]-0x30) ; //得到Xy
if(USART_RX_BUF[6]=='-')Y=-Y;
W=(USART_RX_BUF[12]-0x30)*100+(USART_RX_BUF[13]-0x30)*10+(USART_RX_BUF[14]-0x30) ; //得到w
W=W/100.0; //實際的W值縮小100倍
if(USART_RX_BUF[11]=='-')W=-W;
printf("X=%d\r\n",X); //參數反饋,便於調試
printf("Y=%d\r\n",Y);
printf("W=%f\r\n",W);
kinematics(X,Y,W,&W1,&W2,&W3,&W4); //運動學方程計算,得到四個輪子的轉速
printf("W1=%f\r\n",W1);
printf("W2=%f\r\n",W2);
printf("W3=%f\r\n",W3);
printf("W4=%f\r\n",W4);
// LED0=!LED0; //LED翻轉
set_speed(W1,W2,W3,W4); //設定輪子轉速,實際是更新了PID目標值
TIM_Cmd(TIM6, ENABLE); //使能TIMx 開啟PID
LED1=1;// LIGHT Off
}
if(len==2)
{
if ((USART_RX_BUF[0]=='B') && (USART_RX_BUF[1]=='L') && (flag_bluetooth==1))
{ status_bluetooth=1 ;
flag_bluetooth=0;
printf("blue_OK\r\n");
}
}
USART_RX_STA=0; //數據處理完畢,清除狀態寄存器,准備下組數據接收
len=0;
}
}
}
(1)這種調速方法,有沒有經過脈沖負載加載到電機上,看看速度調整的響應時間,還能和你示波器輸出的波形差不多?
示波器顯示的是反饋信號的頻率f 對應的DAC電壓,想不出別的辦法來看響應曲線了。也用示波器觀看電機過的輸入電壓值,發現看不出什么東西..
(2)pwm3=PID3.sum_error*4.6875 ; //將要輸出PWM的占空比,他的pid輸出與pwm是怎么對應的?
電機反饋信號的頻率f 和電機是成正比的,也就是說,我如果需要電機轉速 w=2pi rad/s的話,我對應的捕獲頻率是電機在這個轉速時光電編碼器反饋波形的頻率大小。所以我PID計算的其實是以這個頻率為標准的調整量,PID的設定值也是頻率,增量計算的也是頻率,當然你用轉速 w 和PWM占空比 a 做PID的計算也是可行的,只要找到 轉速=(系數1)*頻率=(系數2)*占空比 這個關系里面的系數就行。
我來說說這個4.6875怎么來的吧,這里我把占空比看成“占空量”(占空比:在一串理想的脈沖周期序列中(如方波),正脈沖的持續時間與脈沖總周期的比值。),如果PWM周期是1000份,高電平是300份,那我的占空量就是300,在STM32里面這個占空量是可以直接作為參數設定給定時器的,用的函數就是setcompare(),你自己查一下歷程看看。
好,我的電機的最大轉速是2rps,也就是一秒2圈,我的編碼器這個時候的反饋頻率應該是1536(查看編碼器的參數),這個時候占空比需要100%,也就是1000的占空量。電機不轉呢,反饋頻率就是0,占空量就是0。
簡單的就是占空量對應0頻率,1000占空量對應1536頻率。得到 占空量=0.651*頻率。
那我程序是4.6875呢?我繼續說!
例程里面關於PWM波形輸出的TIM3的初始化是:TIM3_PWM_Init(1000-1,72-1);
這樣就是72分頻,定時器頻率變成1M,對應成1 us計算很方便。1000是指PWM波形的周期是1000,這里正好是1000us(注意:這里1000這個參數越大,說明占空比的分辨率越高,但是在定時器頻率不變的前提下,pwm周期越大,輸出PWM的頻率就越小)。但是這樣的參數下,PWM的頻率只有1K,電機產出明顯的噪音,經過調試,電機在10K的頻率下控制的效果比較好。也就是說我要湊個10Kpwm輸出,但是pwm周期不能太小,咱要保證控制精度啊。
所以通過計算:TIM3_PWM_Init(7200-1,1-1);//定時器72M運行,周期7200份,電機頻率正好是10K
好了這里的7200放到之前的計算中,系數就是4.6875了。
(3)大哥,看了你的帖子收益很多 啊,我現在對pid的理解寫給你看看,不知道對不對啊,假設我2v的電壓---對應1000的占空量也就是100%,3v電壓對應------0的占空量也就是0%,2v和3v是我的最大和最小溫度時對應的電壓,采集到的溫度的電壓與占空比成反比,那我根據你的方法計算,占空量=1000/(3-2)*Vpid,我的設定值也是以電壓為單位的,假設設定的溫度對應的電壓值為2.5v,采集到的電壓為2.3v,那么把2.5v和2.3v帶入pid計算也就是Vpid=pid(2.5,2.3);那么最終的pwm值=(1000/(3-2))*pid(2.5,2.3)?
“2v的電壓---對應1000的占空量也就是100%,3v電壓對應------0的占空量也就是0%,2v和3v是我的最大和最小溫度時對應的電壓”光是這句話,你下面的式子就貌似有問題了。電壓以mV為單位,1000對應2000mV,0對應3000mV,那么占空量a和電壓u的關系就是:3000-u=a。個人認為,我這個思路對於線性系統或者近似線性系統應該問題不大,傳統PID對於線性系統是比較適用的,你這里一定要把握好各個物理量之間的變換關系。
(4)大哥,電壓以mV為單位,1000對應2000mV,0對應3000mV,那么占空量a和電壓u的關系就是:3000-u=a;你這里的3000是3000mv嗎?u是pid運算后得到的電壓嗎?你是把電壓放大了1000倍嗎?還有就是如果我的周期的份量是500,那么100%的時候占空量是500,那么他們的對應關系應該是500對應1000mv,0對應1500mv嗎?也就是把500對應2v,0對應3v時的電壓放大了500倍是嗎?
和PID處理無關,U就是你的電壓啊,a就是占空量
(5)不知樓主的電機加速過程中有加速算法沒有,我一直弄不明白加速算法怎么和PID控制結合起來?
我想做步進電機位置環和速度環,加速算法想用梯形加速或者S形加速。
加速算法是不是目的是讓小車走出設定的軌跡啊?
我樣機做出來了,可能會寫個矩形的運動軌跡,看看回程誤差。X Y方向不停測距差不多能實現。
(6)我想問一下啊 pwm輸出時鍾是72M,周期是7200,pwm捕捉又用的是1M時鍾,那捕捉到的占空量為什么會是一樣的啊?
捕獲的頻率1M能保證捕獲到的信號的周期是正負1us
輸出PWM的定時器72M的頻率。而PWM的周期是7200“份”,算一下PWM的頻率是10K。
捕獲到的是確確實實的高電平時間,能直接用讀取寄存器讀出來。和我輸出的時候設置的占空比或“占高量”沒有直接地聯系。
(7)樓主你好,
void PID_setpoint(PIDtypedef*PIDx,u16 setvalue); //設定 PID預期值
void set_speed(float W1,float W2,float W3,float W4);//設定四個電機的目標轉速
預期值難道不是目標轉速嗎?
簡單回答,預期值就是目標值,就是目標轉速(單位不一定是r/s)
void PID_setpoint(PIDtypedef*PIDx,u16 setvalue)
{
PIDx->setpoint=setvalue;
}
//確實是設定的預期轉速,而且根據實際參數,執行一次的話就是改變了一個PID單元的設定值,即setpoint
void set_speed(float W1,float W2,float W3,float W4)
{
float temp;
if(W1>0) //判斷W 正負,正數處理
{
motor1_out0=0;
motor1_out1=1;
temp=W1*122.23;
PID_setpoint(&PID1,temp);
}
else if(W1==0) //零值
{
motor1_out0=0;
motor1_out1=0;
PID_setpoint(&PID1,0);
}
else //負數處理
{
motor1_out0=1;
motor1_out1=0;
temp=-W1*122.23;
PID_setpoint(&PID1,temp);
}
if(W2>0)
{
motor2_out0=0;
motor2_out1=1;
temp=W2*122.23;
PID_setpoint(&PID2,temp);
}
else if(W2==0)
{
motor2_out0=0;
motor2_out1=0;
PID_setpoint(&PID2,0);
}
else
{
motor2_out0=1;
motor2_out1=0;
temp=-W2*122.23;
PID_setpoint(&PID2,temp);
}
if(W3>0)
{
motor3_out0=0;
motor3_out1=1;
temp=W3*122.23;
PID_setpoint(&PID3,temp);
}
else if(W3==0)
{
motor3_out0=0;
motor3_out1=0;
PID_setpoint(&PID3,0);
}
else
{
motor3_out0=1;
motor3_out1=0;
temp=-W3*122.23;
PID_setpoint(&PID3,temp);
}
if(W4>0)
{
motor4_out0=0;
motor4_out1=1;
temp=W4*122.23;
PID_setpoint(&PID4,temp);
}
else if(W4==0)
{
motor4_out0=0;
motor4_out1=0;
PID_setpoint(&PID4,0);
}
else
{
motor4_out0=1;
motor4_out1=0;
temp=-W4*122.23;
PID_setpoint(&PID4,temp);
}
}
//這個函數你應該能看出,之前的PID_setpoint()已經包含在里面了。這個函數的功能是設定四個輪子的預期轉速,但是轉速w有正負,而我這邊都是以大小來算的,電機轉向就交給L298的兩個方向控制線了。故需要判斷正負。
(8)低慣量伺服電機空載2ms從0-3000轉/分鍾 ,不知道PID周期是多少?
這個采樣周期我也沒去研究具體去多少比較好,慣量小的話應該取采樣周期短一些 為好。你可以取幾個T 放進去看看效果
(9)樓主,請教2個個問題,你這里頭定義了KP,Ti, Td, Ki, Kd,其中Ki,Kd是由Ti/Td/Kp計算得來的,而最開始KP/Ti/Td是手動整定得到的,那么為什么不直接整定KP/KI/Kd呢?
第二個問題是你定義的控制周期就是#define T 中的T嗎?,他和轉速采集周期,PWM周期,需要滿足什么關系?
謝謝!
第一個問題,整定參數的話,你調整Td Ti 跟你直接調整Kd Ki 沒什么大的差別,后者是前者變換來的。我用的是試湊法,先比例 后積分 再微分
第二個問題,T是采樣周期=采集轉速的周期,和PWM信號沒什么大的聯系。PWM周期是電機決定的,比如我用1K的周期也行,但是噪音和振動方面不太好。
嗯,理解了,我想最理想的PMW周期應該是最后使轉速或者溫度維持到目標值得時候,占空比維持不變。而最理想的PID控制周期應該是接近PWM周期。
/**********PID參數初始化**********/
void PID_Init(void)
{
sptr->LastError = 0;
sptr->PrevError = 0;
sptr->SumError = 0;
sptr->kp = 3.5;
sptr->ki = 0.15;
sptr->kd = 0;
sptr->SetValue = 0.10;
}
setcpmpare()是stm32的庫函數啊,你學習一下用定時器輸出PWM的那個知識點就行了。
簡單的說setcpmpare(a),a就是PWM信號周期中高電平的量,假如pwm周期用的參數是1000,72分頻,那么1000代表的就是1000us了,即pwm周期是1000us,好的。下面我setcpmpare(500),那么就會輸出正好50%占空比的波形了。
我也在注意這個,上位機要繪圖的話數據還是從下位機來的。本來我想嘗試用matlab的串口接受stm32的數據的,發現matlab這塊沒學好。后來就用示波器的滾動模式顯示實時的波形進行觀察的。如果要繪圖寫文章的話,可以把示波器的數據放進u盤,里面會生出excel表格,然后把數據做成數組用plot()函數繪圖就行了。
總結來說,前期調試和查看波形都是示波器實時看的。
庫函數?我在3.5版本的庫函數索引里面里沒有找到setcpmpare()這函數...不過Lz你解釋后我懂了setcpmpare()的意思了。就是設定占空比的吧
if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)//捕獲1發生捕獲事件
{
duty_TIM2 = TIM_GetCapture1(TIM2); //采集占空比
if (TIM_GetCapture2(TIM2)>600) period_TIM2 = TIM_GetCapture2(TIM2);
CollectFlag_TIM2 = 0;
}
樓主,后來我看代碼,有一點疑惑。 period_TIM2 = TIM_GetCapture2(TIM2); 這條語句中,既然是捕獲TIM2 輸出的PWM周期,為什么不是用TIM_GetCaputer1() 這個函數,而是用TIM_GetCapture2().....我在庫函數的文檔中找到,TIM_GetCaputer1() 返回的是 TIMx_CCR1 的值;TIM_GetCapture2() 返回的是 TIMx_CCR2 的值。然后在PWM輸入中,PWM周期的數值不是儲存在TIMx_CCR1,PWM高電平的值儲存在TIMx_CCR2 中嗎?
恩,樓主,搞定了;發現是我弄錯了。
上次看到你的消息,一直不知道怎么回答,因為這些細節我真心沒去研究,代碼部分就像是用了庫函數一樣移植過來的,然后你說的這段代碼對着示波器看看沒問題就用了
你現在解決了嗎?
stm32里面,PWM的輸入 和 輸出 用的是不用的寄存器。后來看 stm32的手冊的定時器里描述的PWM的功能時寄存器的使用情況,然后對比庫函數里代碼的宏定義,一層一層的把庫函數剝開,看到寄存器那,發現是我沒有看精細,太粗心了