基于STM32F103C8T6最小系统板的两轮平衡小车的创作（四、软件部分）

永远少年0722

已于 2022-07-24 16:57:20 修改

阅读量2k

点赞数 1

分类专栏： STM32/ESP32 文章标签： stm32 单片机 arm

于 2022-07-24 16:35:05 首次发布

本文链接：https://blog.csdn.net/m0_54085294/article/details/125960582

版权

STM32/ESP32 专栏收录该内容

9 篇文章 23 订阅

订阅专栏

众所周知，mpu6050的INT是可以当成中断来使用

这样就很容易办了

/****************************** Exti.c ********************************************/

#include "stm32f10x.h"                  // Device header

/**************************************************************************
函数功能：MPU6050外部中断初始化
入口参数：无
返回  值：无 
**************************************************************************/
void MPU6050_EXTI_Init(void)
{  
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//外部中断，需要使能AFIO时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;	            //端口配置
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;         //上拉输入
	GPIO_Init(GPIOB, &GPIO_InitStructure);					      //根据设定参数初始化GPIOB 
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);

	EXTI_InitTypeDef EXTI_InitStructure;
	EXTI_InitStructure.EXTI_Line=EXTI_Line5;
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;	
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿触发
	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
	EXTI_Init(&EXTI_InitStructure);	 	//根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
	
	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;	//使能按键所在的外部中断通道
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;	//抢占优先级2， 
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;			//子优先级1
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;				//使能外部中断通道
	NVIC_Init(&NVIC_InitStructure); 
}

/****************************** Exti.h ********************************************/

#ifndef __EXTI_H
#define __EXTI_H	

void MPU6050_EXTI_Init(void);

#endif

这样中断就配置好了

但是要注意这里

它决定了几ms 一个中断，很重要，我前面的超声波就是因为这个中断才改的只有30cm探测距离，因为会这个中断会影响到超声波，超声波来回速度太慢了，1ms才34cm。

之前我设置的中断是5ms，但是光处理数据就3ms了，所以迫不得已

=========================================================================

下面是pid控制代码，应该很常见

/****************************** Control.c ********************************************/

#include "stm32f10x.h"                  // Device header
#include "sys.h"  
#include "OLED.h"
#include "Control.h"

#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu.h"
#include "MPU6050.h"

#include "Motor.h"
#include "Encoder.h"

#include "SR04.h"

#define SPEED_Y 18 //俯仰(前后)最大设定速度
#define SPEED_Z 70//偏航(左右)最大设定速度 

float Med_Angle=2;	//机械中值。---在这里修改你的机械中值即可。
float Target_Speed=0;	//期望速度（俯仰）。---二次开发接口，用于控制小车前进后退及其速度。
float Turn_Speed=0;		//期望速度（偏航）
extern u8 car;
uint16_t Distance=0;
u8 Distance_Flag=0;

/*********    10ms    *********/
float 
	Vertical_Kp=540,//直立环KP、KD
	Vertical_Kd=1.8;
float 
	Velocity_Kp=0.3,//速度环KP、KI
	Velocity_Ki=0.0015;

/*********    20ms    *********/
//float 
//	Vertical_Kp=550,//直立环KP、KD
//	Vertical_Kd=2.5;
//float 
//	Velocity_Kp=0.8,//速度环KP、KI
//	Velocity_Ki=0.004;

float 
	Turn_Kd=0,//转向环KP、KD
	Turn_Kp=40;

float Pitch,Roll,Yaw;						//角度
short gyrox,gyroy,gyroz;				//陀螺仪--角速度
short aacx,aacy,aacz;						//加速度
int Encoder_Left,Encoder_Right;	//编码器数据（速度）

u8 stop=0;
int PWM_MAX=5000,PWM_MIN=-5000;	//PWM限幅变量
int MOTO1,MOTO2;								//电机装载变量

int Vertical_out,Velocity_out,Turn_out;//直立环&速度环&转向环 的输出变量

void EXTI9_5_IRQHandler(void)
{
	int PWM_out;
	if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
	{
		if(PBin(5)==0)//二级判定
		{
			EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
			
			//1.采集编码器数据&MPU6050角度信息。
			Encoder_Left=Read_Speed(3);//电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反。
//			Encoder_Right=-Read_Speed(4);
			/*右编码器接触不良设置成和左边一样，上面才是对的*/
			Encoder_Right=Encoder_Left;
						
			mpu_dmp_get_data(&Pitch,&Roll,&Yaw);			//角度
			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//陀螺仪
//			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//加速度
	
			
			/****************					蓝牙遥控     		*************************************/
			/*前后*/
			if((car!=1)&&(car!=2)&&(car==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
			if(car==1)Target_Speed++;//前进1标志位拉高-->需要前进
			if(car==2)Target_Speed--;//
			Target_Speed = Target_Speed>SPEED_Y?SPEED_Y : (Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
			
			/*左右*/
			if((car!=3)&&(car!=4)&&(car==0))Turn_Speed=0;
			if(car==3)Turn_Speed += -10;	//左转
			if(car==4)Turn_Speed += 10;	//右转
			Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100   )
		
			/*转向约束*/
			if((car!=3)&&(car!=4)&&(car==0))Turn_Kd=0;//若无左右转向指令，则开启转向约束
			else if((car==3)||(car==4))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
			/*********************************************************************************************/

			
			
			//2.将数据压入闭环控制中，计算出控制输出量。	
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);	//速度环
			Vertical_out=Vertical(Med_Angle,Roll,gyrox);			//直立环
			Turn_out=Turn(gyroz,Turn_Speed);																//转向环
			
			PWM_out=Vertical_out-Vertical_Kp*Velocity_out;//最终输出
			
			//3.把控制输出量加载到电机上，完成最终的的控制。
			MOTO1=PWM_out-Turn_out;//左电机
			MOTO2=PWM_out+Turn_out;//右电机
			Limit(&MOTO1,&MOTO2);	 //PWM限幅		

			/*平衡车摔倒保护*/
			if(Roll > 40 || Roll < -40)
				Load(0,0),stop=1;
			else
				Load(MOTO1,MOTO2);
			
//			Load(MOTO1,MOTO2);		 //加载到电机上。
			
			Distance = Distance_SR04();     //超声波测距
			
		}
	}
}

/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
	int PWM_out;
	
	PWM_out=Vertical_Kp*Angle+Vertical_Kd*(gyro_Y-0);//【1】
	return PWM_out;
}



/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
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);//舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
	//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);
	
	if(stop==1)Encoder_S=0,stop=0;//清零积分量
	
	//5.速度环控制输出计算
	PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
	return PWM_out;
}



/*********************
转向环：系数*Z轴角速度+系数*遥控数据
*********************/
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;
}

/****************************** Control.h ********************************************/

#ifndef __CONTROL_H
#define __CONTROL_H	

/*函数声明*/
int Vertical(float Med,float Angle,float gyro_Y);
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z,int RC);

#endif

至于如何调参，看他的

(98条消息) 【平衡小车制作】（七）串级PID调参及平衡成果展示（超详解）_鲁乎乎的博客-CSDN博客_平衡小车pid怎么调