问题描述
我使用传感器 mpu6050 来控制我的机械臂的运动。当它是一个独立程序时，代码工作正常，但是当代码被编译到主程序中时，我一直遇到“FIFO溢出”。这是我正在使用的代码。
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "mpu6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
mpu6050 mpu;
RF24 radio(9,8); // CE,CSN
const byte address[6] = "00001";
const int AccReadings = 10;
//Wrist Roll
int datax[AccReadings];
int WRIndex = 0;
int WRtotal = 0;
int WRaverage = 0;
//Wrist Pitch
int DataY[AccReadings];
int WPIndex = 0;
int WPtotal = 0;
int WPaverage = 0;
//Shoulder Lift
int DataY2[AccReadings];
int SLIndex = 0;
int SLtotal = 0;
int SLaverage = 0;
//Elbow Lift
int ELaverage = 0;
//Arm Rotation
int araverage = 0;
float correct;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;
// mpu control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from mpu
uint8_t devstatus; // return status after each device operation (0 = success,!0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w,x,y,z] quaternion container
VectorInt16 aa; // [x,z] accel sensor measurements
VectorInt16 aaReal; // [x,z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x,z] world-frame accel sensor measurements
VectorFloat gravity; // [x,z] gravity vector
float euler[3]; // [psi,theta,phi] Euler angle container
float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container and gravity vector
struct Sensor_Data
{
int WristRoll;
int WristPitch;
int ShoulderLift;
int ElbowLift;
int ArmRotation;
};
Sensor_Data data;
//Interrupt Detection
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
radio.begin();
radio.openWritingPipe(address);
radio.setPALevel(RF24_PA_MIN);
radio.stopListening();
//Zero-fill Arrays
for (int i = 0; i < AccReadings; i++)
{
datax[i] = 0;
}
for (int j = 0; j < AccReadings; j++)
{
DataY[j] = 0;
}
for (int k = 0; k < AccReadings; k++)
{
DataY2[k] = 0;
}
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400,true);
#endif
Serial.begin(115200);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN,INPUT);
devstatus = mpu.dmpInitialize();
mpu.setXGyroOffset(49);
mpu.setYGyroOffset(-18);
mpu.setZGyroOffset(9);
mpu.setZAccelOffset(4427);
if (devstatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),dmpDataReady,RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
// ERROR!
// 1 = initial memory load Failed
// 2 = DMP configuration updates Failed
// (if it's going to break,usually the code will be 1)
// Serial.print(F("DMP Initialization Failed (code "));
//Serial.print(devstatus);
//Serial.println(F(")"));
}
}
void loop()
{
/*smoothWR();
movementWR();
smoothWP();
movementWP();
smoothSL();
movementSL();*/
ElbowMovement();
radio.write(&data,sizeof(Sensor_Data));
}
void smoothWR()
{
WRtotal = WRtotal - datax[WRIndex];
datax[WRIndex] = analogRead(A0);
WRtotal = WRtotal + datax[WRIndex];
WRIndex = WRIndex + 1;
if (WRIndex >= AccReadings)
{
WRIndex = 0;
}
WRaverage = WRtotal / AccReadings;
//Serial.println(WRaverage);
}
void movementWR()
{
WRaverage = map(WRaverage,278,419,180);
data.WristRoll = constrain(WRaverage,180);
//Serial.println(data.WristRoll);
}
void smoothWP()
{
WPtotal = WPtotal - DataY[WPIndex];
DataY[WPIndex] = analogRead(A1);
WPtotal = WPtotal + DataY[WPIndex];
WPIndex = WPIndex + 1;
if (WPIndex >= AccReadings)
{
WPIndex = 0;
}
WPaverage = WPtotal / AccReadings;
//Serial.println(WPaverage);
}
void movementWP()
{
WPaverage = map(WPaverage,280,421,135);
data.WristPitch = constrain(WPaverage,135);
//Serial.println(data.WristPitch);
}
void smoothSL()
{
SLtotal = SLtotal - DataY2[SLIndex];
DataY2[SLIndex] = analogRead(A2);
SLtotal = SLtotal + DataY2[SLIndex];
SLIndex = SLIndex + 1;
if (SLIndex >= AccReadings)
{
SLIndex = 0;
}
SLaverage = SLtotal / AccReadings;
//Serial.println(SLaverage);
}
void movementSL()
{
SLaverage = map(SLaverage,410,270,180);
data.ShoulderLift = constrain(SLaverage,35,180);
//Serial.println(data.ShoulderLift);
}
void ElbowMovement()
{
// if programming Failed,don't try to do anything
if (!dmpReady) return;
// wait for mpu interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize)
{
if (mpuInterrupt && fifoCount < packetSize)
{
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & _BV(mpu6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise,check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & _BV(mpu6050_INTERRUPT_DMP_INT_BIT))
{
// wait for correct available data length,should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer,packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// Get Yaw,Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q,fifoBuffer);
mpu.dmpGetGravity(&gravity,&q);
mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
// Yaw,Pitch,Roll values - radians to degrees
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
// Skip 300 readings (self-calibration process)
if (int l = 0; l <= 300) {
correct = ypr[0]; // Yaw starts at random value,so we capture last value after 300 readings
L++;
}
// After 300 readings
else {
ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract the last random Yaw value from the currrent value to make the Yaw 0 degrees
// Map the values of the mpu6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
ELaverage = map(ypr[0],-90,90,180);
data.ElbowLift = constrain(ELaverage,30,110);
araverage = map(ypr[1],180);
data.ArmRotation = constrain(araverage,180);
//Serial.println(data.ElbowLift);
Serial.println(ypr[1]);
}
#endif
}
}
有什么办法可以解决FIFO溢出问题？此外，当我尝试使用 Jeff Rowberg 的示例代码 mpu6050_DMP6 时，程序将在几秒钟后冻结。有什么解决办法吗？这些是我正在使用的示例代码。
#include "I2Cdev.h"
#include "mpu6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
mpu6050 mpu;
float correct;
int j = 0;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;
// mpu control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from mpu
uint8_t devstatus; // return status after each device operation (0 = success,roll] yaw/pitch/roll container and gravity vector
//Interrupt Detection
volatile bool mpuInterrupt = false; // indicates whether mpu interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup()
{
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400,true);
#endif
Serial.begin(38400);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN,INPUT);
devstatus = mpu.dmpInitialize();
mpu.setXGyroOffset(17);
mpu.setYGyroOffset(-69);
mpu.setZGyroOffset(27);
mpu.setZAccelOffset(1551);
if (devstatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
// ERROR!
// 1 = initial memory load Failed
// 2 = DMP configuration updates Failed
// (if it's going to break,usually the code will be 1)
// Serial.print(F("DMP Initialization Failed (code "));
//Serial.print(devstatus);
//Serial.println(F(")"));
}
}
void loop()
{
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q,fifoBuffer);
mpu.dmpGetGravity(&gravity,&q);
mpu.dmpGetYawPitchRoll(ypr,&gravity);
if (j <= 300)
{
correct = ypr[0]; // Yaw starts at random value,so we capture last value after 300 readings
j++;
}
else
{
ypr[0] = ypr[0] - correct;
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
}
#endif
}
}
解决方法
暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！
如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@）