参考资料：（里面有源码）
STM32系列(HAL库)——F103C8T6通过MPU6050+DMP姿态解算读取角度及温度_嵌入式创客工坊的博客-CSDN博客_6050dmp
【STM32】HAL库 PWM控制电机转速与编码器读取（超详解）_鲁乎乎的博客-CSDN博客_hal库控制步进电机
两位博主的讲解非常详细，在此谢过！
目录
一、CubeMX配置
说明：
咱们就不讲原理什么的了，太多了。我们直接上图。芯片选择咱们就不说了，附一张总图，小车功能实现到直立环、速度环
1、配置RCC、SYS、时钟树
2、配置串口1
3、MPU6050
两个引脚都配置为开漏输出
4、配置编码器模式
5、配置PWM模式
6、配置GPIO
这里的PB12\PB13\PB14\PB15控制电机正反转，我备注了AIN1\2、BIN1\2,代码也会有修改，嫌麻烦的可以不用备注。
7、配置FreeRTOS
这里我FreeRTOS配置了3个进程，分别是Task_MPU6050、Task_Control、Task_Oledshow，入口函数分别是MPU6050_200HZ、Control_100HZ、Oledshow_5HZ，这个自己设置就好，无关紧要。
但是FreeRTOS的时钟我没有区别开，剩下的定时器可能留着有用。不然FreeRTOS的延时用定时器3来做。
8、导出工程
二、部分代码
1、串口1打印 //便于调试
/* USER CODE BEGIN 0 */
//加入头文件 #include "stdio.h"
int fputc(int ch, FILE *fp)
{
while(!(USART1->SR & (1<<7)));
USART1->DR = ch;
return ch;
}
/* USER CODE END 0 */2、freertos.c文件内代码
void MPU6050_200HZ(void const * argument)
{
/* USER CODE BEGIN MPU6050_200HZ */
//MPU6050数据采集
MPU_Init(); //=====初始化MPU6050
while(mpu_dmp_init()); //=====初始化MPU6050的DMP模式,用while等待
printf("MPU6050初始化成功！\n");
/* Infinite loop */
for(;;)
{
MPU6050_Start();
osDelay(5);
}
/* USER CODE END MPU6050_200HZ */
}
/* USER CODE BEGIN Header_Control_100HZ */
/**
* @brief Function implementing the Task_Control thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Control_100HZ */
void Control_100HZ(void const * argument)
{
/* USER CODE BEGIN Control_100HZ */
/* Infinite loop */
for(;;)
{
Control_Start();
osDelay(10);
}
/* USER CODE END Control_100HZ */
}入口MPU6050_Start()代码：
#include "mpu6050.h"
#include "stdio.h"
#include "inv_mpu.h"
float pitch,roll,yaw; //欧拉角(姿态角)
short aac_x,aac_y,aac_z; //加速度传感器原始数据
short gyro_x,gyro_y,gyro_z; //陀螺仪原始数据
void MPU6050_Start(void) //MPU6050数据采集
{
mpu_dmp_get_data(&pitch,&roll,&yaw);
MPU_Get_Accelerometer(&aac_x,&aac_y, &aac_z); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyro_x, &gyro_y, &gyro_z); //得到陀螺仪数据
/**********************测试*************************/
// printf("acc_x = %d\n", aac_x);
// printf("aac_y = %d\n", aac_y);
// printf("aac_z = %d\n", aac_z);
// printf("gyro_x = %d\n", gyro_x);
// printf("gyro_y = %d\n", gyro_y);
// printf("gyro_z = %d\n", gyro_z);
// printf("X:%.2f° Y:%.2f° Z:%.2f°\r\n",pitch,roll,yaw);//串口1输出采集信息
/***************************************************/入口Control_Start()代码：
#include "control.h"
void Control_Start(void)
{
Encoder_Left = Read_Encoder(2); //===读取编码器的值
Encoder_Right = -Read_Encoder(4); //===读取编码器的值
/**********************测试*************************/
// printf("Encoder_Left = %d Encoder_Right = %d\n",Encoder_Left,Encoder_Right);
/***************************************************/
// 1、确定直立环PWM
Balance_Pwm = Balance_UP(pitch,gyro_y);
// 2、确定速度环PWM
Velocity_Pwm = velocity(Encoder_Left,Encoder_Right);
// 3、确定转向环PWM
// 4、确定最终左右电机PWM
Moto1 = Balance_Pwm + Velocity_Pwm;
Moto2 = Balance_Pwm + Velocity_Pwm;
Xianfu_Pwm();
Turn_Off(pitch);
// 5、设置电机
Set_Pwm(Moto1,Moto2);
}3、control.c文件内代码
#include "control.h"
#include "balance_car.h"
int Dead_Zone = 200 ; //死区电压
float Movement = 0 ; //速度调节
float balance_UP_KP = 340 ; // 小车直立环KP
float balance_UP_KD = 0.6 ;
float velocity_KP = -60 ;
float velocity_KI = -0.3 ;
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int Read_Encoder(uint8_t TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
/**************************************************************************
函数功能：直立PD控制
入口参数：角度、机械平衡角度（机械中值）、角速度
返回 值：直立控制PWM
作 者：大鱼电子
**************************************************************************/
int Balance_UP(float Angle,float Gyro)
{
float Bias;
int Balance;
Bias=Angle-Mechanical_balance; //===求出平衡的角度中值和机械相关
Balance=balance_UP_KP*Bias+balance_UP_KD*Gyro; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return Balance;
}
/**************************************************************************
函数功能：速度PI控制
入口参数：电机编码器的值
返回 值：速度控制PWM
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral;
//==================速度PI控制器=============================================================//
Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.8f; //===一阶低通滤波器
Encoder += Encoder_Least*0.2f; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
if(Encoder_Integral>10000) Encoder_Integral=10000; //===积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===积分限幅
Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI; //===速度控制
if(pitch<-40||pitch>40)
Encoder_Integral=0; //===电机关闭后清除积分
return Velocity;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int moto1,int moto2)
{
if(moto1<0) AIN2(0), AIN1(1);
else AIN2(1), AIN1(0);
PWMA=Dead_Zone + myabs(moto1);
if(moto2<0) BIN1(1), BIN2(0);
else BIN1(0), BIN2(1);
PWMB=Dead_Zone + myabs(moto2);
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回 值：unsigned int
目 的：经过直立环和速度环以及转向环计算出来的PWM有可能为负值
而只能赋给定时器PWM寄存器只能是正值。故需要对PWM进行绝对值处理
**************************************************************************/
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回 值：无
**************************************************************************/
void Xianfu_Pwm(void)
{
//===PWM满幅是7200 限制在7000
if(Moto1<-7000 ) Moto1=-7000 ;
if(Moto1> 7000 ) Moto1= 7000 ;
if(Moto2<-7000 ) Moto2=-7000 ;
if(Moto2> 7000 ) Moto2= 7000 ;
}
/**************************************************************************
函数功能：异常关闭电机
入口参数：倾角和电压
返回 值：无
**************************************************************************/
void Turn_Off(float angle)
{
if(angle<-40||angle>40) //===倾角大于40度关闭电机
{
AIN2(0), AIN1(0);
BIN1(0), BIN2(0);
}
}control.h文件内代码
#ifndef __CONTROL_H
#define __CONTROL_H
#include "main.h"
#define Mechanical_balance 0 //机械零点,无超声波、电池平躺着的小车机械中值
#define AIN1(PinState) HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,(GPIO_PinState)PinState)
#define AIN2(PinState) HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,(GPIO_PinState)PinState)
#define BIN1(PinState) HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,(GPIO_PinState)PinState)
#define BIN2(PinState) HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,(GPIO_PinState)PinState)
#define PWMA TIM1->CCR4
#define PWMB TIM1->CCR1
//extern float Angle_Balance,Gyro_Balance; //平衡倾角 平衡陀螺仪 转向陀螺仪
int Read_Encoder(uint8_t TIMX); //读取编码器计数
int Balance_UP(float Angle,float Gyro); //直立环
int velocity(int encoder_left,int encoder_right); //速度环
void Set_Pwm(int moto1,int moto2);
int myabs(int a);
void Xianfu_Pwm(void);
void Turn_Off(float angle);
#endif
三、代码下载
链接：https://pan.baidu.com/s/1zgqSLyWcIFV6QdJP-k0Mug
提取码：DT99
916
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
