#9 Gyro

Optimalisasi pergeseran sudut 0-360 derajat Z(yaw)

Kode Program:

// sumber: https://randomnerdtutorials.com/esp32-mpu-6050-accelerometer-gyroscope-arduino/#include <Wire.h>#include <Adafruit_MPU6050.h>#include <Adafruit_Sensor.h>
Adafruit_MPU6050 mpu;
// Quaternion sederhanafloat q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;float beta = 0.05f; // parameter filter
// Bias gyrofloat gyroBiasX = 0.0f;float gyroBiasY = 0.0f;float gyroBiasZ = 0.0f;
// Offset yaw (default 0, bisa diubah lewat Serial)float yawOffset = 0.0f;
unsigned long lastUpdate = 0;
void setup() {  Serial.begin(115200);  while (!Serial) delay(10);
  if (!mpu.begin()) {    Serial.println("MPU6050 tidak ditemukan!");    while (1) yield();  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);  mpu.setGyroRange(MPU6050_RANGE_500_DEG);  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  lastUpdate = micros();
  Serial.println("Ketik '1' untuk kalibrasi gyro.");  Serial.println("Ketik 'm' untuk set yawOffset = 180 derajat.");  calibrateGyro();  yawOffset = 180.0f;}
void calibrateGyro() {  Serial.println("Kalibrasi gyro... jangan gerakkan sensor!");  float sumX = 0, sumY = 0, sumZ = 0;  const int samples = 500;
  for (int i = 0; i < samples; i++) {    sensors_event_t a, g, temp;    mpu.getEvent(&a, &g, &temp);    sumX += g.gyro.x;    sumY += g.gyro.y;    sumZ += g.gyro.z;    delay(5);  }
  gyroBiasX = sumX / samples;  gyroBiasY = sumY / samples;  gyroBiasZ = sumZ / samples;
  Serial.println("Kalibrasi selesai.");  Serial.print("Bias X: "); Serial.println(gyroBiasX);  Serial.print("Bias Y: "); Serial.println(gyroBiasY);  Serial.print("Bias Z: "); Serial.println(gyroBiasZ);}
void updateYaw(float gx, float gy, float gz,               float ax, float ay, float az, float deltat) {  // Normalisasi accelerometer  float norm = sqrt(ax*ax + ay*ay + az*az);  if (norm == 0) return;  ax /= norm; ay /= norm; az /= norm;
  // Integrasi gyro ke quaternion  gx *= (0.5f * deltat);  gy *= (0.5f * deltat);  gz *= (0.5f * deltat);
  float qa = q0, qb = q1, qc = q2, qd = q3;  q0 += (-qb*gx - qc*gy - qd*gz);  q1 += (qa*gx + qc*gz - qd*gy);  q2 += (qa*gy - qb*gz + qd*gx);  q3 += (qa*gz + qb*gy - qc*gx);
  // Normalisasi quaternion  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);  q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;}
void loop() {  // Cek input serial  if (Serial.available()) {    char c = Serial.read();    if (c == '1') {      calibrateGyro();    } else if (c == 'm') {      yawOffset = 186.0f;      Serial.println("Yaw offset diset ke 180 derajat.");    }  }
  unsigned long now = micros();  float deltat = (now - lastUpdate) / 1000000.0f; // detik  lastUpdate = now;
  sensors_event_t a, g, temp;  mpu.getEvent(&a, &g, &temp);
  // Gyro dalam rad/s dikurangi bias  float gx = g.gyro.x - gyroBiasX;  float gy = g.gyro.y - gyroBiasY;  float gz = g.gyro.z - gyroBiasZ;
  // Accel dalam m/s^2  float ax = a.acceleration.x;  float ay = a.acceleration.y;  float az = a.acceleration.z;
  // Update quaternion hanya untuk yaw  updateYaw(gx, gy, gz, ax, ay, az, deltat);
  // Hitung yaw dari quaternion  float yaw = atan2(2.0f*(q0*q3 + q1*q2),                    1.0f - 2.0f*(q2*q2 + q3*q3)) * 180.0f/M_PI;
  if (yaw < 0) yaw += 360.0f;
  // Tambahkan offset  yaw += yawOffset;  if (yaw >= 360.0f) yaw -= 360.0f;
  Serial.print("Yaw: ");  Serial.println(yaw);}

sumber: https://www.programmingelectronics.com/esp32-infrared-remote/

Report abuse