Connecting Servo Motor with MPU6050

DON’T power the servo off the Arduino 5V. Use an external power supply!!

For servo power, use a LiPo battery with a buck converter voltage regulator for best results.

Filipe-Moleiro: I've changed the offsets to myown ones, it is still not working. My connections are: Mpu to arduino: VCC-5v GND-GND SCL-3 SCA-2 AD0-GND INT-7

servo motor to arduino: yellow-13 red-5v brown-ground

So let me understand what you have implied. you are able to generate offsets with the mpu6050 and mpu calibration? Does the code I've given you generate gyro readings Yaw Pitch and Roll? Are they accurate and meet your needs? How are you trying to control the Servo? (Code example would be good here) Are you using a separate 5V power source for the Servo? Note: analogWrite() uses PWM or Pulse Width Modulation which is often confused with PPM Pulse Position Modulation Pulse Width Modulation: Pulse Position Modulation: Z

zhomeslice: So let me understand what you have implied. you are able to generate offsets with the mpu6050 and mpu calibration? Does the code I've given you generate gyro readings Yaw Pitch and Roll? Are they accurate and meet your needs? How are you trying to control the Servo? (Code example would be good here) Are you using a separate 5V power source for the Servo?

I forgot to click on the monitor serie, so now i tried it, the servo is moving now, but not consenquetly the mpu. Now back to your answers: Yes I am getting the values with the mpu. Yes it does. Yes they do. I am moving the mpu on all the axis to see if the servo is responding to it, which it isn't. No i am not, unfortunately right now i don´t have the necessary materials for it, but tomorrow I will work on it. I am going to power the servo with an external power suply and see if it is responding. Once I do I'll update you. Thank you.

Filipe-Moleiro: I forgot to click on the monitor serie, so now i tried it, the servo is moving now, but not consenquetly the mpu. Now back to your answers: Yes I am getting the values with the mpu. Yes it does. Yes they do. I am moving the mpu on all the axis to see if the servo is responding to it, which it isn't. No i am not, unfortunately right now i don´t have the necessary materials for it, but tomorrow I will work on it. I am going to power the servo with an external power suply and see if it is responding. Once I do I'll update you. Thank you.

This is excellent so the whole control the servo thing is really easy!!

NOTE since you are using the LED pin 13 on your servo be sure to change the line (near the top)

define LED_PIN 13 //

to another pin so the heartbeat i created won't interfere with the servo positioning.

You are going to pulse the servo between 1 and 2 milliseconds depending on where you want to be and then delay for a while. the mpu6050 pulses the int pin every 10ms for us and the servo I think likes a 20 ms delay between pulses (it could handle 10ms but I'm not sure) so we will skip a gyro reading and perform a simple pulse delay. I'm focusing on the MPUMath() function

// ================================================================
// ===                        MPU Math                          ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180.0 / M_PI);
  Pitch = (ypr[1] *  180.0 / M_PI);
  Roll = (ypr[2] *  180.0 / M_PI);
  static bool Toggle ;
  Toggle = Toggle !
  int ServoPosition; // this will contain a value between 1000 and 2000 (1500 is center)
  // this triggers every 10ms and we need to pulse the servo every 20ms so skip every other one.
  if (Toggle ){ 
    ServoPosition = map (Yaw, -180, 180,1000,2000);
    digitalWrite(13,HIGH);
    delayMicroseconds(ServoPostion); // ---Nasty blocking DELAY for testing only!!!--- we will make a change and use a millis timer later
    digitalWrite(13,LOW);
  }

// ... debug stuff 
}

Z

Is there some reason why you don't use the much simpler Servo.h library to control the servo(s)?

Steve

slipstick: Is there some reason why you don't use the much simpler Servo.h library to control the servo(s)?

Steve

none. just providing a basic example that provided functionality test. note the comment: "// ---Nasty blocking DELAY for testing only!!!--- we will make a change and use a millis timer later" Servo.h is much better and doesn't block code like the delayMicroseconds. Z

zhomeslice: This is excellent so the whole control the servo thing is really easy!!

NOTE since you are using the LED pin 13 on your servo be sure to change the line (near the top)

define LED_PIN 13 //

to another pin so the heartbeat i created won't interfere with the servo positioning.

