MPU6050_tockn
MPU6050_tockn copied to clipboard
Published
20 hours ago •
tockn
tockn
Modifications, please check if it can be used
- Allows selection of gyroscope range
- Allows accelerometer selection range
- Allows selection of device address I2C (0x68 / 0x69)
Modifications at setup:
void setup() {
Serial.begin(9600);
Wire.begin();
mpu6050.begin(GYRO_CONFIG_500, ACCEL_CONFIG_16G, MPU6050_ADDR);
mpu6050.calcGyroOffsets(true);
}
Modifications MPU6050_tockn.h:
#ifndef MPU6050_TOCKN_H
#define MPU6050_TOCKN_H
#include "Arduino.h"
#include "Wire.h"
#define MPU6050_ADDR 0x68
#define MPU6050_ADDR1 0x69
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1a
#define MPU6050_GYRO_CONFIG 0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I 0x75
#define MPU6050_PWR_MGMT_1 0x6b
#define MPU6050_TEMP_H 0x41
#define MPU6050_TEMP_L 0x42
#define ACCEL_CONFIG_2G 2
#define ACCEL_CONFIG_4G 4
#define ACCEL_CONFIG_8G 8
#define ACCEL_CONFIG_16G 16
#define GYRO_CONFIG_250 250
#define GYRO_CONFIG_500 500
#define GYRO_CONFIG_1000 1000
#define GYRO_CONFIG_2000 2000
class MPU6050{
public:
MPU6050(TwoWire &w);
MPU6050(TwoWire &w, float aC, float gC);
void begin(int gyros = GYRO_CONFIG_500, int accel = ACCEL_CONFIG_2G, int addr = MPU6050_ADDR);
void setGyroOffsets(float x, float y, float z);
void writeMPU6050(byte reg, byte data);
byte readMPU6050(byte reg);
int16_t getRawAccX(){ return rawAccX; };
int16_t getRawAccY(){ return rawAccY; };
int16_t getRawAccZ(){ return rawAccZ; };
int16_t getRawTemp(){ return rawTemp; };
int16_t getRawGyroX(){ return rawGyroX; };
int16_t getRawGyroY(){ return rawGyroY; };
int16_t getRawGyroZ(){ return rawGyroZ; };
float getTemp(){ return temp; };
float getAccX(){ return accX; };
float getAccY(){ return accY; };
float getAccZ(){ return accZ; };
float getGyroX(){ return gyroX; };
float getGyroY(){ return gyroY; };
float getGyroZ(){ return gyroZ; };
void calcGyroOffsets(bool console = false);
float getGyroXoffset(){ return gyroXoffset; };
float getGyroYoffset(){ return gyroYoffset; };
float getGyroZoffset(){ return gyroZoffset; };
void update();
float getAccAngleX(){ return angleAccX; };
float getAccAngleY(){ return angleAccY; };
float getGyroAngleX(){ return angleGyroX; };
float getGyroAngleY(){ return angleGyroY; };
float getGyroAngleZ(){ return angleGyroZ; };
float getAngleX(){ return angleX; };
float getAngleY(){ return angleY; };
float getAngleZ(){ return angleZ; };
private:
TwoWire *wire;
int16_t rawAccX, rawAccY, rawAccZ, rawTemp,
rawGyroX, rawGyroY, rawGyroZ, mpuAddr;
float gyroXoffset, gyroYoffset, gyroZoffset;
float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
float angleGyroX, angleGyroY, angleGyroZ,
angleAccX, angleAccY, angleAccZ;
float angleX, angleY, angleZ;
long interval, preInterval;
float accCoef, gyroCoef, gyrosFactor, accelFactor,
gForce, gForceInv;
};
#endif
Modifications MPU6050_tockn.ccp:
#include "MPU6050_tockn.h"
#include "Arduino.h"
MPU6050::MPU6050(TwoWire &w){
wire = &w;
accCoef = 0.02f;
gyroCoef = 0.98f;
}
MPU6050::MPU6050(TwoWire &w, float aC, float gC){
wire = &w;
accCoef = aC;
gyroCoef = gC;
}
void MPU6050::begin(int gyros, int accel, int addr){
int gyrosConfig;
int accelConfig;
mpuAddr = addr;
switch (gyros) {
case 250:
gyrosConfig = 0x00;
gyrosFactor = 131.0f;
break;
case 500:
gyrosConfig = 0x08;
gyrosFactor = 65.5f;
break;
case 1000:
gyrosConfig = 0x10;
gyrosFactor = 32.8f;
break;
case 2000:
gyrosConfig = 0x18;
gyrosFactor = 16.4f;
break;
default:
gyrosConfig = 0x08;
gyrosFactor = 65.5f;
break;
}
switch (accel) {
case 2:
accelConfig = 0x00;
accelFactor = 16384.0f;
gForce = 2.0f;
gForceInv = -2.0f;
break;
case 4:
accelConfig = 0x08;
accelFactor = 8192.0f;
gForce = 4.0f;
gForceInv = -4.0f;
break;
case 8:
accelConfig = 0x10;
accelFactor = 4096.0f;
gForce = 8.0f;
gForceInv = -8.0f;
break;
case 16:
accelConfig = 0x18;
accelFactor = 2048.0f;
gForce = 16.0f;
gForceInv = -16.0f;
break;
default:
accelConfig = 0x00;
accelFactor = 16384.0f;
gForce = 2.0f;
gForceInv = -2.0f;
break;
}
writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);
writeMPU6050(MPU6050_CONFIG, 0x00);
writeMPU6050(MPU6050_GYRO_CONFIG, gyrosConfig);
writeMPU6050(MPU6050_ACCEL_CONFIG, accelConfig); // 0x18 = 16G | 0x00 = 2G
writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);
this->update();
angleGyroX = this->getAccAngleX();
angleGyroY = this->getAccAngleY();
Serial.println("=================");
Serial.println(accCoef);
Serial.println(gyroCoef);
}
void MPU6050::writeMPU6050(byte reg, byte data){
wire->beginTransmission(mpuAddr);
wire->write(reg);
wire->write(data);
wire->endTransmission();
}
byte MPU6050::readMPU6050(byte reg) {
wire->beginTransmission(mpuAddr);
wire->write(reg);
wire->endTransmission(true);
wire->requestFrom((uint8_t)mpuAddr, (size_t)2/*length*/);
byte data = wire->read();
wire->read();
return data;
}
void MPU6050::setGyroOffsets(float x, float y, float z){
gyroXoffset = x;
gyroYoffset = y;
gyroZoffset = z;
}
void MPU6050::calcGyroOffsets(bool console){
float x = 0, y = 0, z = 0;
int16_t rx, ry, rz;
if(console){
Serial.println("calculate gyro offsets");
Serial.print("DO NOT MOVE A MPU6050");
}
for(int i = 0; i < 5000; i++){
if(console && i % 1000 == 0){
Serial.print(".");
}
wire->beginTransmission(mpuAddr);
wire->write(0x43); // from GYRO_XOUT_H to GYRO_ZOUT_L
wire->endTransmission(false);
wire->requestFrom((int)mpuAddr, 6, (int)true);
rx = wire->read() << 8 | wire->read(); // GYRO_XOUT_H | GYRO_XOUT_L
ry = wire->read() << 8 | wire->read();
rz = wire->read() << 8 | wire->read(); // GYRO_ZOUT_H | GYRO_ZOUT_L
x += ((float)rx) / gyrosFactor; // 500: 65.5
y += ((float)ry) / gyrosFactor;
z += ((float)rz) / gyrosFactor;
}
gyroXoffset = x / 5000;
gyroYoffset = y / 5000;
gyroZoffset = z / 5000;
if(console){
Serial.println("Done!!!");
Serial.print("X : ");Serial.println(gyroXoffset);
Serial.print("Y : ");Serial.println(gyroYoffset);
Serial.print("Z : ");Serial.println(gyroYoffset);
Serial.print("Program will start after 3 seconds");
delay(3000);
}
}
void MPU6050::update(){
wire->beginTransmission(mpuAddr);
wire->write(0x3B); // from ACCEL_XOUT_H to GYRO_ZOUT_L
wire->endTransmission(false);
wire->requestFrom((int)mpuAddr, 14, (int)true);
rawAccX = wire->read() << 8 | wire->read(); // ACCEL_XOUT_H | ACCEL_XOUT_L
rawAccY = wire->read() << 8 | wire->read();
rawAccZ = wire->read() << 8 | wire->read();
rawTemp = wire->read() << 8 | wire->read();
rawGyroX = wire->read() << 8 | wire->read();
rawGyroY = wire->read() << 8 | wire->read();
rawGyroZ = wire->read() << 8 | wire->read(); // GYRO_ZOUT_H | GYRO_ZOUT_L
temp = (rawTemp + 12412.0) / 340.0;
accX = ((float)rawAccX) / accelFactor; // 2G: 16384 | 16G: 2048
accY = ((float)rawAccY) / accelFactor;
accZ = ((float)rawAccZ) / accelFactor;
angleAccX = atan2(accY, accZ + abs(accX)) * 360 / gForce / PI; // 2.0
angleAccY = atan2(accX, accZ + abs(accY)) * 360 / gForceInv / PI; // -2.0
gyroX = ((float)rawGyroX) / gyrosFactor;
gyroY = ((float)rawGyroY) / gyrosFactor;
gyroZ = ((float)rawGyroZ) / gyrosFactor;
interval = millis() - preInterval;
gyroX -= gyroXoffset;
gyroY -= gyroYoffset;
gyroZ -= gyroZoffset;
angleGyroX += gyroX * (interval * 0.001);
angleGyroY += gyroY * (interval * 0.001);
angleGyroZ += gyroZ * (interval * 0.001);
preInterval = millis();
angleX = (gyroCoef * angleGyroX) + (accCoef * angleAccX);
angleY = (gyroCoef * angleGyroY) + (accCoef * angleAccY);
angleZ = angleGyroZ;
}
Thanks!
I wanna merge your code after fix some bugs.
Could you send me a Pull Request?
in calcGyroOffsets function why did you read from 0x3B 14bytes, as I see you need only gyroscope data so you can read from 0x43 6 bytes.
In file MPU6050_tockn.cpp the first switch (accel) should be switch (gyros) .
In file MPU6050_tockn.cpp the first
switch (accel)should beswitch (gyros).
Done! Thank you!
in calcGyroOffsets function why did you read from 0x3B 14bytes, as I see you need only gyroscope data so you can read from 0x43 6 bytes.
Let's do this, according to the record map, you're correct!
Thank you!
https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
Thanks! I wanna merge your code after fix some bugs. Could you send me a Pull Request?
Sorry for the delay my friend,
I made some corrections as it was suggested,
If I have time again, I will try to learn how to do a pull request,
Thank you!
Thank you ver much fpr extending the files for using two sensors at once on I"C bus. Unfortunately a mini test program compiles perect but the upload stops near the end.
Are there any tweaks neccessary in the modified libraries or where can I get actual libraries which are tested with two sensors used?
Uups! Sorry! - The Flash Issue is resolved: AVR-Dude doesn't like "!" .... I added a few and removed them .... I try to get two sensors run in parallel. I hope it'll work as well as id does with one sensor only! - Thanky you very, very much for your library!
