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

这是我们在宝藏上买的一个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接口共有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接到CS在0上，应以正确的方式连接NCS和CS0

我们正式给的API在集中找到了需要使用的类别

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

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

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

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

GY-91 - NI roboRIO

3V3 - 3.3V

GND - 地线

SCL - SCL

SDA - SDA

#include 
#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 
#include 
#include
#include
#include
#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);
}