初始化提交
This commit is contained in:
王立帮
139
arduino-cli/libraries/MPU6050_tockn/MPU6050_tockn.cpp
Normal file
139
arduino-cli/libraries/MPU6050_tockn/MPU6050_tockn.cpp
Normal file
@@ -0,0 +1,139 @@
|
||||
#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(){
|
||||
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();
|
||||
}
|
||||
|
||||
byte MPU6050::readMPU6050(byte reg) {
|
||||
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;
|
||||
}
|
||||
|
||||
void MPU6050::calcGyroOffsets(bool console, uint16_t delayBefore, uint16_t delayAfter){
|
||||
float x = 0, y = 0, z = 0;
|
||||
int16_t rx, ry, rz;
|
||||
|
||||
delay(delayBefore);
|
||||
if(console){
|
||||
Serial.println();
|
||||
Serial.println("========================================");
|
||||
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);
|
||||
|
||||
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.print("========================================");
|
||||
delay(delayAfter);
|
||||
}
|
||||
}
|
||||
|
||||
void MPU6050::update(){
|
||||
wire->beginTransmission(MPU6050_ADDR);
|
||||
wire->write(0x3B);
|
||||
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();
|
||||
|
||||
temp = (rawTemp + 12412.0) / 340.0;
|
||||
|
||||
accX = ((float)rawAccX) / 16384.0;
|
||||
accY = ((float)rawAccY) / 16384.0;
|
||||
accZ = ((float)rawAccZ) / 16384.0;
|
||||
|
||||
angleAccX = atan2(accY, sqrt(accZ * accZ + accX * accX)) * 360 / 2.0 / PI;
|
||||
angleAccY = atan2(accX, sqrt(accZ * accZ + accY * accY)) * 360 / -2.0 / PI;
|
||||
|
||||
gyroX = ((float)rawGyroX) / 65.5;
|
||||
gyroY = ((float)rawGyroY) / 65.5;
|
||||
gyroZ = ((float)rawGyroZ) / 65.5;
|
||||
|
||||
gyroX -= gyroXoffset;
|
||||
gyroY -= gyroYoffset;
|
||||
gyroZ -= gyroZoffset;
|
||||
|
||||
interval = (millis() - preInterval) * 0.001;
|
||||
|
||||
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;
|
||||
|
||||
preInterval = millis();
|
||||
|
||||
}
|
||||
92
arduino-cli/libraries/MPU6050_tockn/MPU6050_tockn.h
Normal file
92
arduino-cli/libraries/MPU6050_tockn/MPU6050_tockn.h
Normal file
@@ -0,0 +1,92 @@
|
||||
#ifndef MPU6050_TOCKN_H
|
||||
#define MPU6050_TOCKN_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Wire.h"
|
||||
|
||||
#define MPU6050_ADDR 0x68
|
||||
#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
|
||||
|
||||
class MPU6050{
|
||||
public:
|
||||
|
||||
MPU6050(TwoWire &w);
|
||||
MPU6050(TwoWire &w, float aC, float gC);
|
||||
|
||||
void begin();
|
||||
|
||||
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, uint16_t delayBefore = 1000, uint16_t delayAfter = 3000);
|
||||
|
||||
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; };
|
||||
void resetAngle(){angleGyroX=0;angleGyroY=0;angleGyroZ=0;}
|
||||
|
||||
private:
|
||||
|
||||
TwoWire *wire;
|
||||
|
||||
int16_t rawAccX, rawAccY, rawAccZ, rawTemp,
|
||||
rawGyroX, rawGyroY, rawGyroZ;
|
||||
|
||||
float gyroXoffset, gyroYoffset, gyroZoffset;
|
||||
|
||||
float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
|
||||
|
||||
float angleGyroX, angleGyroY, angleGyroZ,
|
||||
angleAccX, angleAccY, angleAccZ;
|
||||
|
||||
float angleX, angleY, angleZ;
|
||||
|
||||
float interval;
|
||||
long preInterval;
|
||||
|
||||
float accCoef, gyroCoef;
|
||||
};
|
||||
|
||||
#endif
|
||||
80
arduino-cli/libraries/MPU6050_tockn/README.md
Normal file
80
arduino-cli/libraries/MPU6050_tockn/README.md
Normal file
@@ -0,0 +1,80 @@
|
||||
# MPU6050_tockn
|
||||
Arduino library for easy communicating with the MPU6050
|
||||
## Usage
|
||||
You can see [example sketch.](https://github.com/Tockn/MPU6050_tockn/tree/master/examples)
|
||||
|
||||
If you want to get data of MPU6050, you must execute `update()` method before get method.
|
||||
`update()` will get all data of MPU6050, and calculating angle by accelerometer, gyroscope and complementary filter.
|
||||
|
||||
### Complementary filter
|
||||
`update()` method calculate angle by accelerometer and gyroscope using complementary filter.
|
||||
Those two coefficients determined by constructor.
|
||||
Default coefficient of accelerometer is 0.02, gyroscope is 0.98.
|
||||
`filtered_angle = (0.02 * accel) + (0.98 * gyro)`
|
||||
#### example
|
||||
If you want to set 0.1 to accelerometer coefficient and 0.9 to gyroscope coefficient, your code is
|
||||
```MPU6050 mpu6050(Wire, 0.1, 0.9);```
|
||||
|
||||
|
||||
### Auto calibration
|
||||
If you use `calcGyroOffsets()` in `setup()`, it will calculate calibration of the gyroscope, and the value of the gyroscope will calibrated.
|
||||
⚠DO NOT MOVE MPU6050 during calculating.⚠
|
||||
```
|
||||
#include <MPU6050_tockn>
|
||||
#include <Wire.h>
|
||||
|
||||
MPU6050 mpu6050(Wire);
|
||||
|
||||
void setup(){
|
||||
Wire.begin();
|
||||
mpu6050.begin();
|
||||
mpu6050.calcGyroOffsets();
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
If you use `calcGyroOffsets(true)` in `setup()`, you can see state of calculating calibration in serial monitor.
|
||||
```
|
||||
#include <MPU6050_tockn>
|
||||
#include <Wire.h>
|
||||
|
||||
MPU6050 mpu6050(Wire);
|
||||
|
||||
void setup(){
|
||||
Serial.begin(9600);
|
||||
Wire.begin();
|
||||
mpu6050.begin();
|
||||
mpu6050.calcGyroOffsets(true);
|
||||
}
|
||||
```
|
||||
Serial monitor:
|
||||
```
|
||||
Calculate gyro offsets
|
||||
DO NOT MOVE MPU6050.....
|
||||
Done!
|
||||
X : 1.45
|
||||
Y : 1.23
|
||||
Z : -1.32
|
||||
Program will start after 3 seconds
|
||||
```
|
||||
|
||||
If you know offsets of gyroscope, you can set them by `setGyroOffsets()`, and you don't have to execute `calcGyroOffsets()`, so you can launch program quickly.
|
||||
#### example
|
||||
```
|
||||
#include <MPU6050_tockn>
|
||||
#include <Wire.h>
|
||||
|
||||
MPU6050 mpu6050(Wire);
|
||||
|
||||
void setup(){
|
||||
Serial.begin(9600);
|
||||
Wire.begin();
|
||||
mpu6050.begin();
|
||||
mpu6050.setGyroOffsets(1.45, 1.23, -1.32);
|
||||
}
|
||||
```
|
||||
## Licence
|
||||
MIT
|
||||
## Author
|
||||
|
||||
[tockn](https://github.com/tockn)
|
||||
@@ -0,0 +1,46 @@
|
||||
|
||||
#include <MPU6050_tockn.h>
|
||||
#include <Wire.h>
|
||||
|
||||
MPU6050 mpu6050(Wire);
|
||||
|
||||
long timer = 0;
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
Wire.begin();
|
||||
mpu6050.begin();
|
||||
mpu6050.calcGyroOffsets(true);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
mpu6050.update();
|
||||
|
||||
if(millis() - timer > 1000){
|
||||
|
||||
Serial.println("=======================================================");
|
||||
Serial.print("temp : ");Serial.println(mpu6050.getTemp());
|
||||
Serial.print("accX : ");Serial.print(mpu6050.getAccX());
|
||||
Serial.print("\taccY : ");Serial.print(mpu6050.getAccY());
|
||||
Serial.print("\taccZ : ");Serial.println(mpu6050.getAccZ());
|
||||
|
||||
Serial.print("gyroX : ");Serial.print(mpu6050.getGyroX());
|
||||
Serial.print("\tgyroY : ");Serial.print(mpu6050.getGyroY());
|
||||
Serial.print("\tgyroZ : ");Serial.println(mpu6050.getGyroZ());
|
||||
|
||||
Serial.print("accAngleX : ");Serial.print(mpu6050.getAccAngleX());
|
||||
Serial.print("\taccAngleY : ");Serial.println(mpu6050.getAccAngleY());
|
||||
|
||||
Serial.print("gyroAngleX : ");Serial.print(mpu6050.getGyroAngleX());
|
||||
Serial.print("\tgyroAngleY : ");Serial.print(mpu6050.getGyroAngleY());
|
||||
Serial.print("\tgyroAngleZ : ");Serial.println(mpu6050.getGyroAngleZ());
|
||||
|
||||
Serial.print("angleX : ");Serial.print(mpu6050.getAngleX());
|
||||
Serial.print("\tangleY : ");Serial.print(mpu6050.getAngleY());
|
||||
Serial.print("\tangleZ : ");Serial.println(mpu6050.getAngleZ());
|
||||
Serial.println("=======================================================\n");
|
||||
timer = millis();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
|
||||
#include <MPU6050_tockn.h>
|
||||
#include <Wire.h>
|
||||
|
||||
MPU6050 mpu6050(Wire);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
Wire.begin();
|
||||
mpu6050.begin();
|
||||
mpu6050.calcGyroOffsets(true);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
mpu6050.update();
|
||||
Serial.print("angleX : ");
|
||||
Serial.print(mpu6050.getAngleX());
|
||||
Serial.print("\tangleY : ");
|
||||
Serial.print(mpu6050.getAngleY());
|
||||
Serial.print("\tangleZ : ");
|
||||
Serial.println(mpu6050.getAngleZ());
|
||||
}
|
||||
9
arduino-cli/libraries/MPU6050_tockn/library.properties
Normal file
9
arduino-cli/libraries/MPU6050_tockn/library.properties
Normal file
@@ -0,0 +1,9 @@
|
||||
name=MPU6050_tockn
|
||||
version=1.5.2
|
||||
author=tockn
|
||||
maintainer=tockn
|
||||
sentence=Arduino library for easy communicating with the MPU6050.
|
||||
paragraph=It can get accel, gyro, and angle data.
|
||||
category=Sensors
|
||||
url=https://github.com/Tockn/MPU6050_tockn
|
||||
architectures=*
|
||||
139
arduino-cli/libraries/MPU6050_tockn/src/MPU6050_tockn.cpp
Normal file
139
arduino-cli/libraries/MPU6050_tockn/src/MPU6050_tockn.cpp
Normal file
@@ -0,0 +1,139 @@
|
||||
#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(){
|
||||
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();
|
||||
}
|
||||
|
||||
byte MPU6050::readMPU6050(byte reg) {
|
||||
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;
|
||||
}
|
||||
|
||||
void MPU6050::calcGyroOffsets(bool console, uint16_t delayBefore, uint16_t delayAfter){
|
||||
float x = 0, y = 0, z = 0;
|
||||
int16_t rx, ry, rz;
|
||||
|
||||
delay(delayBefore);
|
||||
if(console){
|
||||
Serial.println();
|
||||
Serial.println("========================================");
|
||||
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);
|
||||
|
||||
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.print("========================================");
|
||||
delay(delayAfter);
|
||||
}
|
||||
}
|
||||
|
||||
void MPU6050::update(){
|
||||
wire->beginTransmission(MPU6050_ADDR);
|
||||
wire->write(0x3B);
|
||||
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();
|
||||
|
||||
temp = (rawTemp + 12412.0) / 340.0;
|
||||
|
||||
accX = ((float)rawAccX) / 16384.0;
|
||||
accY = ((float)rawAccY) / 16384.0;
|
||||
accZ = ((float)rawAccZ) / 16384.0;
|
||||
|
||||
angleAccX = atan2(accY, sqrt(accZ * accZ + accX * accX)) * 360 / 2.0 / PI;
|
||||
angleAccY = atan2(accX, sqrt(accZ * accZ + accY * accY)) * 360 / -2.0 / PI;
|
||||
|
||||
gyroX = ((float)rawGyroX) / 65.5;
|
||||
gyroY = ((float)rawGyroY) / 65.5;
|
||||
gyroZ = ((float)rawGyroZ) / 65.5;
|
||||
|
||||
gyroX -= gyroXoffset;
|
||||
gyroY -= gyroYoffset;
|
||||
gyroZ -= gyroZoffset;
|
||||
|
||||
interval = (millis() - preInterval) * 0.001;
|
||||
|
||||
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;
|
||||
|
||||
preInterval = millis();
|
||||
|
||||
}
|
||||
92
arduino-cli/libraries/MPU6050_tockn/src/MPU6050_tockn.h
Normal file
92
arduino-cli/libraries/MPU6050_tockn/src/MPU6050_tockn.h
Normal file
@@ -0,0 +1,92 @@
|
||||
#ifndef MPU6050_TOCKN_H
|
||||
#define MPU6050_TOCKN_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Wire.h"
|
||||
|
||||
#define MPU6050_ADDR 0x68
|
||||
#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
|
||||
|
||||
class MPU6050{
|
||||
public:
|
||||
|
||||
MPU6050(TwoWire &w);
|
||||
MPU6050(TwoWire &w, float aC, float gC);
|
||||
|
||||
void begin();
|
||||
|
||||
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, uint16_t delayBefore = 1000, uint16_t delayAfter = 3000);
|
||||
|
||||
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; };
|
||||
void resetAngle(){angleGyroX=0;angleGyroY=0;angleGyroZ=0;}
|
||||
|
||||
private:
|
||||
|
||||
TwoWire *wire;
|
||||
|
||||
int16_t rawAccX, rawAccY, rawAccZ, rawTemp,
|
||||
rawGyroX, rawGyroY, rawGyroZ;
|
||||
|
||||
float gyroXoffset, gyroYoffset, gyroZoffset;
|
||||
|
||||
float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
|
||||
|
||||
float angleGyroX, angleGyroY, angleGyroZ,
|
||||
angleAccX, angleAccY, angleAccZ;
|
||||
|
||||
float angleX, angleY, angleZ;
|
||||
|
||||
float interval;
|
||||
long preInterval;
|
||||
|
||||
float accCoef, gyroCoef;
|
||||
};
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user