Ukur Sudut MPU9250 Arduino Kode Program Dasar

Sensor inersia MPU9250 adalah salah satu komponen elektronik yang populer digunakan dalam berbagai proyek yang melibatkan pengukuran gerakan dan orientasi. Dalam artikel ini, kita akan membahas cara ukur sudut menggunakan sensor MPU9250 dengan bantuan Arduino. Kami akan memandu Anda melalui kode program dasar dan penjelasan singkat tentang setiap langkahnya.

Sensor MPU9250 adalah sensor inersia 9-DOF (Degrees of Freedom) yang mengintegrasikan akselerometer, giroskop, dan magnetometer. Dalam proyek ini, kita akan menggunakan data dari giroskop dan akselerometer untuk mengukur sudut roll dan pitch dari perangkat. Roll mengukur kemiringan sepanjang sumbu X, sementara pitch mengukur kemiringan sepanjang sumbu Y.



Persiapan

Sebelum kita mulai, pastikan Anda memiliki komponen berikut:

  1. Arduino board (contoh: Arduino Uno)
  2. Sensor MPU9250
  3. Kabel jumper
  4. Komputer dengan Arduino IDE terinstal


Instalasi Library

Pertama, pastikan Anda telah menginstal library yang diperlukan untuk komunikasi dengan sensor MPU9250. Anda dapat mengunduh library “MPU9250” dari Library Manager di Arduino IDE.



Sambungkan Perangkat

Sambungkan sensor MPU9250 ke Arduino menggunakan kabel jumper. Hubungkan kabel VCC ke pin 3.3V, kabel GND ke pin GND, kabel SDA ke pin A4, dan kabel SCL ke pin A5 pada Arduino.



Kode Program Tipe Pertama

Buka Arduino IDE dan buat sketch baru. Salin kode program di bawah ini ke dalam sketch:

#include "quaternionFilters.h"
#include "MPU9250.h"

#define SerialDebug true
#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0
//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1

MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);

float AngleRoll, AnglePitch;
float RateRoll, RatePitch;
float RateCalibrationRoll, RateCalibrationPitch;
float KalmanAngleRoll = 0, KalmanUncertaintyAngleRoll = 2 * 2;
float KalmanAnglePitch = 0, KalmanUncertaintyAnglePitch = 2 * 2;
float Kalman1DOutput[] = {0, 0};
uint32_t LoopTimer;

void setup() {
  Wire.begin();
  Serial.begin(115200);

  while (!Serial) {};
  byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);

  if (c == 0x71)
  {
    Serial.println(F("MPU9250 is online..."));

    myIMU.MPU9250SelfTest(myIMU.selfTest);
    myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
    myIMU.initMPU9250();
    myIMU.getAres();
    myIMU.getGres();

  } else {
    Serial.print("Could not connect to MPU9250: 0x");
    Serial.println(c, HEX);
    Serial.println(F("Communication failed, abort!"));
    Serial.flush();
    abort();
  }
}

void loop() {
  gyro_signals();
  RateRoll -= RateCalibrationRoll;
  RatePitch -= RateCalibrationPitch;
  kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
  KalmanAngleRoll = Kalman1DOutput[0];
  KalmanUncertaintyAngleRoll = Kalman1DOutput[1];
  kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
  KalmanAnglePitch = Kalman1DOutput[0];
  KalmanUncertaintyAnglePitch = Kalman1DOutput[1];

  Serial.print("Roll Angle [°] ");
  Serial.print(KalmanAngleRoll);
  Serial.print(" Pitch Angle [°] ");
  Serial.println(KalmanAnglePitch);
  while (micros() - LoopTimer < 4000);
  LoopTimer = micros();
}

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {
  KalmanState = KalmanState + 0.004 * KalmanInput;
  KalmanUncertainty = KalmanUncertainty + 0.004 * 0.004 * 4 * 4;
  float KalmanGain = KalmanUncertainty * 1 / (1 * KalmanUncertainty + 3 * 3);
  KalmanState = KalmanState + KalmanGain * (KalmanMeasurement - KalmanState);
  KalmanUncertainty = (1 - KalmanGain) * KalmanUncertainty;
  Kalman1DOutput[0] = KalmanState;
  Kalman1DOutput[1] = KalmanUncertainty;
}

