Skip to content

Commit

Permalink
Merge branch 'DJQuardaboff-master'
Browse files Browse the repository at this point in the history
  • Loading branch information
@tockn
tockn committed Mar 18, 2019
2 parents 7e5ddef + 3e517ad commit 32cfc21
Show file tree
Hide file tree
Showing 2 changed files with 134 additions and 139 deletions.
183 changes: 89 additions & 94 deletions src/MPU6050_tockn.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,100 +2,95 @@
#include "Arduino.h"

MPU6050::MPU6050(TwoWire &w){
wire = &w;
accCoef = 0.02f;
gyroCoef = 0.98f;
wire = &w;
accCoef = 0.02f;
gyroCoef = 0.98f;
}

MPU6050::MPU6050(TwoWire &w, float aC, float gC){
wire = &w;
accCoef = aC;
gyroCoef = gC;
wire = &w;
accCoef = aC;
gyroCoef = gC;
}

void MPU6050::begin(){
writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);
writeMPU6050(MPU6050_CONFIG, 0x00);
writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);
writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00);
writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);
this->update();
angleGyroX = 0;
angleGyroY = 0;
writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);
writeMPU6050(MPU6050_CONFIG, 0x00);
writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);
writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00);
writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);
this->update();
angleGyroX = 0;
angleGyroY = 0;
angleX = this->getAccAngleX();
angleY = this->getAccAngleY();
preInterval = millis();
}

void MPU6050::writeMPU6050(byte reg, byte data){
wire->beginTransmission(MPU6050_ADDR);
wire->write(reg);
wire->write(data);
wire->endTransmission();
wire->beginTransmission(MPU6050_ADDR);
wire->write(reg);
wire->write(data);
wire->endTransmission();
}

byte MPU6050::readMPU6050(byte reg) {
wire->beginTransmission(MPU6050_ADDR);
wire->write(reg);
wire->endTransmission(true);
wire->requestFrom((uint8_t)MPU6050_ADDR, (size_t)2/*length*/);
byte data = wire->read();
wire->read();
return data;
wire->beginTransmission(MPU6050_ADDR);
wire->write(reg);
wire->endTransmission(true);
wire->requestFrom(MPU6050_ADDR, 1);
byte data = wire->read();
return data;
}

void MPU6050::setGyroOffsets(float x, float y, float z){
gyroXoffset = x;
gyroYoffset = y;
gyroZoffset = z;
gyroXoffset = x;
gyroYoffset = y;
gyroZoffset = z;
}

void MPU6050::calcGyroOffsets(bool console){
float x = 0, y = 0, z = 0;
int16_t rx, ry, rz;
float x = 0, y = 0, z = 0;
int16_t rx, ry, rz;

delay(1000);
if(console){
if(console){
Serial.println();
Serial.println("========================================");
Serial.println("calculate gyro offsets");
Serial.print("DO NOT MOVE A MPU6050");
}
for(int i = 0; i < 3000; i++){
if(console && i % 1000 == 0){
Serial.print(".");
}
wire->beginTransmission(MPU6050_ADDR);
wire->write(0x3B);
wire->endTransmission(false);
wire->requestFrom((int)MPU6050_ADDR, 14);

wire->read() << 8 | wire->read();
wire->read() << 8 | wire->read();
wire->read() << 8 | wire->read();
wire->read() << 8 | wire->read();
rx = wire->read() << 8 | wire->read();
ry = wire->read() << 8 | wire->read();
rz = wire->read() << 8 | wire->read();

x += ((float)rx) / 65.5;
y += ((float)ry) / 65.5;
z += ((float)rz) / 65.5;
}
gyroXoffset = x / 3000;
gyroYoffset = y / 3000;
gyroZoffset = z / 3000;

if(console){
Serial.println("Calculating gyro offsets");
Serial.print("DO NOT MOVE MPU6050");
}
for(int i = 0; i < 3000; i++){
if(console && i % 1000 == 0){
Serial.print(".");
}
wire->beginTransmission(MPU6050_ADDR);
wire->write(0x43);
wire->endTransmission(false);
wire->requestFrom((int)MPU6050_ADDR, 6, (int)true);

rx = wire->read() << 8 | wire->read();
ry = wire->read() << 8 | wire->read();
rz = wire->read() << 8 | wire->read();

x += ((float)rx) / 65.5;
y += ((float)ry) / 65.5;
z += ((float)rz) / 65.5;
}
gyroXoffset = x / 3000;
gyroYoffset = y / 3000;
gyroZoffset = z / 3000;

if(console){
Serial.println();
Serial.println("Done!!!");
Serial.print("X : ");Serial.println(gyroXoffset);
Serial.print("Y : ");Serial.println(gyroYoffset);
Serial.print("Z : ");Serial.println(gyroZoffset);
Serial.println("Program will start after 3 seconds");
Serial.println("Done!!!");
Serial.print("X : ");Serial.println(gyroXoffset);
Serial.print("Y : ");Serial.println(gyroYoffset);
Serial.print("Z : ");Serial.println(gyroZoffset);
Serial.println("Program will start after 3 seconds");
Serial.print("========================================");
delay(3000);
}
delay(3000);
}
}

