arduino陀螺仪蓝牙通讯手势小车

新的改变

目录

手势小车 1
一、学习目标 2
知识目标 2
技能目标 2
二、背景知识 2
三、知识储备 3
ARDUINO NANO 2
ARDUINO UNO 4
面包板 6
智能小车模块 4
蓝牙模块HC-O6 5
蓝牙模块HC-O5 5
陀螺仪模块 5
直流电机 5
Arduino编程语句 6
Arduino逻辑运算符 7
四、案例实现 14
案例描述 14
器械清单15
电路接线 16
电路设计 16
电路仿真 16
单元调试 16
各个程序编写 21
五、任务扩展 21
六、手势小车程序 21

一.学习目标
知识目标

  1. 了解智能小车模块引脚接口图
  2. 了解HC-05,HC-06的工作原理
  3. 了解陀螺仪模块MPU6050工作原理

技能目标
1.学会使用if函数
2.串口通讯监视
3.学会自定义函数
4.学会设置蓝牙
5.学会控制直流电机
6.学会调用库
!](https://www.icode9.com/i/ll/?i=20210619155308197.png?,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl81MjUzMTY5OQ==,size_16,color_FFFFFF,t_70)

二.背景知识
小车的开发已经比较成熟,市场上遍布遥控小车等等,现阶段很多的东西都可以实现手机蓝牙控制,我现在的目的就是实现硬件单片机和单片机之间蓝牙的连接,不断的创新实现家中的东西万物互联。

三.知识储备
Arduino Nano
arduino陀螺仪蓝牙通讯手势小车

Arduino Nano 是一款类似Arduino UNO的开发板。区别是Nano的尺寸更加小巧。Arduino Nano 是一款基于 ATmega328P的开发板。它可以直插面包板的 。Arduino Nano 与Arduino Uno十分类似。它与Uno的区别是Nano没有直流电压供电接口同时Nano通过Mini-B USB 接口与电脑连接。
主要技术参数
微控制器 ATmega328P
工作电压 5伏特
Flash Memory(闪存) 32 KB (ATmega328P) 其中由 0.5 KB用于系统引导(bootloader)
SRAM(静态存储器) 2 KB (ATmega328P)
EEPROM 1 KB (ATmega328P)
模拟输入引脚 8个
EEPROM 1Kb
输入/输出引脚直流电流 40 毫安
输入电压 7-12伏特
数字输入输出引脚 22个(其中有6个引脚可作为PWM引脚)
PWM引脚 6个
3.3V引脚电流 50 毫安
长 45 mm
宽 18 mm
重 7克
时钟频率 16 MHz

Nano的引脚图
arduino陀螺仪蓝牙通讯手势小车

Arduino Uno
arduino陀螺仪蓝牙通讯手势小车

在Arduino开发板家族中,Uno开发板是最适合初学者的Arduino开发板。由于它简单易学、稳定可靠,Uno开发板也是应用最为广泛且参考资料最多的开发板。
Arduino Uno 是一款基于 微控制器 ATmega328P的开发板。它有14个数字输入/输出引脚(这些引脚中有6个引脚可以作为PWM输出引脚),6个模拟输入引脚,16 MHz石英晶振,USB接口,电源接口,支持在线串行编程以及复位按键。用户只需要将开发板与电脑通过USB接口连接就可以使用。
arduino陀螺仪蓝牙通讯手势小车

UNO主要技术参数
微控制器 ATmega328P
工作电压 5伏特
输入电压(推荐) 7-12伏特
输入电压(极限) 6-20伏特
数字输入输出引脚 14个(其中有6个引脚可作为PWM引脚)
PWM引脚 6个
模拟输入引脚 6个
输入/输出引脚直流电流 20 毫安
3.3V引脚电流 50 毫安
Flash Memory(闪存) 32 KB (ATmega328P) 其中由 0.5 KB用于系统引导(bootloader)
SRAM(静态存储器) 2 KB (ATmega328P)
EEPROM 1 KB (ATmega328P)
内置LED引脚 13

面包板
这款面包板竖侧相同(红线处标明),蓝线处显示横侧导通
arduino陀螺仪蓝牙通讯手势小车

如图这款面包板体积较小,没有横侧通的功能但是比较方便,只可竖侧通
arduino陀螺仪蓝牙通讯手势小车

智能小车模块
arduino陀螺仪蓝牙通讯手势小车

这块板子能实现许多小车方面的功能,非常方便试验图中标明了各个引脚的功能,但是以下是我使用后的独到经验。IN3,IN4对应的是左轮; IN1,IN2,对应的是右轮,EN2左轮改变电压调节速度口,EN1右轮改变电压调节速度口,同样循迹模块OUT3,OUT4对应左轮; OUT1,OUT2对应右轮,那4个绿色指示灯就是用来显示循迹模块的反应的。
智能小车上的EN1,EN2引脚作用和L298N模块是一样的
(个人总结:在使用智能小车模块的时候开关处会发烫是正常现象)

arduino陀螺仪蓝牙通讯手势小车

还有就是电源可以连接11.4V的锂电池连接智能小车模块(接口再板子下方),再通过5V电源接口给Arduino供电,注意不同的电机接线方法,电机正反转的方向会不同

HC-06蓝牙模块
arduino陀螺仪蓝牙通讯手势小车

蓝牙通讯
定义:蓝牙通讯是指依据蓝牙协议进行通讯。在Arduino项目中我们常常使用蓝牙模块进行蓝牙通讯,蓝牙模块是一种集成了蓝牙功能的PCB板,可以实现短距离的无线数据传输。

HC-06是一个从机端蓝牙,不可实现像手机这类主机端蓝牙的功能,可以发出信息和接收信息前提是一个主机一个从机端,本次手势小车就是用到其的从机功能用来发送信息给小车上的蓝牙,实现手势控制。切记HC06的初始波特率是9600

(实验发现,HC-06的信息发送延时非常高,所以建议用JDY_31,HC-08等更高级的蓝牙模块)

实例程序

arduino陀螺仪蓝牙通讯手势小车

arduino陀螺仪蓝牙通讯手势小车

如图接法,HC06的RX,TX都必须是3.3V左右,由于Uno高电频为5V电压需要通过欧姆电阻降压,注意如果您是接在UNO板上的RX,TX引脚上上传程序时需先断开蓝牙与UNO的连接因为,UNO上传程序也是通过TX,RX引脚,这两个引脚不能受干扰,不然程序上传不上去的

arduino陀螺仪蓝牙通讯手势小车arduino陀螺仪蓝牙通讯手势小车

通过搜索此软件下载到手机后,就可通过手机连接HC06了,此时的HC06是从机状态,输入0,1就可控制ARDUNIO上的板载灯了。

登陆界面自己去学习学习,探索探索
复制这段内容后打开百度网盘手机App,
链接:https://pan.baidu.com/s/1jnMJvNdNh_yrXr3nuBh_2w
提取码:8686
---- AT指令 ----

指令 答复 说明
AT OK 通讯测试
AT+VERSION OKlinvorV1.8 固件版本
AT+NAMEmyBTmodule OKsetname 设置设备名 “myBTmodule”
AT+PIN6789 OKsetPIN 设置设备 PIN 6789
AT+BAUD1 OK1200 设置波特率 1200
AT+BAUD2 OK2400 设置波特率 2400
AT+BAUD3 OK4800 设置波特率 4800
AT+BAUD4 OK9600 设置波特率 9600
AT+BAUD5 OK19200 设置波特率 19200
AT+BAUD6 OK38400 设置波特率 38400
AT+BAUD7 OK57600 设置波特率 57600
AT+BAUD8 OK115200 设置波特率 115200
AT+BAUD9 OK230400 设置波特率 230400
AT+BAUDA OK460800 设置波特率 460800
AT+BAUDB OK921600 设置波特率 921600
AT+BAUDC OK1382400 设置波特率 1382400
这是HC06的AT指令,打开串口监视器发送AT接收到OK后即可设置了,尽量不要修改密码后文会提及原因的

arduino陀螺仪蓝牙通讯手势小车

如果打开不了AT模式,尝试断开电源,蓝牙重新连接电源,记住一定要再NL和CR下发送信息
HC-05蓝牙模块
HC-05引脚说明

STATE:蓝牙连接状态指示,连接成功此引脚为高电平,没有连接则为低电平
RX:接收端
TX:发送端
GND:接地端
+5V:5V电源输入端
EN:使能端,需要进入AT模式时接3.3V。其实不接也没有关系
接线

HC05—>Arduino
RX —>11
TX —>10
GND —>GND
+5V —>5V
接线实物图

三. 进入AT模式
在HC-05蓝牙模块的正面会有一个圆形按键,摁住的过程中给蓝牙模块上电,蓝牙模块的指示灯慢闪(2秒亮一次),即说明已经进入AT模式。(切记)

四. 蓝牙模块基本配置

模块默认波特率为38400、默认配对密码为1234、默认名称为HC-05, AT模式波特率固定38400,8位数据位、1位停止位、无奇偶校验的通信格式。
AT+NAMEHC05 //修改蓝牙模块名称为HC05
AT+ROLE=1 //蓝牙模式为主模式
AT+CMODE=1 //蓝牙连接模式为任意地址连接模式,也就是说该模块可以被任意蓝牙设备连接
AT+PSWD=1234 //蓝牙配对密码为1234
AT+BAUD4 //蓝牙通信串口波特率为9600

arduino陀螺仪蓝牙通讯手势小车

如图为HC05 AT模式设置程序,设置过程中为了方便后文实验,我们需将其初始波特率38400改成9600,将其设置为主机,可以试试手机APP连接蓝牙如果连接不上,恭喜你成功了,因为主机和主机是无法连接的。切勿修改密码因为HC06,HC05初始PIN都是1234,只要波特率,密码一样这对主从即可连接。

MPU6050陀螺仪
MPU6050是一种非常流行的空间运动传感器芯片,可以获取器件当前的三个加速度分量和三个旋转角速度。
arduino陀螺仪蓝牙通讯手势小车

上面两个图我是从6050芯片的中文资料上截下来的其中对应的6、7号引脚就是XDA和XCL,23、24对应的就是SCL和SDA。所以XDA和XCL就是用来与其他传感器进行I2C通信用的,SDA与SCL就是与控制器进行I2C通信。所以我们这次所要使用的就只有SDA和SCL这两个引脚与Arduino进行通信。而至于INT脚就是用来中断用的。这次实验是用不到的,所以可以选择不接。I2C的 Slave地址的最低有效位( LSB)用AD0设置,如果AD0不接的话从"WHO I AM"寄存器(地址0X75)中读取应该是0X68,如果接低的话应该是0XD0,接高的话就是0XD1。当然这个并不重要,不过可以检测你的I2C通信是否正确。
在UNO板子上,SDA接口对应的是A4引脚,SCL对应的是A5引脚。MPU6050需要5V的电源,可由UNO板直接供电。

Arduino的I2C通信

  因为我们大家都是初学者可能并不会什么I2C通信,但是Arduino就是为了方便我们设计的,它已经有了I2C通信的库文件了,那就是Wire库。具体库的介绍,大家可以查询官方文件什么的,我就不详细介绍了。

arduino陀螺仪蓝牙通讯手势小车

实例程序:
如图试验程序时,我们只需要使用SCL,SDA,VCC,GND这几个引脚,其他的变量正是使用到I2C,这不是我们本次实验要学的内容,但是可以课外拓展拓展。打开串口观察X轴加速度变化。

arduino陀螺仪蓝牙通讯手势小车

左右摇摆观察变化
直流电机

直流电机想必大家都不陌生,所以我就不在过多的讲解,直接上代码

arduino陀螺仪蓝牙通讯手势小车arduino陀螺仪蓝牙通讯手势小车

这是两个程序关于直流电机的,如果你不使用analogWrite();那么它会默认为255“高”电压,把电池的最大电压直接给他而-255意思是电流反向且最大,实现倒退。 切记这些都是在L298N模块条件下实现的功能(智能小车模块自带)。 再次强调,接线不同,电机转的方向不同。
arduino编程语句

  1. if :通过if指令,用户可以让Arduino判断某一个条件是否达到,并且根据这一判断结果执行相应的程序。
    arduino陀螺仪蓝牙通讯手势小车

if(表达式) { 语句块 } [/c] 上述结构表示:如果 “表达式” 的条件得到满足则执行语句块。否则 Arduino将不执行该语句块。(如图所示)
2. while: while循环将会连续地无限地循环,直到圆括号()中的表达式变为假。被测试的表达式变量必须被改变,否则while循环将永远不会中止。可以在代码改变测试变量,比如让该变量递增,或者通过外部条件改变测试变量,比如将一个传感器的读数赋值给测试变量。
arduino陀螺仪蓝牙通讯手势小车

while(表达式/循环条件){ 语句块/循环体 }先计算表达式的值,当值为真(非0)时, 执行循环体语句;执行完循环体语句,再次计算表达式的值,如果为真,继续执行循环体……这个过程会一直重复,直到表达式的值为假(0)才退出循环。其执行过程如图所示:

  1. set up函数: 当Arduino程序开始运行时会调用setup()函数。通常我们setup()函数其中初始化一些变量、引脚状态及一些调用的库等。当Arduino控制器通电或复位后,setup函数会运行一次。

  2. Loop函数: 在setup()函数对程序完成了初始化后,loop()函数将会运行。loop函数是一个循环体,在Arduino启动后,loop()函数中的程序将会不断运行。通过loop()函数你可以利用你的程序来控制Arduino。

  3. Arduino 库 :通过库的使用可以拓展Arduino开发板的功能。因为有了库,我们可以很轻松的实现Arduino与外部硬件的协作或进行数据通讯。

逻辑运算符
<= ; >= 比较运算符用于和 if 联合使用,测试某一条件是否达到。例如测试Arduino的某一个引脚输入值是否达到设定数值。如果是则执行特定程序。换句话说,如果圆括号中的语句为真,大括号中的语句就会运行。否则,程序跳过该代码。【逻辑】-【比较运算符】
模块介绍
布尔运算符可用于if语句中的条件判断:
&& (逻辑与)
arduino陀螺仪蓝牙通讯手势小车

只有在两个操作数都为真时才返回真。 不要混淆布尔与运算符,&&(两个&符号)和按位与运算符&(单个&符号)。它们是完全不同的概念。
|| (逻辑或)
arduino陀螺仪蓝牙通讯手势小车
任意一个为真时返回真
示例程序 (可双击程序内容全选代码):
不要混淆布尔或运算符||(双竖杠)和按位或运算符|(单竖杠)。
! (逻辑非)
arduino陀螺仪蓝牙通讯手势小车

当操作数为假时返回真

四、案例实现
arduino陀螺仪蓝牙通讯手势小车

再把所作的四个程序组装,手势小车的程序就出来了。

用物器械
名称 数量
Ardunio Uno 1个
Ardunio Nano 1个
HC06蓝牙模块 1个
HC05蓝牙模块 1个
直流电机 2个
陀螺仪模块 1个
智能小车模块 1个
锂电池 2个
面包板 2个
杜邦线 1捆

编写主机端程序

arduino陀螺仪蓝牙通讯手势小车arduino陀螺仪蓝牙通讯手势小车arduino陀螺仪蓝牙通讯手势小车

这是主机端HC05的程序,负责接收HC06发出的数据,首先调用softwareserial库实现蓝牙功能,然后就是定义车轮的端口,ena,enb是电机的pwm端口(改变经过直流电机的电压),10行char把val定义成字符串变量,在set up 函数中设置蓝牙波特率同时2224行先让它暂停,然后loop函数中28行如果蓝牙有接收到数据且接收到的数据为“g”,则执行向前走的程序经过直流电机的电压为电池的95/255,并在串口监视器中输出“go”,同理4078行一个意思。(根据程序来连接引脚)

  1. 从机端程序

arduino陀螺仪蓝牙通讯手势小车
arduino陀螺仪蓝牙通讯手势小车arduino陀螺仪蓝牙通讯手势小车

首先调用蓝牙的库,35行都是关于陀螺仪的库,上文介绍过wire库的内容,822行都是关于陀螺仪的算法,set up和loop中有大量的计算公式,这不是我们要学的但是图上有部分介绍如果想学习可以上网搜索。

arduino陀螺仪蓝牙通讯手势小车

arduino陀螺仪蓝牙通讯手势小车

至147行以上数据皆是关于陀螺仪算法的,所以我们的中心在后方,Accy角速度Y轴,Accx角速度X轴,182行如果Accx大于40且小于90度则串口监视器输出“g”,且从机端HC06发送“g”给HC05,同理后面的代码皆是如此。

电路设计:
本实验主要是三部分,

  1. 通过Arduino和库实现蓝牙和蓝牙沟通连接.
  2. 直流电机的速度改变
  3. 陀螺仪算法和程序
    4.蓝牙的硬件设置
    arduino陀螺仪蓝牙通讯手势小车

五.任务扩展:

  1. 制作一个蓝牙和蓝牙沟通连接的程序
  2. 制作一个陀螺仪的程序
  3. 制作一个直流电机小车的程序

四.手势小车实验程序

程序编写:

  1. 任务要求

它的程序就是以上程序的拼装,当你明白以上程序时相信你也会编写了。


//bluetheeth host
//模块A,智能硬件模块,与家居常用物品绑定使用,成为智慧物品
#include<SoftwareSerial.h>
int input1 = 7; // 定义uno的pin 5 向 input1 输出 
int input2 = 2; // 定义uno的pin 6 向 input2 输出
int input3 = 3; // 定义uno的pin 9 向 input3 输出
int input4 = 4; // 定义uno的pin 10 向 input4 输出
int enA = 5;    //这里对应两个ENA引脚
int enB = 6; 
SoftwareSerial BT(10, 11); // uno的10接蓝牙的TX,11接蓝牙的RX
String  *val;//定义参数接收蓝牙的值
long Accx;
//int temps;
void setup() {
  Serial.begin(9600);
  BT.begin(9600);//蓝牙波率设置为9600
pinMode(7,OUTPUT);
pinMode(2,OUTPUT);
pinMode(3,OUTPUT);
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
//val = 0;         //String变量是可以让其只读取字符串不读取字符的
//&val ='j';
 digitalWrite(input1,LOW);
 digitalWrite(input2,LOW);  
 digitalWrite(input3,LOW);
 digitalWrite(input4,LOW);  
 Serial.println( "stop" );
}
void loop() { 
  if(BT.available()>0){
    val=BT.read();
     //Accx=BT.read();
   
 if(val=='g') //如果控制信息数值为FFA857 加号
    {     // go on 向前走
  Accx=BT.read();
  digitalWrite(input1,LOW); //给高电平
  digitalWrite(input2,HIGH);  //给低电平
  digitalWrite(input3,LOW); //给高电平
  digitalWrite(input4,HIGH);  //给低电平  
  analogWrite(enA,100);
  analogWrite(enB,103);      //右轮速度较慢,需要程序调速  
  Serial.println( "go");
  Serial.println(Accx ); 
    } 
   else if(val=='b') //如果控制信息数值为FFE01F 减号
    {          
        // go back 向后走
  Accx=BT.read();
  digitalWrite(input1,HIGH); //给高电平
  digitalWrite(input2,LOW);  //给低电平
  digitalWrite(input3,HIGH); //给高电平
  digitalWrite(input4,LOW);  //给低电平
  analogWrite(enA,80);
  analogWrite(enB,80);
   Serial.println( "back" ); 
  // Serial.println(60 ); 
    }
   else  if(val=='l') 
    {
  Accx=BT.read();    
  digitalWrite(input1,LOW);//左转
  digitalWrite(input2,LOW);  
  digitalWrite(input3,LOW); 
  digitalWrite(input4,HIGH);  
  //digitalWrite(input3,LOW); 
  //digitalWrite(input4,HIGH); 
  analogWrite(enA,110);
  analogWrite(enB,110);
  Serial.println("left" ); 
 // Serial.println(Accx ); 
  }
 else  if(val=='r'){
  Accx=BT.read();
  digitalWrite(input1,LOW); //右转
  digitalWrite(input2,HIGH);
  digitalWrite(input3,LOW); 
  digitalWrite(input4,LOW);   
  analogWrite(enA,110);
  analogWrite(enB,110); 
  Serial.println( "right" ); 
  //Serial.println( Accx ); 
}
  else  if(val=='s'){
 digitalWrite(input1,LOW);
 digitalWrite(input2,LOW);  
 digitalWrite(input3,LOW);
 digitalWrite(input4,LOW);  
 Serial.println( "stop" );}

//else  if(10<Accx<120){          //功能函数
//  Serial.println( "hello" );  //这里是验正是否有Accx数值输出
//  if( temps <= Accx){
//    Serial.println("Accx high");
//    temps = Accx ;
//}
//  }
  
  }
}
//bluetheeth follow  这是陀螺仪方面的代码
#include <SoftwareSerial.h>
SoftwareSerial BTserial(10, 11); 
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt;                                   //微分时间 A4连接SDA   A5连接SCL
int16_t ax, ay, az, gx, gy, gz;             //加速度计陀螺仪原始数据  Typedef符号字符
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0;//角度变量
long axo = 0, ayo = 0, azo = 0;             //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0;             //陀螺仪偏移量
float pi = 3.1415926;
float AcceRatio = 16384.0;                  //加速度计比例系数
float GyroRatio = 131.0;                    //陀螺仪比例系数
uint8_t n_sample = 8;                       //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0};  //x,y轴采样队列
long aax_sum, aay_sum,aaz_sum;                      //x,y轴采样和
float a_x[10]={0}, a_y[10]={0},a_z[10]={0},g_x[10]={0},g_y[10]={0},g_z[10]={0}; //加速度计协方差计算队列
float Px=1, Rx, Kx, Sx, Vx, Qx;             //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy;             //y轴卡尔曼变量
float Pz=1, Rz, Kz, Sz, Vz, Qz;             //z轴卡尔曼变量
volatile int Accy;
volatile int Accx;
void setup() {
  Serial.begin(9600);
  BTserial.begin(9600); 
  Accy = 0;
  Accx = 0;
  Serial.print("HC-06 DEMO/TEST  ");
  
  //BTserial.print("AT");   //可在此处输入设置HC-06蓝牙模块的AT指令。
                          //此AT指令须在HC-06未连接蓝牙状态输入。
  pinMode(LED_BUILTIN, OUTPUT);

  //BTserial.println( "blueteeth is open");
  Wire.begin();

    accelgyro.initialize();                 //初始化
    unsigned short times = 200;             //采样次数
    for(int i=0;i<times;i++)
    {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
        axo += ax; ayo += ay; azo += az;      //采样和
        gxo += gx; gyo += gy; gzo += gz;
    }
    axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
    gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}

 
void loop(){



 unsigned long now = millis();             //当前时间(ms)
    dt = (now - lastTime) / 1000.0;           //微分时间(s)
    lastTime = now;                           //上一次采样时间(ms)
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
    float accx = ax / AcceRatio;              //x轴加速度
    float accy = ay / AcceRatio;              //y轴加速度
    float accz = az / AcceRatio;              //z轴加速度

    aax = atan(accy / accz) * (-180) / pi;    //y轴对于z轴的夹角
    aay = atan(accx / accz) * 180 / pi;       //x轴对于z轴的夹角
    aaz = atan(accz / accy) * 180 / pi;       //z轴对于y轴的夹角

    aax_sum = 0;                              // 对于加速度计原始数据的滑动加权滤波算法
    aay_sum = 0;
    aaz_sum = 0;

    for(int i=1;i<n_sample;i++)
    {
        aaxs[i-1] = aaxs[i];                  //替换算法
        aax_sum += aaxs[i] * i;               
        aays[i-1] = aays[i];
        aay_sum += aays[i] * i;
        aazs[i-1] = aazs[i];
        aaz_sum += aazs[i] * i;
    }  

    aaxs[n_sample-1] = aax;
    aax_sum += aax * n_sample;
    aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90°
    aays[n_sample-1] = aay;                        //此处应用实验法取得合适的系数
    aay_sum += aay * n_sample;                     //本例系数为9/7
    aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
    aazs[n_sample-1] = aaz;
    aaz_sum += aaz * n_sample;
    aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;

    float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
    float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
    float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度

    agx += gyrox;                             //x轴角速度积分
    agy += gyroy;                             //x轴角速度积分
    agz += gyroz;

    Sx = 0; Rx = 0;/* kalman start */
    Sy = 0; Ry = 0;
    Sz = 0; Rz = 0;

    for(int i=1;i<10;i++)
    {                 //测量值平均值运算
        a_x[i-1] = a_x[i];                     //即加速度平均值
        Sx += a_x[i];
        a_y[i-1] = a_y[i];
        Sy += a_y[i];
        a_z[i-1] = a_z[i];
        Sz += a_z[i];
    }
    a_x[9] = aax;
    Sx += aax;
    Sx /= 10;                                 //x轴加速度平均值
    a_y[9] = aay;
    Sy += aay;
    Sy /= 10;                                 //y轴加速度平均值
    a_z[9] = aaz;
    Sz += aaz;
    Sz /= 10;                                 //Z轴加速度平均值

    for(int i=0;i<10;i++)
    {
        Rx += sq(a_x[i] - Sx);
        Ry += sq(a_y[i] - Sy);
        Rz += sq(a_z[i] - Sz);
    }
    Rx = Rx / 9;                              //得到方差
    Ry = Ry / 9;                        
    Rz = Rz / 9;
   
    Px = Px + 0.0025;                         // 0.0025在下面有说明...
    Kx = Px / (Px + Rx);                      //计算卡尔曼增益
    agx = agx + Kx * (aax - agx);             //陀螺仪角度与加速度计速度叠加
    Px = (1 - Kx) * Px;                       //更新p值

    Py = Py + 0.0025;
    Ky = Py / (Py + Ry);
    agy = agy + Ky * (aay - agy);
    Py = (1 - Ky) * Py;

    Pz = Pz + 0.0025;
    Kz = Pz / (Pz + Rz);
    agz = agz + Kz * (aaz - agz);
    Pz = (1 - Kz) * Pz;        /* kalman end */
    
    // 通过串口监视器显示HC-06发送的数据
//    if( BTserial.available()>0 ){            //如果软件串口有HC-06发来的数据
//      char BTserialData =  BTserial.read();  //将软件串口中的数据赋值给变量BTserialData
//      Serial.print( BTserialData );          //通过硬件串口监视器显示HC-06发来的数据
//     
//    }
       
int Accy=accy*100;int Accx=accx*100;
//Serial.println(Accx);
//  Serial.println();
 // Serial.println(Accy);
if (Accx >= 35 && Accx <= 95) {
    Serial.println('g');
    BTserial.println('g');
    delay(100);
    BTserial.println(Accx*1.5);
    Serial.println(Accx*1.5); 
    //BTserial.write(9);
  }
 else if (Accx <= -55 && Accx >= -105) {
    Serial.println('b');
    Serial.println((-Accx*1.5));
    BTserial.println('b'); 
    delay(100);
    BTserial.println((-Accx*1.5));
  }
 else if (Accy <= 85 && Accy >= 35) {
    Serial.println('l');
    Serial.println(Accy*1.5);
    BTserial.println( 'l');
    delay(100);     
    BTserial.println(Accy*1.5);
//BTserial.write(5 );
  }
 else if (Accy <= -40 && Accy >= -95) {
    Serial.println('r');
    Serial.println((-Accy*1.5));
    BTserial.println('r');                   //println是发送字符串的
    delay(200); 
    BTserial.println((-Accy*1.5));            //write是发送数字的(0~255)但是是二进制
  }
 else if (Accy <= 40 && Accy >= -45 && Accx <= 40 && Accx >= -60) {
    Serial.println('s');
    BTserial.println('s'); // BTserial.println努力寻找比其更小的传输方式
    delay(100);
  }
  else {
    BTserial.println('s');
    delay(100);
    }
    
}

上一篇:arduino扩展板引脚和连接图


下一篇:迄今为止最好的arduino开发平台:vscode+platformio,秒杀arduino ide。及vscode+pio对比arduino ide对比Stduino