void gyro_signals() {

  myIMU.readAccelData(myIMU.accelCount);  // Read the x/y/z adc values

  myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;  // - myIMU.accelBias[0];
  myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;  // - myIMU.accelBias[1];
  myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;  // - myIMU.accelBias[2];

  myIMU.readGyroData(myIMU.gyroCount);  // Read the x/y/z adc values

  myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
  myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
  myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;

  myIMU.updateTime();

  MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
                         myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
                         myIMU.mx, myIMU.mz, myIMU.deltat);


  RateRoll = myIMU.gx / 65;
  RatePitch = myIMU.gy / 65;

  AngleRoll = atan(myIMU.ay / sqrt(myIMU.ax * myIMU.ax + myIMU.az * myIMU.az)) * 1 / (3.142 / 180);
  AnglePitch = -atan(myIMU.ax / sqrt(myIMU.ay * myIMU.ay + myIMU.az * myIMU.az)) * 1 / (3.142 / 180);

  Serial.println();
}


Kode Program Tipe Kedua

Kode program kedua ini lebih responsife dan cepat.

#include "quaternionFilters.h"
#include "MPU9250.h"

#define SerialDebug true
#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0
//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1

MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);


float KalmanStateRoll = 0.0;
float KalmanGainRoll = 0.05;
float KalmanStatePitch = 0.0;
float KalmanGainPitch = 0.05;

float gyroRateRoll = 0.0;
float gyroRatePitch = 0.0;
float accelAngleRoll = 0.0;
float accelAnglePitch = 0.0;
float dt = 0.01;  // Time interval (adjust as needed)


void setup() {
  Wire.begin();
  Serial.begin(115200);

  while (!Serial) {};
  byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);

  if (c == 0x71) {
    Serial.println(F("MPU9250 is online..."));

    myIMU.MPU9250SelfTest(myIMU.selfTest);
    myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
    myIMU.initMPU9250();
    myIMU.getAres();
    myIMU.getGres();

  } else {
    Serial.print("Could not connect to MPU9250: 0x");
    Serial.println(c, HEX);
    Serial.println(F("Communication failed, abort!"));
    Serial.flush();
    abort();
  }

  KalmanStateRoll = 0.0;
  KalmanStatePitch = 0.0;
}

void loop() {
  // Read sensor data
  readAccel();  // Read accelerometer data
  readGyro();   // Read gyro rate data

  // Calculate predicted state using rate of change (gyroscope)
  KalmanStateRoll += gyroRateRoll * dt;
  KalmanStatePitch += gyroRatePitch * dt;

  // Update Kalman Gain based on rotation rate
  KalmanGainRoll = KalmanGainRoll / (KalmanGainRoll + gyroRateRoll * dt);
  KalmanGainPitch = KalmanGainPitch / (KalmanGainPitch + gyroRatePitch * dt);

  // Calculate Kalman-filtered angles
  float KalmanAngleRoll = KalmanStateRoll + KalmanGainRoll * (accelAngleRoll - KalmanStateRoll);
  float KalmanAnglePitch = KalmanStatePitch + KalmanGainPitch * (accelAnglePitch - KalmanStatePitch);

  // Print the filtered angles
  Serial.print("Roll Angle: ");
  Serial.print(KalmanAngleRoll);
  Serial.print("°, Pitch Angle: ");
  Serial.print(KalmanAnglePitch);
  Serial.println("°");

  delay(dt * 1000);  // Delay for the time interval
}

void readAccel() {
  int16_t accelCount[3];
  myIMU.readAccelData(accelCount);

  accelAngleRoll = atan((float)accelCount[1] / sqrt(pow((float)accelCount[0], 2) + pow((float)accelCount[2], 2))) * 180.0 / PI;
  accelAnglePitch = atan(-1 * (float)accelCount[0] / sqrt(pow((float)accelCount[1], 2) + pow((float)accelCount[2], 2))) * 180.0 / PI;
}

void readGyro() {
  int16_t gyroCount[3];
  myIMU.readGyroData(gyroCount);

  gyroRateRoll = (float)gyroCount[0] * myIMU.gRes;
  gyroRatePitch = (float)gyroCount[1] * myIMU.gRes;
}

Setelah memasukkan kode program di atas ke dalam sketch Arduino IDE, pastikan Anda telah memilih papan Arduino yang sesuai dan port COM yang benar. Kemudian, unggah (upload) kode program ke papan Arduino Anda.

Setelah mengunggah kode program, buka Serial Monitor di Arduino IDE. Anda akan melihat data sudut roll dan pitch yang diukur dari sensor MPU9250. Pastikan baud rate pada Serial Monitor diatur ke 115200.

Ukur Sudut MPU9250 Arduino Kode Program Dasar
Hasil dari Ukur Sudut MPU9250 Arduino

Sumber dasar perhitungan dari Carbon Aeronautics.

Leave a Comment

Your email address will not be published. Required fields are marked *

Scroll to Top