Professional Documents
Culture Documents
Gy Dy2
Gy Dy2
Gy Dy2
//
// Changelog:
/* ============================================
The above copyright notice and this permission notice shall be included in
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
THE SOFTWARE.
===============================================
*/
//===============================================================
//===============================================================
//be closer to zero then 2 changes have to be made in the #define section below.
//the second change should be "AllowedRange" value should be lowered to acceptable range
//like 50 or 10.
//===============================================================
//===============================================================
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
#define OUTPUT_READABLE_YAWPITCHROLL
//#define OUTPUT_READABLE_REALACCEL
#define OUTPUT_TEAPOT
// components with gravity removed and adjusted for the world frame of
//#define OUTPUT_READABLE_WORLDACCEL
//=================================================================
//===============================================================
//===============================================================
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "MPU6050.h"
#include "Average.h"
//=================================================================
//===============================================================
//===============================================================
// is used in I2Cdev.h
#include "Wire.h"
#endif
//================================================================
//===============================================================
//===============================================================
#define StabilityTime 25000 //25 seconds, could be 40secs, only for dmp
#define LED_PIN 13
#define LED_PIN2 8
#define AllowedRange 150 //this is value is in range round zero that is acceptable and no
calibration of that axis is done.
#define GyroStepSize 1
#define AccStepSize 0.2 // this value can be lower to 0.1 to get calibrated data closer to zero but
then calibration will take more time
#define MaxIncVal 10
//=================================================================
//===============================================================
//===============================================================
MPU6050 mpu;
//=================================================================
//===============================================================
//===============================================================
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
//=================================================================
//===============================================================
//===============================================================
// orientation/motion vars
#endif
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
#endif
#endif
#ifdef OUTPUT_READABLE_EULER
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
#endif
//=================================================================
//===============================================================
//===============================================================
void SetXAccelOffset(MPU6050,int16_t);
void SetYAccelOffset(MPU6050,int16_t);
void SetZAccelOffset(MPU6050,int16_t);
void SetXGyroOffset(MPU6050,int16_t);
void SetYGyroOffset(MPU6050,int16_t);
void SetZGyroOffset(MPU6050,int16_t);
//=================================================================
// ================================================================
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
//===================================================================
// ================================================================
// ================================================================
char DataFlowBreak;
bool waitstatus=false;
Average<int16_t> Mode(ModeRange);
//=====================================================================
void setup() {
Wire.begin();
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
// initialize device
mpu.initialize();
// verify connection
Serial.println("Calibrating Gyroscope");
if(autocalibrate('g','X',mpu)){
}else{
if(autocalibrate('G','y',mpu)){
}else{
if(autocalibrate('g','Z',mpu)){
}else{
Serial.println("Calibrating Accelerometer");
if(autocalibrate('a','x',mpu)){
if(autocalibrate('A','Y',mpu)){
}else{
if(autocalibrate('A','Z',mpu)){
}else{
// SetZAccelOffset(mpu,16384);
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
currentMillis = millis();
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
Serial.print(devStatus);
Serial.println(F(")"));
pinMode(LED_PIN, OUTPUT);
}
void loop() {
if (!dmpReady) return;
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// .
// .
// .
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
// wait for correct available data length, should be a VERY short wait
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
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);
#endif
#ifdef OUTPUT_READABLE_EULER
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
#endif
#ifdef OUTPUT_READABLE_REALACCEL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
//The purpose of this function is to get a value out of a very variable data stream. This function
uses
//mode technique to get a very accurate value. the ModeRange is defined as the maximum
number of values on which modes is to be applied
//here the user have to pass a or g to indicate sensor, axis required i.e. x y z and finally mpu
object
int ii;
int16_t Final;
if(sensor != 'a' && sensor !='A' && sensor != 'g' && sensor !='G'){ //if user enters anything
except a A g or G return false
return 0;
}else if(axis !='x' && axis !='X' && axis !='y' && axis !='Y' && axis !='z' && axis !='Z'){ //if user
enters anything except x X y Y z or Z return false
return 0;}
for(ii=0; ii<=ModeRange; ii++){
case 'a'://acceleormeter is a or A
case 'A':
switch(axis){
break;
case 'y':
case 'Y':
break;
case'z':
case'Z':
break;
break;
case 'G':
switch(axis){
case 'x':
case 'X':
break;
case 'y':
case 'Y':
break;
case'z':
case'Z':
break;
break;
//delay(1);
Mode.push(Final);
Final=Mode.mode();
return Final;
//This function attempts to auto calibrate the Accelerometer and Gyroscope and returns true if
operation is successful
//and the last argument this function takes is the mpu6050 object that is created.
float n=1;
int16_t initalvalue;
int16_t calibrateValue;
float stepsize=0.5;
if(sensor != 'a' && sensor !='A' && sensor != 'g' && sensor !='G'){ //if user enters anything
except a A g or G return false
return false;
}else if(axis !='x' && axis !='X' && axis !='y' && axis !='Y' && axis !='z' && axis !='Z'){ //if user
enters anything except x X y Y z or Z return false
return false;}
case 'a'://acceleormeter is a or A
case 'A':
stepsize=AccStepSize;
switch(axis)
{case 'x': //axis x
FuncPointerToSetOffset = SetXAccelOffset;
break;
case 'y':
case 'Y':
FuncPointerToSetOffset = SetYAccelOffset;
break;
case'z':
case'Z':
FuncPointerToSetOffset =SetZAccelOffset;
break;
break;
case 'G':
stepsize=GyroStepSize;
switch(axis)
break;
case 'y':
case 'Y':
FuncPointerToSetOffset = SetYGyroOffset;
break;
case'z':
case'Z':
FuncPointerToSetOffset = SetZGyroOffset;
break;
break;
if (abs(calibrateValue) <=AllowedRange ) //when you dont need to calibrate after all. when
data is near to zero in allowable range (default 150)
return true;
calibrateValue=initalvalue/n;
n+=stepsize;
calibrateValue=initalvalue/n;
FuncPointerToSetOffset(mpu, calibrateValue);
//here we check wether the value due to previous offset is closer to zero or the value due to
current offset is closer to zero
//additionally the sign of value is also checked i.e the moment it changes we stop (fro
example value goes from +ve to -ve)
n=n-stepsize;
break;
}else{
firstoutput=secondoutput; //if the current value is lesser than previous value than we
check next value.
return true;
} //end of function
//the reason for creation of these function was that function pointer was used to point to these
function.
//function pointer can not point to a static member function of an object i think. so these
function had to be used.
//additionally we can use these functions to add subtract any other constants for offset
calibration such as +16384 for Az can be inserted here.
mpu.setXAccelOffset(value);
mpu.setYAccelOffset(value);
mpu.setZAccelOffset(value);
mpu.setXGyroOffset(value);
}
void SetYGyroOffset(MPU6050 mpu ,int16_t value){
mpu.setYGyroOffset(value);
mpu.setZGyroOffset(value);