HI
I have been playing around with a MPU-6500 attached to a Uno. I had everything working, so decided to create a gyro class with a .h and .cpp file with all the necessary coding so I can create a gyro class in my main loop. However, as soon as I create a instance to the Gyro class the serial monitor doesn't work and I have issues with Comport not available. If I comment out the the line to create the class everything works ok.
Here is my main sketch. Most is commented out and I still cannot get to work:
#include "GyroAccel.h"
#include "Arduino.h"
GyroAccel gyro;
float Pitch=0,Roll=0,Yaw=0;//to hold values from Gyro,Accelerometer and compass
void setup() {
Serial.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
//gyro.GetRaw();
//gyro.ProcessRaw();
delay(300);
//Pitch=gyro.AnglePitch;
//Roll=gyro.AngleRoll;
//Yaw=gyro.Yaw;
Serial.print(F("Roll = ")); Serial.print(Roll);
//Serial.print(F(" :Pitch = ")); Serial.print(Pitch);
//Serial.print(F(" :Yaw = ")); Serial.println(Yaw);
//delay(1000);
}
Here is the header file
/*
gyroAccel.h - Library for the MPU6050 values from Gyro & Accelerometer
created by Nathan Jones 16/03/2016
*/
#include "Arduino.h"
#ifndef GyroAccel_h
#define GyroAccel_h
class GyroAccel
{
public:
GyroAccel();
void GetRaw();
void ProcessRaw();
float AnglePitch, AngleRoll, Yaw;
private:
//MPU6050 mpu;
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int16_t GyXPrev;
int GyroPercentage=0.99; //Fudge Value used in filter usually between 0.9-0.9999999....
float Xg,Yg,Zg;
float angleX,angleY,angleZ,angleXc,angleYc,angleZc;
float AccelXangle, AccelYangle;
unsigned long start, finished, elapsed;
float dt=0.015;
};
Here is the .cpp
/*
gyroAccel.cpp - Library for the MPU6050 values from Gyro & Accelerometer
created by Nathan Jones 16/03/2016
*/
#include "GyroAccel.h"
#include <Wire.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include "I2Cdev.h"
#include "Arduino.h"
MPU6050 mpu; //create the MPU6050 object
GyroAccel::GyroAccel() //constructor. All variables initialized
{
mpu.initialize();
AnglePitch=0;
AngleRoll=0;
Yaw=0;
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(31);
mpu.setYGyroOffset(17);
mpu.setZGyroOffset(11);
mpu.setZAccelOffset(1245);
mpu.setYAccelOffset(-1501);
mpu.setXAccelOffset(-1061);
start=millis();
}
void GyroAccel::GetRaw()
{
//Calculating dt//The firstloop is going to be non representative. It finishes here as we need to know the timer from the last read
finished=millis();
elapsed=finished-start;
if (elapsed<0)
{
elapsed=0;
}//just incase first loop is to quick, cant have a negative number
dt=elapsed/1000.0;
start = elapsed = 0;//reset timers
start=millis();//timer starts running
mpu.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
}
void GyroAccel::ProcessRaw()
{
//Stage 1 Convert gyro data to Angles (degrees)//////////////////////////////////////////////////////
// From the datasheet: 131 mdps/digit as sensitivity set to 250d/s
Xg=GyX*0.131; // Angular rate
// Angular_rate * dt = angle
angleXc = Xg*dt;
angleX = angleX + angleXc;
//---------- Y - Axis
Yg=GyY*0.131;
angleYc = Yg*dt;
angleY = angleY + angleYc;
//---------- Z - Axis
Zg=GyZ*0.131;
angleZc = Zg*dt;
angleZ = angleZ + angleZc;
//Stage 2 convert accelerometer raw to angles///////////////////////////////////////////////////////////
AccelXangle = 57.295*atan((float)AcY/ sqrt(pow((float)AcZ,2)+pow((float)AcX,2))); //pitch
AccelYangle = 57.295*atan((float)-AcX/ sqrt(pow((float)AcZ,2)+pow((float)AcY,2)));//roll
//Stage 3 Complimentary Filter///////////////////////////////////////////////////////////////////////////
AnglePitch=(GyroPercentage)*angleX+(1-GyroPercentage)*AccelXangle;
AngleRoll=(GyroPercentage)*angleY+(1-GyroPercentage)*AccelYangle;
//Yaw=(GyroPercentage)*angleZ+(1-GyroPercentage)*YawCompass;//no compass yet!
Yaw=angleZ;//temporary to give yaw a value
}
Any advice greatly appreciated as it driving me nuts. Any better ways to do this?
Thanks
Nathan
