FRC自购IIC陀螺仪GY-91使用

最新推荐文章于 2024-11-06 02:59:07 发布

ZhangzrJerry

最新推荐文章于 2024-11-06 02:59:07 发布

阅读量594

点赞数 1

分类专栏： FRC 文章标签：单片机硬件工程

本文链接：https://blog.csdn.net/frc8011_zzr/article/details/122153887

版权

FRC 专栏收录该内容

7 篇文章 0 订阅

订阅专栏

硬件准备

这是我们在某宝上购买的一款SPI&IIC陀螺仪

https://item.taobao.com/item.htm?spm=a230r.1.14.1.113c4b20IbC4N3&id=523806329384&ns=1&abbucket=12#detail

它即将被我们连接到主板NI roboRIO上

https://www.andymark.com/products/ni-roborio-2?via=Z2lkOi8vYW5keW1hcmsvV29ya2FyZWE6Ok5hdmlnYXRpb246OlNlYXJjaFJlc3VsdHMvJTdCJTIyYnV0dG9uJTIyJTNBJTIyc2VhcmNoJTIyJTJDJTIycSUyMiUzQSUyMnJvYm9yaW8lMjIlMkMlMjJ1dGY4JTIyJTNBJTIyJUUyJTlDJTkzJTIyJTdE

SPI尝试

我们首先尝试用SPI的通信协议

主板上的SPI接口共有10个

我们打开商家给的GY-91陀螺仪文档手册，结合NI roboRIO的说明进行接线

Serial Buses — FIRST Robotics Competition documentation (wpilib.org)docs.wpilib.org

GY-91 - NI roboRIO

3V3 - 3.3V

GND - 地线

SCL - SCLK

SDA - MOSI

SD0/SA0 - MISO

NCS - CS0/CS1/CS2/CS3

我们这里是测试时将CSB接到CS0上，正确方式应该连接NCS和CS0

程序准备

我们在官方给的API集里面找到了需要用到的类

https://docs.wpilib.org/en/latest/docs/hardware/sensors/serial-buses.html?highlight=spi#mxp-expansion-port

我们先建出陀螺仪的对象_gyro

/**
* 将SPI头文件导入 
*/
#include <frc/SPI.h>
/**
* 为陀螺仪建立SPI对象
*/
frc::SPI *_gyro = new frc::SPI(frc::SPI::Port::kOnboardCS0);

接下来我们希望找到能够读取到陀螺仪值的函数，我们在adafruit的网站上找到了同款芯片的arduino代码

https://learn.adafruit.com/adafruit-tdk-invensense-icm-20948-9-dof-imu/arduino

但arduino的库和frc的库存在很大的差别，我们并没有从中获得很大的启示，于是我把目光重新投回WPILIB上，我使用这段代码来进行测试

uint32_t* data = new uint32_t[9]; 
frc::SmartDashboard::PutNumber("ReadAutoReciveData",_gyro->ReadAutoReceivedData(data,16,1000.0)); 
frc::SmartDashboard::PutNumber("0",data[0]); 
frc::SmartDashboard::PutNumber("1",data[1]); 
frc::SmartDashboard::PutNumber("2",data[2]); 
frc::SmartDashboard::PutNumber("3",data[3]); 
frc::SmartDashboard::PutNumber("4",data[4]); 
frc::SmartDashboard::PutNumber("5",data[5]); 
frc::SmartDashboard::PutNumber("6",data[6]); 
frc::SmartDashboard::PutNumber("7",data[7]); 
frc::SmartDashboard::PutNumber("8",data[8]);

因为文档上介绍这款陀螺仪是九轴的，且输出数据为16位，于是我将上面代码修改为

 uint32_t* data = new uint32_t[10];  
 frc::SmartDashboard::PutNumber("ReadAutoReciveData",_gyro->ReadAutoReceivedData(data,16,1000.0));  
 frc::SmartDashboard::PutNumber("0",data[0]);  
 frc::SmartDashboard::PutNumber("1",data[1]);  
 frc::SmartDashboard::PutNumber("2",data[2]);  
 frc::SmartDashboard::PutNumber("3",data[3]);  
 frc::SmartDashboard::PutNumber("4",data[4]);  
 frc::SmartDashboard::PutNumber("5",data[5]);  
 frc::SmartDashboard::PutNumber("6",data[6]); 
 frc::SmartDashboard::PutNumber("7",data[7]);  
 frc::SmartDashboard::PutNumber("8",data[8]); 
 frc::SmartDashboard::PutNumber("9",data[9]);

我试图从十个打印值中找到规律，但很明显我没有成功，我重新看回技术手册希望得到些许启示

