Connecting Servo Motor with MPU6050

Projects Programming
27

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.