Servos glitching out with MPU6050

Hi, everyone I found this code and altered it to read two MPU6050 sensors on serial monitor and it works almost flawlessly. However Iv been stumped for a while on how to get 2 servos to follow two different of the readings on one Arduino board. My goal I to have servo 1 following MPU1 and servo2 following MPU2.
The way I’m doing it is just not working. I am also getting degree readings from the sensors so I am just trying to convert those numbers over to servo degree angles.

It would be great if someone could help.

#include <Wire.h>

#include <MPU6050.h>

#include <Servo.h>

MPU6050 gyro1 (0x68);
MPU6050 gyro2 (0x69);

int16_t ax1, ay1, az1;  //creates angle variables 1
int16_t gx1, gy1, gz1;

int16_t ax2, ay2, az2;  //creates angle variables 2
int16_t gx2, gy2, gz2;

Servo sholvert;
Servo sholhora;

void setup() {
  sholvert.attach (8);
  sholhora.attach (9);
  Wire.begin();  //starts I2C communication


void loop() {
  for (int j = 8; j <= 10; j++) {
    for (int k =  8; k <= 10; k++) {
      digitalWrite(k, HIGH);  //sets all sensors to high address
    digitalWrite(j, LOW);  //sets one sensor to low address
    gyro1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);  //calls values from sensor
    gyro2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);  //calls values from sensor

    ax1 = map(ax1, -17000, 17000, 0, 180);  //maps x axis rotation values from 0 to 180
    ay1 = map(ay1, -17000, 17000, 0, 180);
    az1 = map(az1, -17000, 17000, 0, 180);

    ax2 = map(ax2, -17000, 17000, 0, 180);  //maps x axis rotation values from 0 to 180
    ay2 = map(ay2, -17000, 17000, 0, 180);
    az2 = map(az2, -17000, 17000, 0, 180);

    Serial.println(" ");
    Serial.print("gyro1   ");
    Serial.print("  ");
    Serial.print("  ");
    Serial.print("       gyro2");
    Serial.print("   ");
    Serial.print("  ");
    sholvert.write(ay2);                    //myservo.write(180-(90+ ypr[2] * 180/M_PI)); 
    Serial.print("  ");


Please give more detail of "just not working". What exactly does happen? Do the servos move? What connection is there between the servo movements and the MPU outputs? Do your serial prints show the values you expect?

If your problem is just that only one servo moves it's not surprising as you never write anything to sholhora.


Sorry about that I was kind of in a rush. I am only trying to work on one servo now which is sholvert. The servo appears to have no correlation between there angle and the Serial monitor MPU6050. The servos just go back and forth really quick as if glitching. They will pause for a little them=n start again. It seems to have no pattern. Any good ideas?

The most common issue with servos is insufficient power. How are yours powered?

Can you get the sweep example working?

And post the simplified code that only references one gyro and one servo.

After taking care of power issues you might then consider using writeMicroseconds like so: X_Axis.writeMicroseconds( iX_HowMuch ).

If you have not placed limits on servo min/max that will give you a range from 500uSec to 2500uSec for a total servo torque range of 2000uS. If the servo is a 180 degree servo you will be able to do the math of 2000/180 = 11.11uSec per degree. You can then do some more math to figure out the number of uS for the angle given by 11.11 * AngleGiven = TorqueValue. Than you can apply that torque value to the servo, most likely doing a bit more math to either torque with the movement or torque opposite the movement angle measured.

If you place limits on the min/max, you will need to recompute your uS per degree. So the formula becomes 2000/total angle distance of servo.

Here is a low pass filter:

////Low Pass Filter
      fXg = Xg * alpha + (fXg * (1.0 - alpha));
      fYg = Yg * alpha + (fYg * (1.0 - alpha));
      fZg = Zg * alpha + (fZg * (1.0 - alpha));

where alpha is some number less then one, such as .97. You can play around with alpha for best results.

Here is another useful formula: //Roll & Pitch Equations from accelerometers only // float roll2 = (atan2(-mpu9250.getAyScaled(), mpu9250.getAzScaled())*180.0)/M_PI; // float pitch2 = (atan2( mpu9250.getAxScaled(), sqrt(mpu9250.getAyScaled()*mpu9250.getAyScaled() + mpu9250.getAzScaled()*mpu9250.getAzScaled()))*180.0)/M_PI;

The ESP32 is a bit different, using the ESP32 PWM API, you can get 3 clock ticks per uSec granularity. Much more accurate torque value then what the hobby servo can do.

If you want to go in all big, you can use the MahonyQuaternionUpdate ( mpu9250.getAxScaled(), mpu9250.getAyScaled(), mpu9250.getAzScaled(), mpu9250.getGxScaled() * PI / 180.0f, mpu9250.getGyScaled() * PI / 180.0f, mpu9250.getGzScaled() * PI / 180.0f, mpu9250.getMxScaled(), mpu9250.getMyScaled(), mpu9250.getMzScaled() ); You'd want to supply Kp and Ki values for error correction. I found that these values work really well, as a starting point, for the MPU9250:

#define Kp 7.50f
// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 1.7f

. In short order the MahonyQuaternionUpdate method will change those values., a helpful site.

Oh, A UNO will not be able to speedily do the MahonyQuaternionUpdate method.