So Im using the MPU6050 DMP files which have been fantastic for learning to use the module. Im trying to control a small vehicle while taking some readings from the unit but Im getting a FIFO error when I include the control code. I was hoping someone here might have an idea whats going on. Im fairly new to arduino programming.
Thanks in advance!
Whiplash
#include <Servo.h>
#include <WString.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
float speedX;
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3];
uint8_t packet[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
Servo cameraX;
Servo cameraY;
int CX = 10;
int CY = 11;
int cameraXPos = 90;
int cameraYPos = 90;
int E1 = 6; //M1 Speed Control
int E2 = 5; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 7; //M2 Direction Control
int led = 13; //LED Jumper
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup(void)
{
//Joining I2C bus
Wire.begin();
TWBR = 12;
Serial.begin(250000);//baud rate
int i;
for (i = 5; i <= 8; i++)
pinMode(i, OUTPUT);
//Setup
mpu.initialize();
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
while (Serial.available() && Serial.read()); // empty buffer
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(0);
mpu.setZGyroOffset(0);
mpu.setZAccelOffset(0);
if (devStatus == 0) {
mpu.setDMPEnabled(true);
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
cameraX.attach(CX);
cameraX.write(cameraXPos);
cameraY.attach(CY);
cameraY.write(cameraYPos);
Serial.println("Online: Autobots Roll Out");
}
void loop(void)
{
while (Serial.available() < 1) {
}
char val = ' ';
int leftspeed = 150;
int rightspeed = 150;
if (val == 'e' || val == 'e') {
cameraXPos += 1;
if (cameraXPos > 150)
cameraXPos = 150;
cameraX.write(cameraXPos);
}
if (val == 'q' || val == 'Q') {
cameraXPos -= 1;
if (cameraXPos < 20)
cameraXPos = 20;
cameraX.write(cameraXPos);
}
if (val == 'i' || val == 'I') {
cameraYPos += 1;
if (cameraYPos > 150)
cameraYPos = 150;
cameraY.write(cameraYPos);
}
if (val == 'k' || val == 'K') {
cameraYPos -= 1;
if (cameraYPos < 20)
cameraYPos = 20;
cameraY.write(cameraYPos);
}
switch (val) // Perform an action depending on the command
{
case 'w'://Move Forward
case 'W':
forward(leftspeed, rightspeed);
break;
case 's'://Move Backwards
case 'S':
reverse(leftspeed, rightspeed);
break;
case 'a'://Turn Left
case 'A':
left(leftspeed, rightspeed);
break;
case 'd'://Turn Right
case 'D':
right(leftspeed, rightspeed);
break;
default:
stop();
break;
}
if (!dmpReady) return;//break if CHIP NOT READY
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10 || fifoCount == 1024)) {
mpu.resetFIFO();
Serial.println(F("FIFO Overlfow"));
}
else if(mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.resetFIFO();
fifoCount -= packetSize;
}
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
}
void stop(void) //Stop
{
digitalWrite(E1, LOW);
digitalWrite(E2, LOW);
}
void forward(char a, char
{
analogWrite(E1, a);
digitalWrite(M1, LOW);
analogWrite(E2, ;
digitalWrite(M2, LOW);
}
void reverse(char a, char
{
analogWrite(E1, a);
digitalWrite(M1, HIGH);
analogWrite(E2, ;
digitalWrite(M2, HIGH);
}
void left(char a, char
{
analogWrite(E1, a);
digitalWrite(M1, HIGH);
analogWrite(E2, ;
digitalWrite(M2, LOW);
}
void right(char a, char
{
analogWrite(E1, a);
digitalWrite(M1, LOW);
analogWrite(E2, ;
digitalWrite(M2, HIGH);
}
