Hello, I'm currently working on a project that will require me to use an MPU6050 module in order to control the motion of the servo motors.
I haven't had much background with Arduino but with some programming knowledge gathered up enough information to alter an already made program to fit my project criteria. The original file was taken from the following video How to control servo using MPU6050 Gyroscope with Arduino - YouTube and subsequently from the link in the description.
I also used some other sketches to alter mine.
The code works fine, and achieves the goal I am trying to achieve, which is the control of two servo motors using roll and pitch motion from the MPU, I had to change up the orientation and the direction but that's not what effecting the error I am having.
The problem I am having in all of the codes that I have used, altered and unaltered is the fact that after running for some time the Arduino reinitialized, causing my servos to move back into the starting position and sometimes turning off completely, as well as some other problems such as not initializing first try. But the issue I am asking help with is why is it re initializing, and turning off.
"My" aka altered Code:
#include <Servo.h>
Servo myservoY;
Servo myservoX;
#include "I2Cdev.h"
#include "math.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
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, 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]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
uint8_t teapotPacket[14] = { '
Here are the example of how the program is running
First initialization:
Random re initialization:
another random re initialization:
I thought this had to deal with the variable FIFO, as I thought once it overflows the program resets but the resets happen at random, can happen in 2 seconds, or in 30, regardless of how many values are read.
I've tried to dig into the interrupt, but with no luck, I either don't understand it or it doesn't effect it. I'm leaning towards me not having a clue lol.
I can of course provide full setup of my project if needed.
This is more or less the last portion of my project and I would greatly appreciate any help, to help me fix the problem, or at least educate me on what the hell is FIFO, and what exactly does the interrupt do in this case. Thank You!, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
myservoY.attach(9); // Attach Y servo to pin 9
myservoX.attach(10);// Attach X servo to pin 10
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(37);
mpu.setYGyroOffset(-11);
mpu.setZGyroOffset(-2);
mpu.setZAccelOffset(1688); // 1688 factory default for my test chip
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus(); Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
// Serial.print(ypr[0] * 180/M_PI);
if (ypr[1] > 0)
{
ypr[1]=ypr[1]-M_PI;
}
else if (ypr[1] < 0)
{
ypr[1]=ypr[1]+M_PI;
}
if (ypr[2] > 0)
{
ypr[2]=ypr[2]-M_PI;
}
else if (ypr[2] < 0)
{
ypr[2]=ypr[2]+M_PI;
}
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
myservoY.write(int(ypr[2] * -180/M_PI)+90); // Rotation around Y
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
myservoX.write(int(ypr[1] * 180/M_PI)+90); // Rotation around X
#endif
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
Here are the example of how the program is running
First initialization:

Random re initialization:

another random re initialization:

I thought this had to deal with the variable FIFO, as I thought once it overflows the program resets but the resets happen at random, can happen in 2 seconds, or in 30, regardless of how many values are read.
I've tried to dig into the interrupt, but with no luck, I either don't understand it or it doesn't effect it. I'm leaning towards me not having a clue lol.
I can of course provide full setup of my project if needed.
This is more or less the last portion of my project and I would greatly appreciate any help, to help me fix the problem, or at least educate me on what the hell is FIFO, and what exactly does the interrupt do in this case. Thank You!