Servo SG90 and MPU6050 angle estimation

Other Hardware Sensors
1

In my project, I have an MPU6050 sensor mounted 2.5 cm away from the center of an SG90 servo motor arm. The servo is controlled using PWM, and it moves in a sinusoidal pattern, while the MPU6050 measures angular velocity (gyro) and linear acceleration. My goal is to accurately calculate the yaw angle (rotation around the Z-axis) of the MPU6050 as the servo moves, along with the linear acceleration due to its position on the servo arm. However, I’m facing a problem with gyro drift and inaccurate readings due to improper offset values. I need a reliable method to calibrate the MPU6050 and ensure it provides accurate yaw angle and linear acceleration measurements even when moving.

thanks!

#include <MPU6050.h>
#include <Servo.h>

MPU6050 mpu;
Servo myServo;

// Servo and MPU6050 Parameters
const int SERVO_MIN_PWM = 1000; // -90°
const int SERVO_MAX_PWM = 2000; // +90°
const int SERVO_MID_PWM = 1500; // 0°
const float ARM_LENGTH = 0.025; // Servo arm length (meters - 2.5 cm)

float yawAngle = 0;
float omegaZ = 0;
float alphaZ = 0;
int gz_offset = 0;
unsigned long lastTime;
unsigned long lastPrintTime = 0;

// Calibration Function
void calibrateMPU6050() {
  long sum_gz = 0;
  const int N = 1000;
  Serial.println(">> Gyro Z-axis offset calibration...");
  delay(1000);
  
  for (int i = 0; i < N; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sum_gz += gz;
    delay(5);
  }

  gz_offset = sum_gz / N;
  Serial.print(">> GZ Offset: ");
  Serial.println(gz_offset);
}

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

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }

  myServo.attach(3);
  myServo.write(0);  // Start servo at 0°
  yawAngle = 0;      // Start yaw angle at 0°
  
  lastTime = micros();
  calibrateMPU6050();
  Serial.println(">> System initialized.");
}

void loop() {
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0;
  lastTime = now;

  // Sinusoidal Servo Movement (-45° to +45°)
  float t = now / 1000000.0;
  float sineAngle = 45.0 * sin(2 * PI * 0.5 * t); // -45° to +45°
  float commandAngle = sineAngle;

  // PWM Calculation for Servo
  int pulseWidth = SERVO_MID_PWM + (sineAngle * (SERVO_MAX_PWM - SERVO_MIN_PWM) / 180.0);
  pulseWidth = constrain(pulseWidth, SERVO_MIN_PWM, SERVO_MAX_PWM);
  myServo.writeMicroseconds(pulseWidth);

  // Reading MPU6050 Data
  int16_t ax_raw, ay_raw, az_raw, gx, gy, gz_raw;
  mpu.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx, &gy, &gz_raw);

  // Gyro Data (Z-axis - Yaw Calculation)
  float gz_dps = (gz_raw - gz_offset) / 131.0; // Gyro sensitivity (MPU6050: 131.0 for 250°/s)
  omegaZ = gz_dps;                // Angular Velocity (Yaw Rate)
  yawAngle += omegaZ * dt;        // Yaw Angle Calculation (Integral of Angular Velocity)

  // Linear Acceleration Calculation (Due to Rotation)
  float ax_linear = -ARM_LENGTH * pow(omegaZ * PI / 180.0, 2) * sin(yawAngle * PI / 180.0);
  float ay_linear = ARM_LENGTH * pow(omegaZ * PI / 180.0, 2) * cos(yawAngle * PI / 180.0);

  // Printing Data Every 50 ms
  if (millis() - lastPrintTime >= 50) {
    lastPrintTime = millis();

    Serial.print("Servo Command Angle: ");
    Serial.print(commandAngle, 2);
    Serial.println("°");

    Serial.print("MPU6050 Measured Yaw Angle: ");
    Serial.print(yawAngle, 2);
    Serial.println("°");

    Serial.print("Yaw Rate (ω): ");
    Serial.print(omegaZ, 2);
    Serial.println(" °/s");

    Serial.print("Linear Acceleration X (ax): ");
    Serial.print(ax_linear, 4);
    Serial.println(" m/s²");

    Serial.print("Linear Acceleration Y (ay): ");
    Serial.print(ay_linear, 4);
    Serial.println(" m/s²");

    // CSV Format for Data Logging (Matlab, Excel, etc.)
    Serial.print(commandAngle, 2);
    Serial.print(",");
    Serial.print(yawAngle, 2);
    Serial.print(",");
    Serial.print(omegaZ, 2);
    Serial.print(",");
    Serial.print(ax_linear, 4);
    Serial.print(",");
    Serial.println(ay_linear, 4);
    Serial.println();
  }

  delay(5);
}
2

It would help if you posted exactly what you think the output should be and what it is.

3

Check the offset correction by calculating omegaZ for some period of time with the servo arm stationary.

For accurate numerical integration, small the time steps are required. The delay(5) seems pointless, so try removing that line. Accuracy is also lost due to the time taken by the step printing the unnecessarily verbose descriptive headings and IMU data.

4

for the output I want command angle and MPU6050 angle of Z-axis similar or have a little difference.

5

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.