目录
一。小车介绍
1.1小车硬件介绍
该小车使用大鱼电子的整套硬件设备，其中包括主控stm32f103c8t6,蓝牙模块，mpu6050模块，TB6612FNG电机驱动芯片，两个JGA25-370减速电机，还有一些车子的配件。
硬件占用的 GPIO 的框架:
编码器1--PA0/PA1---TIM2
编码器2--PB6/PB7---TIM4
电机1--PB12/PB13
电机2--PB14/PB15
PWM1--PA8
PWM1--PA11
MPU6050中断引脚--PB5
MPU6050的IIC--PB3/PB4
1.2pid理论分析
位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
各项作用：
比例项:提高响应速度，减小静差。
积分项: 消除稳态误差。
微分项:减小震荡以及超调。
速度环：Kp *Ek+Ki *Ek_S
直立环:PD控制器，Kp *Ek+Kd *Ek_D
转向环：Kd *Z轴角速度+系数 *遥控数据
二。代码编写
2.1 定时器开启编码器功能（使用TIM2和TIM4）
//TIM2编码器模式
void Encoder_TIM2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=65535;
TIM_TimeBaseInitStruct.TIM_Prescaler=0;
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式
TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获
TIM_ICInitStruct.TIM_ICFilter=10;//滤波器
TIM_ICInit(TIM2,&TIM_ICInitStruct);
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
TIM_SetCounter(TIM2,0);//清零定时器计数值
TIM_Cmd(TIM2,ENABLE);//开启定时器
}
//TIM4编码器模式
void Encoder_TIM4_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;
GPIO_Init(GPIOB,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=65535;
TIM_TimeBaseInitStruct.TIM_Prescaler=0;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter=10;
TIM_ICInit(TIM4,&TIM_ICInitStruct);
TIM_ClearFlag(TIM4,TIM_FLAG_Update);
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
TIM_SetCounter(TIM4,0);
TIM_Cmd(TIM4,ENABLE);
}
//读取编码器值
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{
case 2:value_1=(short)TIM_GetCounter(TIM2);
TIM_SetCounter(TIM2,0);
break;
//IF是定时器2，1.采集编码器的计数值并保存。2.将定时器的计数值清零。
case 4:value_1=(short)TIM_GetCounter(TIM4);
TIM_SetCounter(TIM4,0);
break;
default:value_1=0;
}
return value_1;
}
2.2 TIM1开启PWM模式（使用TIM1）
void PWM_Init_TIM1(u16 Psc,u16 Per)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//初始化GPIO--PA8、PA11为复用推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=Per;//ARR值
TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;//分频值
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//初始化输出比较
TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;//选择有效电平时为高电平
TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;
TIM_OCInitStruct.TIM_Pulse=0;//CCR的值
TIM_OC1Init(TIM1,&TIM_OCInitStruct);
TIM_OC4Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器专属--MOE主输出使能
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);//ENABLE//OC1预装载寄存器使能
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);//OC4预装载寄存器使能
TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1在ARR上预装载寄存器使能
TIM_Cmd(TIM1,ENABLE);//开定时器。
}
2.3 电机初始化
//电机引脚初始化
void Motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_12 |GPIO_Pin_13 |GPIO_Pin_14 |GPIO_Pin_15;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE);//开启时钟
}
//pwm限幅函数
void Limit(int *motoA,int *motoB)
{
if(*motoA>PWM_MAX)*motoA=PWM_MAX;
else if(*motoA<PWM_MIN)*motoA=PWM_MIN;
if(*motoB>PWM_MAX)*motoB=PWM_MAX;
else if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}
//把带符号的数据转为绝对值的函数
int GFP_abs(int p)
{
int q;
q=p>0?p:(-p);
return q;
}
//将pwm加载到电机的函数
void Load(int moto1,int moto2)//moto1=-200：反转200个脉冲
{
//1.研究正负号，对应正反转
if(moto1>0) Ain1=1,Ain2=0;//正转
else Ain1=0,Ain2=1;//反转
//2.研究PWM值
TIM_SetCompare1(TIM1,GFP_abs(moto1));
if(moto2>0) Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM1,GFP_abs(moto2));
}
//电机停止函数，当电机实际角度偏离超过50度时，停止电机，防止小车摔倒
char PWM_Zero=0,stop=0;
void Stop(float *Med_Angel,float *Angle)
{
if(GFP_abs(*Angle-*Med_Angel)>50)Load(PWM_Zero,PWM_Zero);
}
2.4 mpu6050初始化
mpu6050驱动是调用InvenSense官方的DMP运动驱动库，然后在借鉴正点原子的驱动代码把它移植到stm32上，然后直接使用就可以了。
平衡小车使用了MPU6050的中断引脚，由于MPU6050的采样率设置为100Hz,所以中断引脚的中断频率为10ms一次。
当进入中断后，对小车进行控制操作。
下面是中断引脚初始化：
void MPU6050_EXTI_Init(void)
{
EXTI_InitTypeDef EXTI_InitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD ,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU;//中断来时，引脚电平被拉低
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_2;//PD2配置为上拉输入
GPIO_Init(GPIOD,&GPIO_InitStruct);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOD,GPIO_PinSource2);
EXTI_InitStruct.EXTI_Line=EXTI_Line2;
EXTI_InitStruct.EXTI_LineCmd=ENABLE;
EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling; //下降沿
EXTI_Init(&EXTI_InitStruct);
NVIC_InitTypeDef NVIC_InitStructure; NVIC_PriorityGroupConfig(NVIC_PriorityGroup_3);
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}2.5PID控制代码
//转向环
int Turn(int gyro_Z,int RC)
{
int PWM_out;
//这不是一个严格的PD控制器，Kd针对的是转向的约束，但Kp针对的是遥控的转向。
PWM_out=Turn_Kd*gyro_Z + Turn_Kp*RC;
return PWM_out;
}
//直立环pd
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
return PWM_out;
}
//速度环pi
int Velocity(int Target,int encoder_left,int encoder_right)
{
static int Encoder_S,EnC_Err_Lowout_last,PWM_out,Encoder_Err,EnC_Err_Lowout;
float a=0.7;
//1.计算速度偏差
Encoder_Err=((encoder_left+encoder_right)-Target);//舍去误差
//2.对速度偏差进行低通滤波
//low_out=(1-a)*Ek+a*low_out_last;
//使得波形更加平滑，滤除高频干扰，防止速度突变。
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;
EnC_Err_Lowout_last=EnC_Err_Lowout;
//3.对速度偏差积分，积分出位移
Encoder_S+=EnC_Err_Lowout;
//4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度环控制输出计算
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
return PWM_out;
}
2.6 初始化蓝牙模块使用到的串口USATRT4
void USART4_Init(u32 bound)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC|RCC_APB2Periph_AFIO, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4,ENABLE);
//UART4_TX PC10
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//UART4_RX PC11
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//USART 初始化设置
USART_InitStructure.USART_BaudRate = bound;//一般蓝牙模块是默认设置为9600;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(UART4, &USART_InitStructure);
USART_ITConfig(UART4, USART_IT_RXNE, ENABLE);//开启中断
USART_Cmd(UART4, ENABLE); //使能串口
}
/**************************************************************
0x00：刹车(中间)
0x01：前进
0x02: 左上角(其实右上角)
0x03：左转(对应是右转,这里电机方向不同 所以是左转)
0x04: 左下角(其实右下角,电机方向不同导致)
0x05：后退
0x06:右下角(其实是左下角)
0x07：右转(其实是左转)
0x08: 右上角(其实是左上角)
**************************************************************/
//串口4中断接收数据
void UART4_IRQHandler(void)
{
char Bluetooth_data;
if(USART_GetITStatus(UART4,USART_IT_RXNE)!=RESET)//接收中断标志位拉高
{
Bluetooth_data=USART_ReceiveData(UART4);//保存接收的数据
if(Bluetooth_data==0x00) Fore=0,Back=0,Left=0,Right=0;//刹
else if(Bluetooth_data==0x01) Fore=1,Back=0,Left=0,Right=0;//前
else if(Bluetooth_data==0x05) Fore=0,Back=1,Left=0,Right=0;//后
else if(Bluetooth_data==0x03) Fore=0,Back=0,Left=1,Right=0;//左
else if(Bluetooth_data==0x07) Fore=0,Back=0,Left=0,Right=1;//右
else if(Bluetooth_data==0x02) Fore=1,Back=0,Left=1,Right=0;//左上角
else if(Bluetooth_data==0x04) Fore=0,Back=1,Left=1,Right=0;//左下角
else if(Bluetooth_data==0x06) Fore=0,Back=1,Left=0,Right=1;//右下角
else if(Bluetooth_data==0x08) Fore=1,Back=0,Left=0,Right=1;//右上角
}
}2.7 EXTI2_IRQHandler中断处理
//EXTI2_IRQHandler中断服务函数，对应MPU6050的中断输入引脚，10ms进入一次
void EXTI2_IRQHandler(void)
{
int PWM_out;
if(EXTI_GetITStatus(EXTI_Line2)!=0)
{
if(PDin(2)==0)
{
EXTI_ClearITPendingBit(EXTI_Line2);//清除中断标志位
//1、采集编码器数据&MPU6050角度信息。
//电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反。
Encoder_Left=-Read_Speed(2);
Encoder_Right=Read_Speed(4);
//这三个函数是调用DMP运动驱动库的，移植后可以直接使用
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
//前后移动
if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令->速度清零，稳在原地
else if(Fore==1)Target_Speed--;//Target_Speed是目标速度，由于方向是反的，所以减才是前进
else if(Back==1)Target_Speed++;//
Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
//左右移动
if((Left==0)&&(Right==0))Turn_Speed=0;
else if(Left==1)Turn_Speed+=30; //左转
else if(Right==1)Turn_Speed-=30; //右转
Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅
//转向约束
if((Left==0)&&(Right==0))Turn_Kd=-0.8;//若无左右转向指令，则开启转向约束
else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
//2、将数据压入闭环控制中，计算出控制输出量。
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy);//直立环
Turn_out=Turn(gyroz,Turn_Speed);//转向环
PWM_out=Vertical_out;//最终输出
//3、把控制输出量加载到电机上，完成最终的的控制。
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Limit(&MOTO1,&MOTO2); //PWM限幅
Load(MOTO1,MOTO2); //加载到电机上。
Stop(&Med_Angle,&Pitch);//安全检测
}
}
}2.8 main函数代码
int mian()
{
int encoder4,encoder2;
USART4_Init(); //串口4(蓝牙使用)
Encoder_TIM4_Init();//编码器初始化
Encoder_TIM2_Init();//
MPU6050_EXTI_Init();//MPU6050中断引脚
MPU_Init(); // MPU6050初始化
mpu_dmp_init(); //MPU6050的DMP初始化
PWM_Init_TIM1(0,7199);//pwm初始化
Motor_Init();//电机初始化
OLED_Init();//OLED初始化
while(1)
{
encoder2=Read_Speed(2);
encoder4=Read_Speed(4);
sprintf((char*)oledtext,"enc2:%d",encoder2 );
OLED_ShowString(1,1,(char*)oledtext);
sprintf((char*)oledtext,"enc4:%d",encoder4 );
OLED_ShowString(2,1,(char*)oledtext);
OLED_ShowString(3,1,(char*)Bluetooth_data);
}
}总结
PID调参需要根据硬件来选择参数，也需要一定的方法和耐心。
656
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
