Im using ARDUINO UNO to read data from IMU6050 and an hall effect sensor. This data are stored in an external MicroSd memory (using a MicroSd reader). The problem is that Arduino sometimes send data continuosely for a long period (more that 20 minutes). But sometimes it stop sending data to the serial port and no data can be founded in the Sd card. Sometimes Arduino restart counting from zero, so the hall sensor contour became zero and then it start to count again. I can't find the problem source, and to iliminate buffer overflow, I've used a fonction which empty the buffer after each reading. How to deal with this problem ?.
This is my code
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include "math.h"
#include <SPI.h>
#include <SD.h>
File myFile;
MPU6050 mpu;
#define DEBUG
#ifdef DEBUG
//#define DPRINT(args...) Serial.print(args) //OR use the following syntax:
#define DPRINTSTIMER(t) for (static unsigned long SpamTimer; (unsigned long)(millis() - SpamTimer) >= (t); SpamTimer = millis())
#define DPRINTSFN(StrSize,Name,...) {charS[StrSize];Serial.print("\t");Serial.print(Name);Serial.print(" ");
Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Percision
#define DPRINTLN(...) Serial.println(__VA_ARGS__)
#else
#define DPRINTSTIMER(t) if(false)
#define DPRINTSFN(...) //blank line
#define DPRINTLN(...) //blank line
#define PI 3.1415926535897932384626433832795
#endif
#define LED_PIN 13 //
// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino
// -4232 -706 1729 173 -94 37
// XA YA ZA XG YG ZG
//int MPUOffsets[6] = { -4232, -706, 1729, 173, -94, 37};
int MPUOffsets[6] = { 436,-1485,914,47,-24,-28};//{413, -1423, 907, 51, -38, -35}
int led = 13 ; // LED on arduino
int digitalPin = 7; // linear Hall magnetic sensor digital interface
int analogPin = A0; // linear Hall magnetic sensor analog interface
int digitalVal ; // digital readings
int analogVal; // analog readings
int count=0;
float myTime;
int a=0,def=0,def2=0;
float b=0,dist=0,pres_dist=0,prec_dist=0,x=0,m=0;
float angle=0;
// ================================================================
// === i2c SETUP Items ===
// ================================================================
void i2cSetup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#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
}
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === MPU DMP SETUP ===
// ================================================================
int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20; // counts interrupt start at -20 to get 20+ good values before assuming connected
// MPU control/status vars
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
byte StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)
void MPU6050Connect() {
static int MPUInitCntr = 0;
// initialize device
mpu.initialize(); // same
// load and configure the DMP
devStatus = mpu.dmpInitialize();// same
if (devStatus != 0) {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
MPUInitCntr++;
Serial.print(F("MPU connection Try #"));
Serial.println(MPUInitCntr);
Serial.print(F("DMP Initialization failed (code "));
Serial.print(StatStr[devStatus]);
Serial.println(F(")"));
if (MPUInitCntr >= 10) return; //only try 10 times
delay(10);
MPU6050Connect(); // Lets try again
return;
}
mpu.setXAccelOffset(MPUOffsets[0]);
mpu.setYAccelOffset(MPUOffsets[1]);
mpu.setZAccelOffset(MPUOffsets[2]);
mpu.setXGyroOffset(MPUOffsets[3]);
mpu.setYGyroOffset(MPUOffsets[4]);
mpu.setZGyroOffset(MPUOffsets[5]);
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
Serial.print("mpu.getInterruptDrive= "); Serial.println(mpu.getInterruptDrive());
attachInterrupt(0, dmpDataReady, RISING); //pin 2 on the Uno
mpuIntStatus = mpu.getIntStatus(); // Same
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
delay(2000); // Let it Stabalize
mpu.resetFIFO(); // Clear fifo buffer
mpu.getIntStatus();
mpuInterrupt = false; // wait for next interrupt
}
// ================================================================
// === MPU DMP Get Data ===
// ================================================================
void GetDMP() { // Best version I have made so far
// Serial.println(F("FIFO interrupt at:"));
// Serial.println(micros());
static unsigned long LastGoodPacketTime;
mpuInterrupt = false;
FifoAlive = 1;
fifoCount = mpu.getFIFOCount();
if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
mpu.resetFIFO();// clear the buffer and start overmpu.resetFIFO();
} else {
while (fifoCount >= packetSize) { // Get the packets until we have the latest!
mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
fifoCount -= packetSize;
}
LastGoodPacketTime = millis();
MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
}
}
// ================================================================
// === MPU Math ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Yaw = -(ypr[0] * 180.0 / M_PI);
Pitch = (ypr[1] * 180.0 / M_PI);
Roll = (ypr[2] * 180.0 / M_PI);
def2=b-Yaw;
if (def2>340 || def2<-340)
{ Yaw=Yaw; }
else if (def2>10 || def2<-10) {
Yaw=b;
}
// DPRINTSTIMER(150) {
// Serial.print(Yaw);
// DPRINTSFN(15, "\tYaw:", Yaw, 6, 1);
// DPRINTSFN(15, "\tPitch:", Pitch, 6, 1);
// DPRINTSFN(15, "\tRoll:", Roll, 6, 1);
// DPRINTLN();
// }
}
// ================================================================
// === Setup ===
// ================================================================
void setup() {
pinMode (led, OUTPUT);
pinMode (digitalPin, INPUT);
Serial.begin(115200); //115200,115200,57600
while (!Serial);
Serial.print("Initializing SD card...");
if (!SD.begin(10)) {
Serial.println("initialization failed!");
while (1);
}
Serial.println("initialization done.");
Serial.println("i2cSetup");
i2cSetup();
Serial.println("MPU6050Connect");
MPU6050Connect();
Serial.println("Setup complete");
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === Loop ===
// ================================================================
void loop() {
myFile = SD.open("t21.txt", FILE_WRITE);
if (myFile) {
Serial.print("Writing to test.txt...");
// myFile.println("This is a test file :)");
for (int i = 0; i < 100; i++) {
if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
GetDMP();
}
analogVal = analogRead(analogPin);
def=analogVal-a;
if (def<-20){
count++;
}
pres_dist=count;
dist=pres_dist-prec_dist;
prec_dist=pres_dist;
dist=dist*0.35 ;
a=analogVal;
b=Yaw;
Serial.println(i);
angle=Yaw*PI/180;
x=x+dist*cos(angle);
m=m+dist*sin(angle);
myTime = millis();
myFile.print(x);
myFile.print("\t");
myFile.print(m);
myFile.print("\t");
myFile.println(myTime/1000);
delay(20);
}
myFile.close();
Serial.println("done.");
Serial.println("End");
} else {Serial.println("error opening test.txt");
}
Serial.print(x);
Serial.print("\t");
Serial.print(m);
Serial.print("\t");
Serial.print(myTime/1000);
Serial.print("\t");
Serial.println(Yaw);
}