https://learn.adafruit.com/adafruit-tdk-invensense-icm-20948-9-dof-imu

我在adafruit的网站上找到了这款芯片的详细信息，还有ICM20X的API文档

https://adafruit.github.io/Adafruit_ICM20X/html/index.html

经过了很多次的尝试，但并没有找到有效的解决方法

IIC连接

于是我们更换了一种连接方式，尝试用IIC的通信协议来将他们连接

GY-91 - NI roboRIO

3V3 - 3.3V

GND - 地线

SCL - SCL

SDA - SDA

#include <frc/I2C.h>
#define ADDR 0x68
frc::I2C gyro{frc::I2C::kOnboard,ADDR};
frc::I2C temp{frc::I2C::kOnboard,0x0c};

void Robot::RobotInit() {
  uint8_t id[1];
  int device_id=gyro.Read(0x75,1,id);
  frc::SmartDashboard::PutNumber("id",device_id);
  gyro.Write(0x6a,0x00);
  frc::Wait(0.05);
  gyro.Write(0x37,0x02);
  temp.Write(0x0a,0x16);
  gyro.Write(29,9);
  gyro.Write(106,96);
}

void Robot::TeleopPeriodic() {
  temp.Write(0x0a,0x16);
  uint8_t gout[6];
  bool bl=gyro.Read(0x43,6,gout);
  frc::SmartDashboard::PutBoolean("bl",bl);
  frc::SmartDashboard::PutNumber("X",(gout[0]<<8)|gout[1]);
  frc::Wait(0.5);
}

我们简单的读取陀螺仪的数据，但遇到了数据波动的问题

程序控制

为了降低数据波动，我们寻找一种合适的滤波算法

在官方电控文档中，我看到了一些过滤器算法的库

https://docs.wpilib.org/en/latest/docs/software/advanced-controls/filters/index.html

我们为数据添加了线性过滤器和中位值过滤器各两种

frc::MedianFilter<double> median_filter2(20);
frc::LinearFilter<double> linear_filter2 = 	frc::LinearFilter<double>::SinglePoleIIR(0.5,0.02_s);
frc::MedianFilter<double> median_filter(5);
frc::LinearFilter<double> linear_filter = frc::LinearFilter<double>::SinglePoleIIR(0.1,0.02_s);
void Robot::TeleopPeriodic(){
  temp.Write(0x0a,0x16);
  uint8_t gout[6];
  bool bl=gyro.Read(0x43,6,gout);
  frc::SmartDashboard::PutBoolean("bl",bl);
  double giao=(gout[0]<<8|gout[1]);
  frc::SmartDashboard::PutNumber("X",giao);
  //frc2::Wait(0.05_s);
  frc::SmartDashboard::PutNumber("median_filter",median_filter.Calculate(giao));
  frc::SmartDashboard::PutNumber("linear_filter",linear_filter.Calculate(giao));
  frc::SmartDashboard::PutNumber("median_filter2",median_filter2.Calculate(giao));
  frc::SmartDashboard::PutNumber("linear_filter2",linear_filter2.Calculate(giao));
}

在glass工具中将5条数据显示出来

我们观察到linear_filter2对数据有更好的过滤作用

完整代码

#include "Robot.h"
#include <iostream>
#include <frc/smartdashboard/SmartDashboard.h>
#include<frc/MedianFilter.h>
#include<frc/LinearFilter.h>
#include<frc/I2C.h>
#define ADDR 0X68
frc::I2C gyro{frc::I2C::kOnboard,ADDR};//陀螺仪对象
frc::I2C temp{frc::I2C::kOnboard,0x0c};
frc::LinearFilter<double> linear_filter = frc::LinearFilter<double>::SinglePoleIIR(0.5,0.02_s);//线性过滤器
void Robot::RobotInit(){
  uint8_t id[1];
  int device_id=gyro.Read(0x75,1,id);
  frc::SmartDashboard::PutNumber("id",device_id);
  gyro.Write(0x6a,0x00);
  //frc2::Wait(0.05_s);
  gyro.Write(0x37,0x02);
  temp.Write(0x0a,0x16);
  gyro.Write(29,9);
  gyro.Write(106,96);
}

void Robot::TeleopPeriodic(){
  temp.Write(0x0a,0x16);
  uint8_t gout[6];
  bool bl=gyro.Read(0x43,6,gout);
  frc::SmartDashboard::PutBoolean("bl",bl);
  double data=(gout[0]<<8|gout[1]);//调整gout[]获取不同的值  
  frc::SmartDashboard::PutNumber("X",data);
  frc::SmartDashboard::PutNumber("linear_filter2",linear_filter2.Calculate(data));
  //frc2::Wait(0.05_s);
}