void MPU6050::update(){
Expand All @@ -104,42 +99,42 @@ void MPU6050::update(){
wire->endTransmission(false);
wire->requestFrom((int)MPU6050_ADDR, 14);

rawAccX = wire->read() << 8 | wire->read();
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();
rawAccX = wire->read() << 8 | wire->read();
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();

temp = (rawTemp + 12412.0) / 340.0;
temp = (rawTemp + 12412.0) / 340.0;

accX = ((float)rawAccX) / 16384.0;
accY = ((float)rawAccY) / 16384.0;
accZ = ((float)rawAccZ) / 16384.0;
accX = ((float)rawAccX) / 16384.0;
accY = ((float)rawAccY) / 16384.0;
accZ = ((float)rawAccZ) / 16384.0;

angleAccX = atan2(accY, accZ + abs(accX)) * 360 / 2.0 / PI;
angleAccY = atan2(accX, accZ + abs(accY)) * 360 / -2.0 / PI;
angleAccX = atan2(accY, accZ + abs(accX)) * 360 / 2.0 / PI;
angleAccY = atan2(accX, accZ + abs(accY)) * 360 / -2.0 / PI;

gyroX = ((float)rawGyroX) / 65.5;
gyroY = ((float)rawGyroY) / 65.5;
gyroZ = ((float)rawGyroZ) / 65.5;
gyroX = ((float)rawGyroX) / 65.5;
gyroY = ((float)rawGyroY) / 65.5;
gyroZ = ((float)rawGyroZ) / 65.5;

gyroX -= gyroXoffset;
gyroY -= gyroYoffset;
gyroZ -= gyroZoffset;
gyroX -= gyroXoffset;
gyroY -= gyroYoffset;
gyroZ -= gyroZoffset;

interval = (millis() - preInterval) * 0.001;
interval = (millis() - preInterval) * 0.001;

angleGyroX += gyroX * interval;
angleGyroY += gyroY * interval;
angleGyroZ += gyroZ * interval;
angleGyroX += gyroX * interval;
angleGyroY += gyroY * interval;
angleGyroZ += gyroZ * interval;

angleX = (gyroCoef * (angleX + gyroX * interval)) + (accCoef * angleAccX);
angleY = (gyroCoef * (angleY + gyroY * interval)) + (accCoef * angleAccY);
angleZ = angleGyroZ;
angleX = (gyroCoef * (angleX + gyroX * interval)) + (accCoef * angleAccX);
angleY = (gyroCoef * (angleY + gyroY * interval)) + (accCoef * angleAccY);
angleZ = angleGyroZ;

preInterval = millis();
preInterval = millis();

}

90 changes: 45 additions & 45 deletions src/MPU6050_tockn.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,77 +15,77 @@
#define MPU6050_TEMP_L 0x42

class MPU6050{
public:
public:

MPU6050(TwoWire &w);
MPU6050(TwoWire &w, float aC, float gC);
MPU6050(TwoWire &w);
MPU6050(TwoWire &w, float aC, float gC);

void begin();
void begin();

void setGyroOffsets(float x, float y, float z);
void setGyroOffsets(float x, float y, float z);

void writeMPU6050(byte reg, byte data);
byte readMPU6050(byte reg);
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 getRawAccX(){ return rawAccX; };
int16_t getRawAccY(){ return rawAccY; };
int16_t getRawAccZ(){ return rawAccZ; };

int16_t getRawTemp(){ return rawTemp; };
int16_t getRawTemp(){ return rawTemp; };

int16_t getRawGyroX(){ return rawGyroX; };
int16_t getRawGyroY(){ return rawGyroY; };
int16_t getRawGyroZ(){ return rawGyroZ; };
int16_t getRawGyroX(){ return rawGyroX; };
int16_t getRawGyroY(){ return rawGyroY; };
int16_t getRawGyroZ(){ return rawGyroZ; };

float getTemp(){ return temp; };
float getTemp(){ return temp; };

float getAccX(){ return accX; };
float getAccY(){ return accY; };
float getAccZ(){ return accZ; };
float getAccX(){ return accX; };
float getAccY(){ return accY; };
float getAccZ(){ return accZ; };

float getGyroX(){ return gyroX; };
float getGyroY(){ return gyroY; };
float getGyroZ(){ return gyroZ; };
float getGyroX(){ return gyroX; };
float getGyroY(){ return gyroY; };
float getGyroZ(){ return gyroZ; };

void calcGyroOffsets(bool console = false);
void calcGyroOffsets(bool console = false);

float getGyroXoffset(){ return gyroXoffset; };
float getGyroYoffset(){ return gyroYoffset; };
float getGyroZoffset(){ return gyroZoffset; };
float getGyroXoffset(){ return gyroXoffset; };
float getGyroYoffset(){ return gyroYoffset; };
float getGyroZoffset(){ return gyroZoffset; };

void update();
void update();

float getAccAngleX(){ return angleAccX; };
float getAccAngleY(){ return angleAccY; };
float getAccAngleX(){ return angleAccX; };
float getAccAngleY(){ return angleAccY; };

float getGyroAngleX(){ return angleGyroX; };
float getGyroAngleY(){ return angleGyroY; };
float getGyroAngleZ(){ return angleGyroZ; };
float getGyroAngleX(){ return angleGyroX; };
float getGyroAngleY(){ return angleGyroY; };
float getGyroAngleZ(){ return angleGyroZ; };

float getAngleX(){ return angleX; };
float getAngleY(){ return angleY; };
float getAngleZ(){ return angleZ; };
float getAngleX(){ return angleX; };
float getAngleY(){ return angleY; };
float getAngleZ(){ return angleZ; };

private:
private:

TwoWire *wire;
TwoWire *wire;

int16_t rawAccX, rawAccY, rawAccZ, rawTemp,
rawGyroX, rawGyroY, rawGyroZ;
int16_t rawAccX, rawAccY, rawAccZ, rawTemp,
rawGyroX, rawGyroY, rawGyroZ;

float gyroXoffset, gyroYoffset, gyroZoffset;
float gyroXoffset, gyroYoffset, gyroZoffset;

float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;

float angleGyroX, angleGyroY, angleGyroZ,
angleAccX, angleAccY, angleAccZ;
float angleGyroX, angleGyroY, angleGyroZ,
angleAccX, angleAccY, angleAccZ;

float angleX, angleY, angleZ;
float angleX, angleY, angleZ;

float interval;
long preInterval;
long preInterval;

float accCoef, gyroCoef;
float accCoef, gyroCoef;
};

#endif

0 comments on commit 32cfc21

Please sign in to comment.