采用增量式PID算法配合编码器控制电机转速,但在给定转速之后,相关引脚不输出PWM波形,目前所有代码可以正常编译,目前考虑应该是配置问题,不知道是哪里代码有问题。不清楚是PID算法的问题还是电机配置的问题还是编码器的问题。
#include "stm32f10x.h" // Device header
#include "Moto.h"
#include "PID.h"
#include <math.h>
#include "Encoder.h"
_Moto_Str Left_moto = MOTO_STR_DEFAULT;
_Moto_Str Right_moto = MOTO_STR_DEFAULT;
void Moto_PWM_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE); //
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC,&GPIO_InitStructure);
//初始化TIM8
TIM_TimeBaseStructure.TIM_Period = 1000 - 1 ; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1 ; //设置用来作为TIMx时钟频率除数的预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV2; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
//初始化TIM8 Channel PWM模式
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_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
//TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OC1Init(TIM8, &TIM_OCInitStructure);//根据T指定的参数初始化外设TIM8 OC1
TIM_OC2Init(TIM8, &TIM_OCInitStructure);
TIM_OC3Init(TIM8, &TIM_OCInitStructure);
TIM_OC4Init(TIM8, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM8,ENABLE);
TIM_Cmd(TIM8, ENABLE);
TIM_CtrlPWMOutputs(TIM8,ENABLE);
}
void Moto_PWM_Output(void)//(signed short Moto_Left,sign ed short Moto_Right)
{
if(Left_moto.ESC_Output_PWM >= 0){
TIM_SetCompare1(TIM8,0);
TIM_SetCompare2(TIM8,Left_moto.ESC_Output_PWM);
}
else{
TIM_SetCompare1(TIM8,-Left_moto.ESC_Output_PWM);
TIM_SetCompare2(TIM8,0);
}
if(Right_moto.ESC_Output_PWM >= 0){
TIM_SetCompare3(TIM8,Right_moto.ESC_Output_PWM);
TIM_SetCompare4(TIM8,0);
}
else{
TIM_SetCompare3(TIM8,0);
TIM_SetCompare4(TIM8,-Right_moto.ESC_Output_PWM);
}
}
这个是PID模块的
#include "stm32f10x.h" // Device header
#include "PID.h"
#include "Moto.h"
#include <climits>
#define KP 60.0f // 次调节
#define KI 200.0f // 先调节
#define KD 50.0f // 最后调节
float Robot_ASR_PID_Control(_Moto_Str* MotoStr)//( )//增量型离散PID
{
float Error = 0;
float P_Error = 0;
float I_Error = 0;
float D_Error = 0;
float add = 0;
Error = MotoStr->Target_Speed - MotoStr->Current_Speed;
// Update the current one-two third-order error for the current proportional integral and differential calculation
I_Error = Error;
P_Error = Error - MotoStr->L_Error;
D_Error = Error - 2*MotoStr->L_Error + MotoStr->LL_Error;
// calculation current proportional integral and differential
add = KP * P_Error + KI * I_Error + KD * D_Error;
MotoStr->ESC_Output_PWM += add;
MotoStr->LL_Error = MotoStr->L_Error;
MotoStr->L_Error = Error;
// Speed PID output limit, limit the maximum duty cycle //ESC_Output_PWM (-950 - 950)
MotoStr->ESC_Output_PWM = LIMIT_MIN_MAX_FUN(MotoStr->ESC_Output_PWM, -ESC_output_PWM_LIMT, ESC_output_PWM_LIMT);
// 给定的目标速度也为零时, 延迟2S后取消电机闭环,延迟的目的是防止干扰电机的减速过程
(MotoStr->Target_Speed == 0 && MotoStr->Current_Speed == 0)?(ADDITION_OPERATION(MotoStr->Cancel_Control_Count, UINT_MAX)):(MotoStr->Cancel_Control_Count = 0);
MotoStr->ESC_Output_PWM = (MotoStr->Cancel_Control_Count >= MOTO_CANCEL_CONTROL_TIME_LINMT)?(0):(MotoStr->ESC_Output_PWM);
return MotoStr->ESC_Output_PWM;
}
这个是编码器模块
#include "stm32f10x.h" // Device header
#include "Encoder.h"
#include "Moto.h"
void RightMoto_Encoder_Input_init(void)
{
GPIO_InitTypeDef GPIO_Initstructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);//
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//哪个时钟哪个端口啊??
GPIO_Initstructure.GPIO_Mode=GPIO_Mode_IN_FLOATING;//浮空输入
GPIO_Initstructure.GPIO_Pin=GPIO_Pin_6 | GPIO_Pin_7;//端口配置
GPIO_Initstructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_Initstructure);
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
//TI1FP1和TI2FP2边沿上的计数器计数取决于另一个输入的电平。
TIM_Cmd(TIM3, ENABLE);
}
void LeftMoto_Encoder_Input_init(void)
{
GPIO_InitTypeDef GPIO_Initstructure;
RCC_APB2PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//使能PA端口时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//使能GPIOB时钟
GPIO_Initstructure.GPIO_Mode=GPIO_Mode_IN_FLOATING;//浮空输入
GPIO_Initstructure.GPIO_Pin=GPIO_Pin_6 | GPIO_Pin_7;//端口配置
GPIO_Initstructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_Initstructure);
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
//TI1FP1和TI2FP2边沿上的计数器计数取决于另一个输入的电平。
TIM_Cmd(TIM4, ENABLE);
}
void Robot_Encoder_Start(void)
{
TIM3->CNT = 0x7fff;
TIM4->CNT = 0x7fff;
}
void Robot_Encoder_Get_CNT(void)
{
Left_moto.Encoder_Value = (TIM3->CNT)-0x7fff; //读取左右轮子的脉冲累计数
Right_moto.Encoder_Value = -((TIM4->CNT)-0x7fff);
Left_moto.Current_Speed = -CURRENT_MOTO_SPEED(Left_moto.Encoder_Value);
Right_moto.Current_Speed = CURRENT_MOTO_SPEED(Right_moto.Encoder_Value);
TIM3->CNT = 0x7fff; //清除左右轮子的脉冲数
TIM4->CNT = 0x7fff;
}
第二级PID主要由PID三项控制构成,原本决定不用I控制,但是在实际调试过程中,由于电机存在一定的死区,在平衡位置附近PWM输出很小,电机不转,严重影响控制品质;可以按固定的PWM值进行死区补偿,但是这样较为死板,考虑到倒立摆的摆动是一个相对慢的过程,所以使用积分项。同样需要积分限幅。
引用chatGPT作答,根据提供的代码,问题可能出在如下几个方面:
电机的接线和驱动是否正确,可以检查一下电机接线是否正确,电机驱动电路的元器件是否损坏,是否使用正确的电源电压。
编码器的接线和工作是否正确,可以检查一下编码器的接线是否正确,信号是否正确读取。
PWM输出模块的初始化和配置是否正确,可以检查一下PWM输出模块的初始化和配置是否正确,占空比是否被正确设置。
增量式PID算法是否正确实现,可以检查一下增量式PID算法是否正确实现,参数是否被正确设置。
以下是可能的解决方案:
电机驱动和接线问题:
1)检查一下电机接线是否正确,是否与引脚的定义一致。
2)检查电机驱动电路的元器件是否损坏,是否使用正确的电源电压。
编码器问题:
1)检查编码器的接线是否正确,信号是否正确读取。
2)使用示波器等工具检查编码器输出的波形是否正确。
PWM输出模块问题:
1)检查一下PWM输出模块的初始化和配置是否正确,占空比是否被正确设置。
2)使用示波器等工具检查PWM输出波形是否正确。
增量式PID算法问题:
1)检查一下增量式PID算法是否正确实现,参数是否被正确设置。
2)使用示波器等工具检查PID输出波形是否正确。
建议逐一排查以上问题,以确定问题所在。
把复用换成重映射试试,F1可能会出现这样的情况。