You are going to pulse the servo between 1 and 2 milliseconds depending on where you want to be and then delay for a while. the mpu6050 pulses the int pin every 10ms for us and the servo I think likes a 20 ms delay between pulses (it could handle 10ms but I'm not sure) so we will skip a gyro reading and perform a simple pulse delay. I'm focusing on the MPUMath() function

// ================================================================
// ===                        MPU Math                          ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180.0 / M_PI);
  Pitch = (ypr[1] *  180.0 / M_PI);
  Roll = (ypr[2] *  180.0 / M_PI);
  static bool Toggle ;
  Toggle = Toggle !
  int ServoPosition; // this will contain a value between 1000 and 2000 (1500 is center)
  // this triggers every 10ms and we need to pulse the servo every 20ms so skip every other one.
  if (Toggle ){ 
    ServoPosition = map (Yaw, -180, 180,1000,2000);
    digitalWrite(13,HIGH);
    delayMicroseconds(ServoPostion); // ---Nasty blocking DELAY for testing only!!!--- we will make a change and use a millis timer later
    digitalWrite(13,LOW);
  }

// ... debug stuff }



Z

So I powered the servo with an external suply, and copy paste the code of the MPU Math. This is mine right now

// ================================================================
// ===                        MPU Math                          ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180.0 / M_PI);
  Pitch = (ypr[1] *  180.0 / M_PI);
  Roll = (ypr[2] *  180.0 / M_PI);
  static bool Toggle ;
  Toggle = Toggle ;
  int ServoPosition = 11; // this will contain a value between 1000 and 2000 (1500 is center)
  // this triggers every 10ms and we need to pulse the servo every 20ms so skip every other one.
  if (Toggle ){ 
    ServoPosition = map (Yaw, -180, 180,1000,2000);
    digitalWrite(11,HIGH);
    delayMicroseconds(ServoPosition); // ---Nasty blocking DELAY for testing only!!!--- we will make a change and use a millis timer later
    digitalWrite(11,LOW);
  }
  
  DPRINTSTIMER(100) {
    DPRINTSFN(15, " W:", q.w, -6, 4);
    DPRINTSFN(15, " X:", q.x, -6, 4);
    DPRINTSFN(15, " Y:", q.y, -6, 4);
    DPRINTSFN(15, " Z:", q.z, -6, 4);

    DPRINTSFN(15, " Yaw:", Yaw, -6, 2);
    DPRINTSFN(15, " Pitch:", Pitch, -6, 2);
    DPRINTSFN(15, " Roll:", Roll, -6, 2);
    DPRINTSFN(15, " Yaw:", ypr[0], -6, 2);
    DPRINTSFN(15, " Pitch:", ypr[1], -6, 2);
    DPRINTSFN(15, " Roll:", ypr[2], -6, 2);
    DPRINTLN();
  }
}

It is not working, the servo is still not moving as expected. I changed the LED_PIN from 13 to the 11 too.

Filipe-Moleiro: So I powered the servo with an external suply, and copy paste the code of the MPU Math. This is mine right now

// ================================================================
// ===                        MPU Math                          ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180.0 / M_PI);
  Pitch = (ypr[1] *  180.0 / M_PI);
  Roll = (ypr[2] *  180.0 / M_PI);
  static bool Toggle ;
  Toggle = Toggle ;
  int ServoPosition = 11; // this will contain a value between 1000 and 2000 (1500 is center)
  // this triggers every 10ms and we need to pulse the servo every 20ms so skip every other one.
  if (Toggle ){ 
    ServoPosition = map (Yaw, -180, 180,1000,2000);
    digitalWrite(11,HIGH);
    delayMicroseconds(ServoPosition); // ---Nasty blocking DELAY for testing only!!!--- we will make a change and use a millis timer later
    digitalWrite(11,LOW);
  }
  
  DPRINTSTIMER(100) {
    DPRINTSFN(15, " W:", q.w, -6, 4);
    DPRINTSFN(15, " X:", q.x, -6, 4);
    DPRINTSFN(15, " Y:", q.y, -6, 4);
    DPRINTSFN(15, " Z:", q.z, -6, 4);

   DPRINTSFN(15, " Yaw:", Yaw, -6, 2);    DPRINTSFN(15, " Pitch:", Pitch, -6, 2);    DPRINTSFN(15, " Roll:", Roll, -6, 2);    DPRINTSFN(15, " Yaw:", ypr[0], -6, 2);    DPRINTSFN(15, " Pitch:", ypr[1], -6, 2);    DPRINTSFN(15, " Roll:", ypr[2], -6, 2);    DPRINTLN();  } }




