最简单DIY基于STM32F407探索者开发板的MPU6050陀螺仪姿态控制舵机程序
STM32库函数开发系列文章目录
第一篇：STM32F103ZET6单片机双串口互发程序设计与实现
第二篇：最简单DIY基于STM32单片机的蓝牙智能小车设计方案
第三篇：最简单DIY基于STM32F407探索者开发板的MPU6050陀螺仪姿态控制舵机程序
文章目录
前言
daodanjishui物联网核心原创技术之最简单DIY基于STM32F407探索者开发板的MPU6050陀螺仪姿态控制舵机程序。
市面上有各种开源STM32舵机控制器，但是有复杂的有简单的，如果想快速入门stm32f407zgt6姿态传感器的舵机控制，这个方案会给你一个快捷高效的方案。
一、最简单DIY基于STM32F407探索者开发板的MPU6050陀螺仪姿态控制舵机程序是什么？
我记得在“51单片机智能小设计”专栏中讲述了51单片机如何通过寄存器激发PWM波控制sg90舵机的博文。但是由于51单片机资源有限，再加上复杂的姿态解算，很难同时完成姿态解算控制舵机的任务，所以诞生了这个博文。
这次的方案主要是：基于正点原子STM32F407探索者开发板和MPU6050陀螺仪来采集航向角从而使用IO口直接控制一个舵机翻转。下面请看全家福：
这次的程序使用的stm32库函数来编程，说实话也修改了正点原子开发板配套的例程，正点原子移植了arduino版本的IIC控制MPU6050的库才有了我这个下文，为了实现这个STM32舵机的控制，我查阅了网上很多有关舵机控制的资料和视频，我几乎横扫了全部正点原子关于舵机控制的论坛帖子，有含金量的帖子基本上没有多少，给出源码的程序我觉得多多少少有点问题，也踩了很多坑！有些人会说：“daodanjishui你何必那么执着呢？大把arduino的库给你使用来控制舵机，控制MPU6050，你却硬要从底层出发去控制舵机！”我说：“一个合格的软硬件设计工程师至少要攻破底层，才能真正流畅地控制硬件。做到这一点，随随便便去控制一个机械手又有何难呢？再写一个上位机去控制舵机又有何难呢？再多写一个手机控制舵机的APP又有何难？假如一直在用匿名四轴的上位机调试MPU6050，你能写一个自己的上位机出来吗？人家目前不开源，你还能写出这样的上位机么？呵呵”。到目前为止，我确实写出了一个舵机控制的上位机：最简单DIY基于C#和51单片机上下位机一体化的PCA9685舵机控制程序，总之写代码是痛苦的，在这个吃快餐的时代最好的办法就是复制粘贴“拿来主义”，但是并不是什么都可以拿的，有时候自己去尝试的时候才发现没有东西可以拿，只能硬着头皮撸起袖子加油干。
优酷视频演示地址：https://v.youku.com/v_show/id_XNTAxMDg2NzE0OA==.html
直接观看
最简单DIY基于STM32F407探索者开发板的MPU6050陀螺仪姿态控制舵机程序
二、使用步骤
1.准备硬件
1.1正点原子探索者开发板一个，如果没有的话用STM32F407ZGT6核心板也可以的，现在美国ST公司禁售该芯片，估计以后这个芯片就被淘汰了。
1.2舵机sg90一个。
1.3MPU6050模块一个。
1.4正点原子4.3寸电容显示屏一个。（如果没有的话可以屏蔽该部分代码）
2.准备正点原子开源的MPU6050的代码
我采用库函数开发，所以代码直接使用该开发板配套的免费开源源码进行二次开发。源码来源：探索者光盘资料\A盘\4，程序源码\2，标准例程-库函数版本\实验32 MPU6050六轴传感器实验
代码如下（示例）：
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "key.h"
#include "lcd.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
//ALIENTEK 探索者STM32F407开发板 实验32
//MPU6050六轴传感器 实验 -库函数版本
//技术支持：www.openedv.com
//淘宝店铺：http://eboard.taobao.com
//广州市星翼电子科技有限公司
//作者：正点原子 @ALIENTEK
//串口1发送1个字符
//c:要发送的字符
void usart1_send_char(u8 c)
{
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,c);
}
//传送数据给匿名四轴上位机软件(V2.6版本)
//fun:功能字. 0XA0~0XAF
//data:数据缓存区,最多28字节!!
//len:data区有效数据个数
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32];
u8 i;
if(len>28)return; //最多28字节数据
send_buf[len+3]=0; //校验数置零
send_buf[0]=0X88; //帧头
send_buf[1]=fun; //功能字
send_buf[2]=len; //数据长度
for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和
for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串口1
}
//发送加速度传感器数据和陀螺仪数据
//aacx,aacy,aacz:x,y,z三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
{
u8 tbuf[12];
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1
}
//通过串口1上报结算后的姿态数据给电脑
//aacx,aacy,aacz:x,y,z三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
//roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度
//pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
//yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
{
u8 tbuf[28];
u8 i;
for(i=0;i<28;i++)tbuf[i]=0;//清0
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
tbuf[18]=(roll>>8)&0XFF;
tbuf[19]=roll&0XFF;
tbuf[20]=(pitch>>8)&0XFF;
tbuf[21]=pitch&0XFF;
tbuf[22]=(yaw>>8)&0XFF;
tbuf[23]=yaw&0XFF;
usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
}
int main(void)
{
u8 t=0,report=1; //默认开启上报
u8 key;
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
short temp; //温度
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168); //初始化延时函数
uart_init(500000); //初始化串口波特率为500000
LED_Init(); //初始化LED
KEY_Init(); //初始化按键
LCD_Init(); //LCD初始化
MPU_Init(); //初始化MPU6050
POINT_COLOR=RED;//设置字体为红色
LCD_ShowString(30,50,200,16,16,"Explorer STM32F4");
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");
LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
LCD_ShowString(30,110,200,16,16,"2014/5/9");
while(mpu_dmp_init())
{
LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
delay_ms(200);
LCD_Fill(30,130,239,130+16,WHITE);
delay_ms(200);
}
LCD_ShowString(30,130,200,16,16,"MPU6050 OK");
LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");
POINT_COLOR=BLUE;//设置字体为蓝色
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
LCD_ShowString(30,200,200,16,16," Temp: . C");
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
while(1)
{
key=KEY_Scan(0);
if(key==KEY0_PRES)
{
report=!report;
if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
}
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,220,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数部分
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,240,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数部分
temp=yaw*10;
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,260,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数部分
t=0;
LED0=!LED0;//LED闪烁
}
}
t++;
}
}
3.准备正点原子PWM输出的代码
因为控制舵机需要用到PWM波，那么就需要用到PWM有关的开源代码，代码路径是：探索者光盘资料\A盘\4，程序源码\2，标准例程-库函数版本\实验9 PWM输出实验
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "pwm.h"
//ALIENTEK 探索者STM32F407开发板 实验9
//PWM输出实验-库函数版本
//技术支持：www.openedv.com
//淘宝店铺：http://eboard.taobao.com
//广州市星翼电子科技有限公司
//作者：正点原子 @ALIENTEK
int main(void)
{
u16 led0pwmval=0;
u8 dir=1;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168); //初始化延时函数
uart_init(115200);//初始化串口波特率为115200
TIM14_PWM_Init(500-1,84-1); //84M/84=1Mhz的计数频率,重装载值500，所以PWM频率为 1M/500=2Khz.
while(1) //实现比较值从0-300递增，到300后从300-0递减，循环
{
delay_ms(10);
if(dir)led0pwmval++;//dir==1 led0pwmval递增
else led0pwmval--; //dir==0 led0pwmval递减
if(led0pwmval>300)dir=0;//led0pwmval到达300后，方向为递减
if(led0pwmval==0)dir=1; //led0pwmval递减到0后，方向改为递增
TIM_SetCompare1(TIM14,led0pwmval); //修改比较值，修改占空比
}
}
4.修改源码和组合源码（这部分原创）
这里有两部分原创的代码：
（1）第一部分
//加入PWM控制舵机部分
//tempp=pitch;//俯仰角
aaaa=yaw;//航向角
//bbbb=aaaa/2;
//led0pwmva2=880+tempp;//原作者的程序部分，我不解释，因为不好用，看得懂算你厉害。
//led0pwmval=880+bbbb;//原作者的程序部分，我不解释，因为不好用
//下面的逻辑是根据航向角转动的幅度控制一个舵机也转动相应的角度
//也是舵机控制的核心代码，打折的代码我不会去解释的，加价我可以考虑，如果有付费需求请联系我
if((yaw>=0)&&(yaw<=180)){//在舵机的控制量程范围，180度的舵机
led0pwmval=(u16)10*(1755-yaw)/18.0;//将航向角转化为定时器对比值，如果直接加上yaw不会得出准确的结果，免费代码我不会解释这里关键的公式的，收费的会有解释
TIM_SetCompare1(TIM14,led0pwmval);
delay_ms(10);//这个延时如果调整到500ms会导致采集陀螺仪数据出问题的
}
（2）第二部分
//下面的测试代码，让舵机从0度转到180，再回到0度，不断循环
led0pwmval=975;//0度
TIM_SetCompare1(TIM14,led0pwmval);
delay_ms(1000);
led0pwmval=950;//45度
TIM_SetCompare1(TIM14,led0pwmval);
delay_ms(1000);
led0pwmval=925;//90度
TIM_SetCompare1(TIM14,led0pwmval);
delay_ms(1000);
led0pwmval=900;//135度
TIM_SetCompare1(TIM14,led0pwmval);
delay_ms(1000);
led0pwmval=875;//180度
TIM_SetCompare1(TIM14,led0pwmval);
delay_ms(1000);
说明：第一个部分是舵机控制的核心算法，我自己推导出来化简的。第二部分是舵机测试程序，测试舵机固定动作的。读者根据这些代码推敲出结果是没有问题的，我当时也花了不少时间去推敲。如果真的不想推敲了，请下载我最后附录上的工程代码。
三、运行与调试
（1）功能说明：用MPU6050的姿态数据控制舵机翻转0到180度，并且支持用匿名四轴上位机调试仿真波形和舵机状态，买家可以根据代码改为多个舵机控制。
（2）代码说明：用MDK5写的库函数代码
（3）硬件说明：需要用到正点原子探索者开发板，配套他们自家的4.3寸电容屏，配套他们自己家的MPU6050模块，一个其他家的SG90舵机。如果经济条件不错的买家就可以拿正点原子全家桶套装来测试，连线基本上不用考虑，直接插上就能用，除了舵机那个数据线要自己接而已。要是想省钱，那就自己琢磨程序里面有注释的IO管脚定义去自己接线了,同时也要考虑没有接显示屏会遇到什么错误自己慢慢排查，反正接舵机的IO口是PA7，我是全家桶套装的使用者，不需要考虑那么多
（4） 软件说明：用了正点原子探索者库函数版本 实验32 MPU6050六轴传感器实验 的代码进行修改而成，正点原子的手册也介绍的程序基本使用方法和代码注释，另外还可以使用匿名四轴的上位机来调试舵机。需要接上串口，波特率调到500000（最大）。如下图所示：
仿真说明：姿态波形展示如下图
舵机翻转角度图，航向角变化就是舵机角度的变化如下图是0度
舵机翻转角度图，航向角变化就是舵机角度的变化如下图是26度
舵机翻转9度情况如下图显示
翻转90度的舵机如下
通过上面运行与调试证明了程序状态良好，达到博文提出的要求。
总结
有些读者说：为什么不用PCA9685控制舵机呢？其实我已经推出，只不过这是STM32控制舵机的入门篇，不能一下子加大难度。
扩展说明：如果搞定了一个舵机，那么就可以搞定多个舵机，用库函数写代码效率是很高的，不像我上次用51单片机控制舵机那么麻烦的，全部是配置寄存器和定时器。所以我称之为最简单的舵机控制程序，我写的核心的代码就几行，能看懂的读者就赚大了。剩下的就是正点原子的MPU6050示例代码，不懂的读者可以亲自看看正点原子的教程文档。
下期预告：下期用stm32控制PCA9685模块间接去集群控制多个舵机，并且用红外线遥控器控制舵机组成的机械臂，精彩不容错过，值得期待。
最后附上本博文代码下载地址：https://gf.bilibili.com/item/detail/1107743114
直接跳转
旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。
更多推荐
18
0- 0
已为社区贡献2条内容
所有评论(0)