#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
int motorPins[] = {13, 14, 27, 12};
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
for (int i = 0; i < 4; i++) {
pinMode(motorPins[i], OUTPUT);
}
if (!mpu.testConnection()) {
Serial.println("MPU6050 tidak terhubung!");
while (1);
}
}
void loop() {
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float pitch = atan2(ay, az) * 180 / PI;
float roll = atan2(-ax, az) * 180 / PI;
int motorSpeed1 = constrain(map(pitch - roll, -180, 180, 0, 255), 0, 255); // Kiri depan
int motorSpeed2 = constrain(map(pitch + roll, -180, 180, 0, 255), 0, 255); // Kanan depan
int motorSpeed3 = constrain(map(-pitch - roll, -180, 180, 0, 255), 0, 255); // Kiri belakang
int motorSpeed4 = constrain(map(-pitch + roll, -180, 180, 0, 255), 0, 255); // Kanan belakang
analogWrite(motorPins[0], motorSpeed1);
analogWrite(motorPins[1], motorSpeed2);
analogWrite(motorPins[2], motorSpeed3);
analogWrite(motorPins[3], motorSpeed4);
Serial.print("Pitch: "); Serial.print(pitch);
Serial.print(" Roll: "); Serial.print(roll);
Serial.print(" | Motor Speeds: ");
Serial.print(motorSpeed1); Serial.print(" ");
Serial.print(motorSpeed2); Serial.print(" ");
Serial.print(motorSpeed3); Serial.print(" ");
Serial.print(motorSpeed4);
if (motorSpeed1 > motorSpeed3 && motorSpeed2 > motorSpeed4 && motorSpeed1 > 50 && motorSpeed2 > 50) {
Serial.println(" | Miring ke Depan");
} else if (motorSpeed3 > motorSpeed1 && motorSpeed4 > motorSpeed2 && motorSpeed3 > 50 && motorSpeed4 > 50) {
Serial.println(" | Miring ke Belakang");
} else if (motorSpeed1 > motorSpeed2 && motorSpeed3 > motorSpeed4 && motorSpeed1 > 50 && motorSpeed3 > 50) {
Serial.println(" | Miring ke Kiri");
} else if (motorSpeed2 > motorSpeed1 && motorSpeed4 > motorSpeed3 && motorSpeed2 > 50 && motorSpeed4 > 50) {
Serial.println(" | Miring ke Kanan");
} else {
Serial.println(" | Melayang Stabil (Hover)");
}
delay(100);
}
// Kondisi "Miring ke Depan": Jika motor depan (motorSpeed1 dan motorSpeed2) lebih cepat dari motor belakang (motorSpeed3 dan motorSpeed4), itu berarti drone condong ke depan.
// Kondisi "Miring ke Belakang": Jika motor belakang lebih cepat dari motor depan, berarti drone condong ke belakang.
// Kondisi "Miring ke Kiri": Jika motor kiri (motorSpeed1 dan motorSpeed3) lebih cepat dari motor kanan (motorSpeed2 dan motorSpeed4), berarti drone condong ke kiri.
// Kondisi "Miring ke Kanan": Jika motor kanan lebih cepat dari motor kiri, berarti drone condong ke kanan.
// Kondisi "Melayang Stabil": Jika semua motor berada pada kecepatan rendah (di bawah threshold yang ditentukan), drone dianggap stabil.
// Threshold Kecepatan
// Dalam kondisi penentuan arah, terdapat threshold kecepatan motor (misalnya, > 50)
// untuk memastikan bahwa perubahan arah benar-benar signifikan. Ini membantu mencegah
// pembacaan palsu yang mungkin disebabkan oleh fluktuasi kecil dalam data sensor atau kecepatan motor.