It is not working, the servo is still not moving as expected. I changed the LED_PIN from 13 to the 11 too.

do you have code that can control the servo by itself? Can I see that code?

zhomeslice:
do you have code that can control the servo by itself?
Can I see that code?

I’m just using the code you gave me, ill put it as an attachment. The servo is working, I tired it when I got it.

MPU6050_Latest_code_Leonardo_int_pin_7.ino (8.31 KB)

Filipe-Moleiro: I'm just using the code you gave me, ill put it as an attachment. The servo is working, I tired it when I got it.

do you have the code that you used to test the servo. Does the servo still function if you uploaded that code? can you attach it so I can see it. I need an idea as to where the disconnect is. Z

zhomeslice:
do you have the code that you used to test the servo. Does the servo still function if you uploaded that code?
can you attach it so I can see it. I need an idea as to where the disconnect is.
Z

When I tried the servo, I used the Sweep Code. Here it is (sorry for not understanding your request at first)

Sweep.ino (1020 Bytes)

zhomeslice: do you have the code that you used to test the servo. Does the servo still function if you uploaded that code? can you attach it so I can see it. I need an idea as to where the disconnect is. Z

I tried to connect the 3 servos, they are working weirdly, they try to move, but they dont, they just tremble.

Filipe-Moleiro: I tried to connect the 3 servos, they are working weirdly, they try to move, but they dont, they just tremble.

That is very often insufficient power. How exactly are the servos connected (all 3 wires) and what is powering them? If they are connected via a breadboard that may be the problem, breadboards can't handle the relatively high currents that servos need.

Steve

So I found out my problem, I wasn´t connecting the whole circuit because of the grounds. But now it is corrected. Theres still a problem, when I use the code, the servos move, but not consequently the mpu, they go from 180 degrees to 0 degrees.

Filipe-Moleiro: So I found out my problem, I wasn´t connecting the whole circuit because of the grounds. But now it is corrected. Theres still a problem, when I use the code, the servos move, but not consequently the mpu, they go from 180 degrees to 0 degrees.

So does your sweep.I'm guide work properly

zhomeslice: So does your sweep.I'm guide work properly

So i'm gonna explain my situation clearly. I tested all my sg90 with Sweep Code, they worked how they are supposed to with sweep code (moved from 180 degrees to 0 degrees). Next I used the code you gave me.Everythings connected properly. When I use the code and move the MPU, the servos move from 180 degrees to 0 degrees, but, before they move again, they tremble a bit and then they go from 0 degrees to 180 again (always like this). This is my situation right now. Any question, please ask.

It's about time you posted the latest version of the code you are using. Trying to work out what is wrong without it is hopeless.

Steve

slipstick:
It’s about time you posted the latest version of the code you are using. Trying to work out what is wrong without it is hopeless.

Steve

Here it is.

MPU6050_Latest_code_Leonardo_int_pin_7.ino (8.29 KB)

That code only attempts to write to one servo on pin 11, which you also have defined as LEDpin and write to at odd times.

So if you have several servos moving from 180 to 0 and back either that isn't the code or some sort of miracle is happening. I can't help with miracles or with the wrong code.

Steve

Try this

#include <Servo.h>
Servo myservo;  // create servo object to control a servo
int pos = 0;    // variable to store the servo position


#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu;

#define DEBUG
#ifdef DEBUG
//#define DPRINT(args...)  Serial.print(args)             //OR use the following syntax:
#define DPRINTSTIMER(t)    for (static unsigned long SpamTimer; (unsigned long)(millis() - SpamTimer) >= (t); SpamTimer = millis())
#define  DPRINTSFN(StrSize,Name,...) {char S[StrSize];Serial.print("\t");Serial.print(Name);Serial.print(" "); Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Percision
#define DPRINTLN(...)      Serial.println(__VA_ARGS__)
#else
#define DPRINTSTIMER(t)    if(false)
#define DPRINTSFN(...)     //blank line
#define DPRINTLN(...)      //blank line
#endif




