// Bellatrix Gracia - 21/481327/TK/53129 - Teknik Biomedis
// Brahmantio Farhan Rabbani - 21/482806/TK/53375 - Teknik Elektro
// Muhammad Ikhsan Faturrahman Wiguna- 21/480831/TK/53068 - Teknik Elektro
// Sefvia Lie - 21/478184/TK/52695 - Teknik Biomedis
// -------------------------------------------------------------------
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
float AccX, AccY, AccZ;
float speedX, speedY, speedZ;
float distanceX, distanceY, distanceZ;
float previousTime, currentTime, elapsedTime;
float angularVelocityX, angularVelocityY, angularVelocityZ;
float angleRoll, anglePitch, angleYaw;
Adafruit_MPU6050 mpu;
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_2000_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);
// percepatan linear dari arah gerak
AccX = a.acceleration.x;
AccY = a.acceleration.y;
AccZ = a.acceleration.z;
// perpecatan sudut
angularVelocityX = g.gyro.x;
angularVelocityY = g.gyro.y;
angularVelocityZ = g.gyro.z;
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
// kecepatan linear GLBB: v = v + at
speedX = speedX + AccX * elapsedTime;
speedY = speedY + AccY * elapsedTime;
speedZ = speedZ + AccZ * elapsedTime;
// perpindahan linear GLBB: x = x + vt
distanceX = distanceX + speedX * elapsedTime;
distanceY = distanceY + speedY * elapsedTime;
// sudut Gerak Melingkar Berubah Beraturan: teta = teta + wt
angleRoll = (angleRoll + angularVelocityX * elapsedTime) * 180/PI;
anglePitch = (anglePitch + angularVelocityY * elapsedTime) * 180/PI;
angleYaw = (angleYaw + angularVelocityZ * elapsedTime) * 180/PI;
/* 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("Velocity X: ");
Serial.print(speedX);
Serial.print(", Y: ");
Serial.print(speedY);
Serial.println(" m/s");
Serial.print("Distance X: ");
Serial.print(distanceX);
Serial.print(", Y: ");
Serial.print(distanceY);
Serial.println(" m/s");
Serial.print("Angular Velocity X: ");
Serial.print(angularVelocityX);
Serial.print(", Y: ");
Serial.print(angularVelocityY);
Serial.print(", Z: ");
Serial.print(angularVelocityZ);
Serial.println(" rad/s");
Serial.print("Angle Roll: ");
Serial.print(angleRoll);
Serial.print(", Pitch: ");
Serial.print(anglePitch);
Serial.print(", Yaw: ");
Serial.print(angleYaw);
Serial.println(" theta");
Serial.print("Wahana : ");
if(angleRoll == 0)
Serial.println(" jalan rata");
else if(angleRoll < 0)
Serial.println(" miring kiri");
else if (angleRoll > 0)
Serial.println(" miring kanan");
Serial.print("Posisi robot : ");
if(angleYaw == 0)
Serial.println(" jalan lurus");
else if(angleYaw < 0)
Serial.println(" serong kiri");
else if (angleYaw > 0)
Serial.println(" serong kanan");
Serial.print("Temperature: ");
Serial.print(temp.temperature);
Serial.println(" degC\n");
Serial.println("");
delay(1000);
}