hello,
im using an arduino UNO,
Im using the imu 6050 and an encoder in order to estimate distance. I get the yaw angle from the imu and the distance from the encoder (ky-040) and with a formule I estimate position. this positions are stored after that in the sd card by a MicroSD reader for arduino.
my problem is that after some iteration (1 ,5, 10, 40 more or less) my arduino stop sending information to the SD Card.
This is my code:
#include <MPU9250.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#define YAW 0
#define PITCH 1
#define ROLL 2
MPU6050 mpu;
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
int a = 0;
double t1;
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false; // Indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
Serial.begin(9600);
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
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.setXAccelOffset(436);//556
mpu.setYAccelOffset(-1485);//1448
mpu.setZAccelOffset(914);//902
mpu.setXGyroOffset(47);//81
mpu.setYGyroOffset(-24);//-13
mpu.setZGyroOffset(-28);//-27/8
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0 : #pin2)..."));
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(")"));
}
}
void loop() {
if (!dmpReady) {
return;
}
while (!mpuInterrupt && fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
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;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); ///save in this order [yaw, pitch, roll] in ypr table
Serial.print(ypr[YAW] * (180 / M_PI)); Serial.print("\t");
Serial.print(ypr[PITCH] * (180 / M_PI)); Serial.print("\t");
Serial.println(ypr[ROLL] * (180 / M_PI));
}
}
