stm32四輪小車pid代碼求助不論怎么設置初值，車輪總是全速轉動

ID:881103 · 發表于 2021-2-21 09:56

自己寫了一個四輪的小車，主控是stm32f103rct6
目前的問題是，不論怎么設置初值，車輪總是全速轉動，并且拔掉編碼器的線，車輪轉速依舊沒啥變化，不知道是哪里除了問題

單片機源程序如下:

#include "sys.h"
#include "delay.h"
#include "encoder.h"
#include "timer.h"
#include "pwm.h"
#include "pid.h"
#include "motor.h"
#include <stdio.h>
#include "usart.h"
int FleftSpeedNow =0;
int FrightSpeedNow =0;
int BleftSpeedNow =0;
int BrightSpeedNow =0;
int FleftSpeeSet =400;//mm/s
int FrightSpeedSet = 2000;//mm/s
int BleftSpeeSet = 600;//mm/s
int BrightSpeedSet = 600;//mm/s
int main(void)
{
GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 啟用 SWD
MY_NVIC_PriorityGroupConfig(2); //=====設置中斷分組
delay_init(); //=====延時函數初始化
Motor_Init(); //電機初始化
Encoder_Init_TIM2(); //=====初始化編碼器
Encoder_Init_TIM1();
Encoder_Init_TIM3();
Encoder_Init_TIM4(); //=====初始化編碼器
PWM_Init(7199,0); //=====初始化PWM
TIM5_Int_Init(50-1,7200-1); //=====定時器初始化 5ms一次中斷
PID_Init(); //=====PID參數初始化
uart_init(9600);
FrontmotorLeft=400;
//閉環速度控制
while(1)
{
//給速度設定值，想修改速度，就更該leftSpeeSet、rightSpeedSet變量的值
Fpid_Task_Letf.speedSet = FleftSpeeSet;
Fpid_Task_Right.speedSet = FrightSpeedSet;
Bpid_Task_Letf.speedSet = BleftSpeeSet;
Bpid_Task_Right.speedSet = BrightSpeedSet;
//給定速度實時值
Fpid_Task_Letf.speedNow = FleftSpeedNow;
Fpid_Task_Right.speedNow = FrightSpeedNow;
Bpid_Task_Letf.speedNow = BleftSpeedNow;
Bpid_Task_Right.speedNow = BrightSpeedNow;
//執行PID控制函數
Pid_Ctrl(&FrontmotorLeft ,&FrontmotorRight,&BackmotorLeft ,&BackmotorRight);
//根據PID計算的PWM數據進行設置PWM
Set_Pwm_Front(FrontmotorLeft,FrontmotorRight);
Set_Pwm_Back(BackmotorLeft,BackmotorRight);
delay_ms(10);
}
}
//5ms 定時器中斷服務函數 --> 計算速度實時值，運行該程序之前，確保自己已經能獲得輪速，如果不懂，可看之前電機測速的文章
void TIM5_IRQHandler(void) //TIM7中斷
{
if(TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) //檢查指定的TIM中斷發生與否
{
TIM_ClearITPendingBit(TIM5, TIM_IT_Update); //清除TIMx的中斷待處理位
Get_Motor_Speed(&FleftSpeedNow ,&FrightSpeedNow,&BleftSpeedNow,&BrightSpeedNow);//計算電機速度
}
}
#include "sys.h"
#include "delay.h"
#include "encoder.h"
#include "timer.h"
#include "pwm.h"
#include "pid.h"
#include "motor.h"
#include <stdio.h>
#include "usart.h"
int FleftSpeedNow =0;
int FrightSpeedNow =0;
int BleftSpeedNow =0;
int BrightSpeedNow =0;
int FleftSpeeSet =400;//mm/s
int FrightSpeedSet = 2000;//mm/s
int BleftSpeeSet = 600;//mm/s
int BrightSpeedSet = 600;//mm/s
int main(void)
{
GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 啟用 SWD
MY_NVIC_PriorityGroupConfig(2); //=====設置中斷分組
delay_init(); //=====延時函數初始化
Motor_Init(); //電機初始化
Encoder_Init_TIM2(); //=====初始化編碼器
Encoder_Init_TIM1();
Encoder_Init_TIM3();
Encoder_Init_TIM4(); //=====初始化編碼器
PWM_Init(7199,0); //=====初始化PWM
TIM5_Int_Init(50-1,7200-1); //=====定時器初始化 5ms一次中斷
PID_Init(); //=====PID參數初始化
uart_init(9600);
FrontmotorLeft=400;
//閉環速度控制
while(1)
{
//給速度設定值，想修改速度，就更該leftSpeeSet、rightSpeedSet變量的值
Fpid_Task_Letf.speedSet = FleftSpeeSet;
Fpid_Task_Right.speedSet = FrightSpeedSet;
Bpid_Task_Letf.speedSet = BleftSpeeSet;
Bpid_Task_Right.speedSet = BrightSpeedSet;
//給定速度實時值
Fpid_Task_Letf.speedNow = FleftSpeedNow;
Fpid_Task_Right.speedNow = FrightSpeedNow;
Bpid_Task_Letf.speedNow = BleftSpeedNow;
Bpid_Task_Right.speedNow = BrightSpeedNow;
//執行PID控制函數
Pid_Ctrl(&FrontmotorLeft ,&FrontmotorRight,&BackmotorLeft ,&BackmotorRight);
//根據PID計算的PWM數據進行設置PWM
Set_Pwm_Front(FrontmotorLeft,FrontmotorRight);
Set_Pwm_Back(BackmotorLeft,BackmotorRight);
delay_ms(10);
}
}
//5ms 定時器中斷服務函數 --> 計算速度實時值，運行該程序之前，確保自己已經能獲得輪速，如果不懂，可看之前電機測速的文章
void TIM5_IRQHandler(void) //TIM7中斷
{
if(TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) //檢查指定的TIM中斷發生與否
{
TIM_ClearITPendingBit(TIM5, TIM_IT_Update); //清除TIMx的中斷待處理位
Get_Motor_Speed(&FleftSpeedNow ,&FrightSpeedNow,&BleftSpeedNow,&BrightSpeedNow);//計算電機速度
}
}