#define interruptPin 7
#define LED_PIN 11 // 


// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino
//                       XA      YA      ZA      XG      YG      ZG
//int MPUOffsets[6] = {    2471,   -563,   690,   66,     -29,     39}; //
//int MPUOffsets[6] = {1136, -44, 1047 , 52, 5, 26};// Test Board 
int MPUOffsets[6] = {-769, -427, 1955 , 93, -6, 4};// Test Board 9255

// ================================================================
// ===                      i2c SETUP Items                     ===
// ================================================================
void i2cSetup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
}

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

// ================================================================
// ===                      MPU DMP SETUP                       ===
// ================================================================
int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected
// MPU control/status vars
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

// orientation/motion vars
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
byte StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)

void MPU6050Connect() {
  static int MPUInitCntr = 0;
  // initialize device
  mpu.initialize(); // same
  // load and configure the DMP
  devStatus = mpu.dmpInitialize();// same

  if (devStatus != 0) {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)

    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[devStatus]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }
  mpu.setXAccelOffset(MPUOffsets[0]);
  mpu.setYAccelOffset(MPUOffsets[1]);
  mpu.setZAccelOffset(MPUOffsets[2]);
  mpu.setXGyroOffset(MPUOffsets[3]);
  mpu.setYGyroOffset(MPUOffsets[4]);
  mpu.setZGyroOffset(MPUOffsets[5]);

  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);
  // enable Arduino interrupt detection
  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
  Serial.print("mpu.getInterruptDrive=  "); Serial.println(mpu.getInterruptDrive());
  attachInterrupt(digitalPinToInterrupt(interruptPin), dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus(); // Same
  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
  delay(1000); // Let it Stabalize
  mpu.resetFIFO(); // Clear fifo buffer
  mpu.getIntStatus();
  mpuInterrupt = false; // wait for next interrupt
}

// ================================================================
// ===                    MPU DMP Get Data                      ===
// ================================================================
void GetDMP() { // Best version I have made so far
  // Serial.println(F("FIFO interrupt at:"));
  // Serial.println(micros());
  static unsigned long LastGoodPacketTime;
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
    digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
    mpu.resetFIFO();// clear the buffer and start over
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      fifoCount -= packetSize;
    }
    LastGoodPacketTime = millis();
    MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
  }
}


// ================================================================
// ===                        MPU Math                          ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180.0 / M_PI);
  Pitch = (ypr[1] *  180.0 / M_PI);
  Roll = (ypr[2] *  180.0 / M_PI);

  myservo.write(map (Yaw, -180, 180,0,180));             

  DPRINTSTIMER(100) {
    DPRINTSFN(15, " W:", q.w, -6, 4);
    DPRINTSFN(15, " X:", q.x, -6, 4);
    DPRINTSFN(15, " Y:", q.y, -6, 4);
    DPRINTSFN(15, " Z:", q.z, -6, 4);

    DPRINTSFN(15, " Yaw:", Yaw, -6, 2);
    DPRINTSFN(15, " Pitch:", Pitch, -6, 2);
    DPRINTSFN(15, " Roll:", Roll, -6, 2);
    DPRINTSFN(15, " Yaw:", ypr[0], -6, 2);
    DPRINTSFN(15, " Pitch:", ypr[1], -6, 2);
    DPRINTSFN(15, " Roll:", ypr[2], -6, 2);
    DPRINTLN();
  }
}
// ================================================================
// ===                         Setup                            ===
// ================================================================
void setup() {
  Serial.begin(115200); //115200
  while (!Serial);
  Serial.println("i2cSetup");
  i2cSetup();
  Serial.println("MPU6050Connect");
  MPU6050Connect();
  Serial.println("Setup complete");
  pinMode(LED_PIN, OUTPUT);
    myservo.attach(9);  // attaches the servo on pin 9 to the servo object

}
// ================================================================
// ===                          Loop                            ===
// ================================================================
void loop() {
  if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
    GetDMP();
  }
}