380Kg.cm servo motor glitching/gittering out with gyro sensor

Hi everyone,

So I am working on a really big project that involves 8 380kg.cm servo motors that are all run by separate arduinos and gyros however all the big motors power supply is run by one battery...

... I will attach two videos and 1 picture below but they might take awhile to upload.
I have an arduino nano with a GY521 gyro sensor connected at pins

VCC - 5v
SCL - A5
SDA - A4

Then I have a PWM servo driver connected at its general pins... The PWM driver is there just to supply external poweer source to the servo which it has 4 AA batterys powering the PWM servo driver. The servo is connected to the servo driver then the accual motor is connected to a bigger battery above. The servos are...


... OK, so my problem is that the servo glitches when I move the gyro. I have reading and it all works fine for a little until I move the gyro to fast (which it needs to be in application), or after a while it will glitch and start gittering back and forth really fast. The interesting this is that the gyro readings will also go bizerk and its not because the gyro disconnected. The readings and gyro work fine by them selves and even a 9g servo works fine on the PWM even flawlessly. I have the servos signal wires hooked up to the PWM board because I originally had them hooked up to the arduino but it had the same results. The board on the servo wants 3 - 5 volts but when I give it that it goes bizerk like always. Also the servo alone goes fine when hooked up to RC.

Im 15 and really want to go into robotics and this really makes or breaks my project at least for now. Its like the gyro is kind of glitching out then the servo is reading the gyro and following it making the servo glitch out and gitter.

I really need help and any advice that can make this project go forward.

Thank you for your time for reading all this as well

(pictures follow)

Speed: 0.5Sec/60deg @ 24V 1.0sec/60deg @12V

Are you turning the gyro faster than 120 degrees per second?
Post your code.


#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include <Servo.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

Servo myservo;

MPU6050 mpu;   // 0x69 = HIGH, 0x68 = LOW

#define LED_PIN 13
#define SERVO_PIN 7
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;

#define SERVOMIN  217 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  417 // this is the 'maximum' pulse length count (out of 4096)

uint8_t servonum = 0;

void setup() {

  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  Fastwire::setup(400, true);

  // initialize serial communication
  // (115200 chosen because it is required for Teapot Demo output, but it's
  // really up to you depending on your project)
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  // initialize device
  Serial.println(F("Initializing I2C devices..."));

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));


  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  // configure LED for output
  pinMode(LED_PIN, OUTPUT);

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  if (!dmpReady) return;

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print(ypr[2] * 180 / M_PI);

    int angle = (180 - (90 + ypr[2] * 180 / M_PI));
    int pulse = map(angle, 0, 180, SERVOMIN, SERVOMAX);
    Serial.print("\t   ANGLE: ");
    Serial.print("\tpulse: ");
    pwm.setPWM(0, 0, pulse);

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);

I am not moving it faster than 120 degrees per second and Im having trouble uploading the videos