#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "LiquidCrystal_I2C.h"
#include "Wire.h"
// ---------------------------------------------------------------------------
#define SERIALSPEED 115200
#define YAW 0
#define PITCH 1
#define ROLL 2
#define COLCHAR 16
#define LINCHAR 2
#define LCDADDR 0x27
// Values that you get from calibration tool
#define AcelX_Offset -498
#define AcelY_Offset -806
#define AcelZ_Offset 1103
#define GiroX_Offset -17
#define GiroY_Offset -106
#define GiroZ_Offset 69
// --------------------- Liquid Crystal - I2C -------------------------------
LiquidCrystal_I2C lcd(LCDADDR, COLCHAR, LINCHAR);
// --------------------- MPU650 variables ------------------------------------
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
// 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
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
// ---------------------------------------------------------------------------
/**
* Interrup d�tection routine
*/
void dmpDataReady() {
mpuInterrupt = true;
}
/**
* Setup configuration
*/
void setup() {
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
Serial.begin(SERIALSPEED);
Serial.println(F("Initializing I2C devices..."));
lcd.begin();
lcd.clear();
lcd.setCursor(1, 0);
lcd.print("MPU6050 / LCD");
lcd.backlight();
mpu.initialize();
// Verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// Load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
lcd.setCursor(2, 1);
lcd.print("Initializing DMP...");
// MPU calibration: set YOUR offsets here.
mpu.setXAccelOffset(AcelX_Offset);
mpu.setYAccelOffset(AcelY_Offset);
mpu.setZAccelOffset(AcelZ_Offset);
mpu.setXGyroOffset(GiroX_Offset);
mpu.setYGyroOffset(GiroY_Offset);
mpu.setZGyroOffset(GiroZ_Offset);
// Returns 0 if it worked
if (devStatus == 0) {
// Turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// Enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0 : #pin2)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// Set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// Get expected DMP packet size for later comparison
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(")"));
}
lcd.clear();
}
/**
* Main program loop
*/
void loop() {
float Ax, Ay, Az;
// If programming failed, don't try to do anything
if (!dmpReady) {
lcd.setCursor(0, 1);
lcd.print("Err: dmpReady ");
return;
}
// Wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
mpu.resetFIFO();
Serial.print("Err: mpuInterrupt ");
Serial.print(fifoCount);
Serial.print("/");
Serial.println(packetSize);
}
// 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 & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
Serial.print("FIFO overflow! ");
Serial.println(fifoCount);
mpu.resetFIFO();
// Otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// 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;
// Convert Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Print angle values in degrees.
Ax = ypr[ROLL] * (180 / M_PI);
Ay = ypr[PITCH] * (180 / M_PI);
Az = ypr[YAW] * (180 / M_PI);
lcd.setCursor(0, 0);
lcd.print("X=");
lcd.print(Ax);
lcd.print(" ");
lcd.setCursor(8, 0);
lcd.print("Y=");
lcd.print(Ay);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(" Z=");
lcd.print(Az);
lcd.print(" ");
}
}
#include <MPU6050_6Axis_MotionApps20.h>
#include <I2Cdev.h>
// I2Cdev and MPU6050 must be installed as libraries
#include "Wire.h"
/////////////////////////////////// CONFIGURATION /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone=8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone=1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // <-- use for AD0 high
int16_t ax, ay, az,gx, gy, gz;
int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
/////////////////////////////////// SETUP ////////////////////////////////////
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
// initialize serial communication
Serial.begin(115200);
// initialize device
accelgyro.initialize();
// wait for ready
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()){
Serial.println(F("Send any character to start sketch.\n"));
delay(1500);
}
while (Serial.available() && Serial.read()); // empty buffer again
// start message
Serial.println("\nMPU6050 Calibration Sketch");
delay(2000);
Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
delay(3000);
// verify connection
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(1000);
// reset offsets
accelgyro.setXAccelOffset(0);
accelgyro.setYAccelOffset(0);
accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(0);
accelgyro.setYGyroOffset(0);
accelgyro.setZGyroOffset(0);
}
/////////////////////////////////// LOOP ////////////////////////////////////
void loop() {
if (state==0){
Serial.println("\nReading sensors for first time...");
meansensors();
state++;
delay(1000);
}
if (state==1) {
Serial.println("\nCalculating offsets...");
calibration();
state++;
delay(1000);
}
if (state==2) {
meansensors();
Serial.println("\nFINISHED!");
Serial.print("\nSensor readings with offsets:\t");
Serial.print(mean_ax);
Serial.print("\t");
Serial.print(mean_ay);
Serial.print("\t");
Serial.print(mean_az);
Serial.print("\t");
Serial.print(mean_gx);
Serial.print("\t");
Serial.print(mean_gy);
Serial.print("\t");
Serial.println(mean_gz);
Serial.print("Your offsets:\t");
Serial.print(ax_offset);
Serial.print("\t");
Serial.print(ay_offset);
Serial.print("\t");
Serial.print(az_offset);
Serial.print("\t");
Serial.print(gx_offset);
Serial.print("\t");
Serial.print(gy_offset);
Serial.print("\t");
Serial.println(gz_offset);
Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
while (1);
}
}
/////////////////////////////////// FUNCTIONS ////////////////////////////////////
void meansensors(){
long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;
while (i<(buffersize+101)){
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
buff_ax=buff_ax+ax;
buff_ay=buff_ay+ay;
buff_az=buff_az+az;
buff_gx=buff_gx+gx;
buff_gy=buff_gy+gy;
buff_gz=buff_gz+gz;
}
if (i==(buffersize+100)){
mean_ax=buff_ax/buffersize;
mean_ay=buff_ay/buffersize;
mean_az=buff_az/buffersize;
mean_gx=buff_gx/buffersize;
mean_gy=buff_gy/buffersize;
mean_gz=buff_gz/buffersize;
}
i++;
delay(2); //Needed so we don't get repeated measures
}
}
void calibration(){
ax_offset=-mean_ax/8;
ay_offset=-mean_ay/8;
az_offset=(16384-mean_az)/8;
gx_offset=-mean_gx/4;
gy_offset=-mean_gy/4;
gz_offset=-mean_gz/4;
while (1){
int ready=0;
accelgyro.setXAccelOffset(ax_offset);
accelgyro.setYAccelOffset(ay_offset);
accelgyro.setZAccelOffset(az_offset);
accelgyro.setXGyroOffset(gx_offset);
accelgyro.setYGyroOffset(gy_offset);
accelgyro.setZGyroOffset(gz_offset);
meansensors();
Serial.println("...");
if (abs(mean_ax)<=acel_deadzone) ready++;
else ax_offset=ax_offset-mean_ax/acel_deadzone;
if (abs(mean_ay)<=acel_deadzone) ready++;
else ay_offset=ay_offset-mean_ay/acel_deadzone;
if (abs(16384-mean_az)<=acel_deadzone) ready++;
else az_offset=az_offset+(16384-mean_az)/acel_deadzone;
if (abs(mean_gx)<=giro_deadzone) ready++;
else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);
if (abs(mean_gy)<=giro_deadzone) ready++;
else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);
if (abs(mean_gz)<=giro_deadzone) ready++;
else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);
if (ready==6) break;
}
}