rushilwiz/engine-software
1
0
Fork 0
mirror of https://github.com/PotentiaRobotics/engine-software.git synced 2025-04-21 12:30:16 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

Accounting for gravity when finding the acceleration from the IMU

Browse Source
This commit is contained in:
supreetm8 2021-07-18 16:39:44 -04:00 committed by GitHub
parent bec63d645f
commit 158b6cbe59
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 133 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
imu/imuGravityTest.ino Normal file
View File

@ -0,0 +1,133 @@
// Basic demo for accelerometer readings from Adafruit MPU6050
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
double xG = 0;
double yG = 0;
double zG = 0;
void setup(void) {
Serial.begin(115200);
while (!Serial)
delay(10); // will pause Zero, Leonardo, etc until serial console opens
Serial.println("Adafruit MPU6050 test!");
// Try to initialize!
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
Serial.println("MPU6050 Found!");
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
Serial.print("Accelerometer range set to: ");
switch (mpu.getAccelerometerRange()) {
case MPU6050_RANGE_2_G:
Serial.println("+-2G");
break;
case MPU6050_RANGE_4_G:
Serial.println("+-4G");
break;
case MPU6050_RANGE_8_G:
Serial.println("+-8G");
break;
case MPU6050_RANGE_16_G:
Serial.println("+-16G");
break;
}
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
Serial.print("Gyro range set to: ");
switch (mpu.getGyroRange()) {
case MPU6050_RANGE_250_DEG:
Serial.println("+- 250 deg/s");
break;
case MPU6050_RANGE_500_DEG:
Serial.println("+- 500 deg/s");
break;
case MPU6050_RANGE_1000_DEG:
Serial.println("+- 1000 deg/s");
break;
case MPU6050_RANGE_2000_DEG:
Serial.println("+- 2000 deg/s");
break;
}
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
Serial.print("Filter bandwidth set to: ");
switch (mpu.getFilterBandwidth()) {
case MPU6050_BAND_260_HZ:
Serial.println("260 Hz");
break;
case MPU6050_BAND_184_HZ:
Serial.println("184 Hz");
break;
case MPU6050_BAND_94_HZ:
Serial.println("94 Hz");
break;
case MPU6050_BAND_44_HZ:
Serial.println("44 Hz");
break;
case MPU6050_BAND_21_HZ:
Serial.println("21 Hz");
break;
case MPU6050_BAND_10_HZ:
Serial.println("10 Hz");
break;
case MPU6050_BAND_5_HZ:
Serial.println("5 Hz");
break;
}
Serial.println("");
delay(100);
}
void loop() {
/* Get new sensor events with the readings */
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
/* Edited part of code */
double accX = a.acceleration.x;
double accY = a.acceleration.y;
double accZ = a.acceleration.z;
xG = 0.9 * xG + 0.1 * accX;
yG = 0.9 * yG + 0.1 * accY;
zG = 0.9 * zG + 0.1 * accZ;
accX = accX - xG;
accY = accY - yG;
accZ = accZ - zG;
/* It will decrease slowly, probably can go faster
if you put a delay of 10 ms
instead of 500 ms (the last line of code) */
/* Print out the values */
Serial.print("Acceleration X: ");
Serial.print(accX);
Serial.print(", Y: ");
Serial.print(accY);
Serial.print(", Z: ");
Serial.print(accZ);
Serial.println(" m/s^2");
Serial.print("Rotation X: ");
Serial.print(g.gyro.x);
Serial.print(", Y: ");
Serial.print(g.gyro.y);
Serial.print(", Z: ");
Serial.print(g.gyro.z);
Serial.println(" rad/s");
Serial.print("Temperature: ");
Serial.print(temp.temperature);
Serial.println(" degC");
Serial.println("");
delay(500);
}