Using values of mpu6050 gyro to control 4 servos. Connection are fine but i m facing issue with angle measurements. How do i fix my code?

Other Hardware Sensors
1 

#include<Wire.h>                //I2C Wire Library 

#include<Servo.h>               //Servo Motor Library


Servo servo_1;      

Servo servo_2;

Servo servo_3;

Servo servo_4;


const int MPU_addr=0x68;        //MPU6050 I2C Address

int16_t axis_X,axis_Y,axis_Z;

int minVal=265;

int maxVal=402;


double x;

double y;

double z;

 

void setup()

{

  Serial.begin(9600);

  

  Wire.begin();                      //Initilize I2C Communication

  Wire.beginTransmission(MPU_addr);  //Start communication with MPU6050

  Wire.write(0x6B);                  //Writes to Register 6B

  Wire.write(0);                     //Writes 0 into 6B Register to Reset

  Wire.endTransmission(true);        //Ends I2C transmission

  

  servo_1.attach(2);   // Forward/Reverse_Motor

  servo_2.attach(3);   // Up/Down_Motor

  servo_3.attach(4);    // Forward/Reverse_Motor

  servo_4.attach(5);   // Left/Right_Motor

  

}


void loop()

{

  Wire.beginTransmission(MPU_addr);    

  Wire.write(0x3B);                  //Start with regsiter 0x3B 

  Wire.endTransmission(false);     

  Wire.requestFrom(MPU_addr,14,true);  //Read 14 Registers 

  

  axis_X=Wire.read()<<8|Wire.read();                //Reads the MPU6050 X,Y,Z AXIS Value

  axis_Y=Wire.read()<<8|Wire.read();

  axis_Z=Wire.read()<<8|Wire.read();

  

  int xAng = map(axis_X,minVal,maxVal,-90,90);     // Maps axis values in terms of -90 to +90  

  int yAng = map(axis_Y,minVal,maxVal,-90,90);

  int zAng = map(axis_Z,minVal,maxVal,-90,90);

       

  x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);       //Formula to convert into degree

  y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);

  z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);


   //forward backward using 2 mid servo(m1 m3)-ROLL

  if(x >=0 && x <= 60) 

  {

     int mov1 = map(x,0,60,0,90);

     Serial.print("Movement in up/down = ");

     Serial.print(mov1);

     Serial.println((char)176);

 
       servo_1.write(mov1),servo_3.write(mov1);

  } 

 

  else if(x >=300 && x <= 360) 

  {

     int mov1 = map(x,360,250,0,180);

     Serial.print("Movement in Up/Down = ");

     Serial.print(mov1);

     Serial.println((char)176);
     
     servo_1.write(mov1),servo_3.write(mov1);

  } 

  //up and down using m2-PITCH
  
   if(y >=0 && y <= 60) 

  {

     int mov2 = map(y,0,60,0,90);

     Serial.print("Movement in F/R = ");

     Serial.print(mov2);

     Serial.println((char)176);
 
     servo_4.write(mov2);

  } 

 

  else if(y >=300 && y <= 360) 

  {

     int mov2 = map(y,360,250,0,180);

     Serial.print("Movement in F/R = ");

     Serial.print(mov2);

     Serial.println((char)176);
     
     servo_4.write(mov2);

  } 


//left right using bottom servo(m4)-YAW

 if(z >=0 && z <= 60) 

  {

     int mov3 = map(z,0,60,90,180);

     Serial.print("Movement in Left = ");

     Serial.print(mov3);

     Serial.println((char)176);

     servo_2.write(mov3);

  } 

 

  else if(z >=300 && z <= 360) 

  {

     int mov3 = map(z,360,300,90,0);

     Serial.print("Movement in Right = ");

     Serial.print(mov3);

     Serial.println((char)176);

     servo_2.write(mov3);

  } 

}```
2

Please don't put the whole question in the title!

What "issue", exactly, are you facing?

What testing/investigation/debugging have you done to find the issue?

What did you find out during your tests/investigations/debugging ?

3

You can try this:

#include <Wire.h>     // I2C Wire Library
#include <Servo.h>    // Servo Motor Library

Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;

const int MPU_addr = 0x68;  // MPU6050 I2C Address
int16_t axis_X, axis_Y, axis_Z;
int minVal = 265;
int maxVal = 402;

double x;
double y;
double z;

void setup()
{
  Serial.begin(9600);

  Wire.begin();                               // Initialize I2C Communication
  Wire.beginTransmission(MPU_addr);           // Start communication with MPU6050
  Wire.write(0x6B);                           // Writes to Register 6B
  Wire.write(0);                              // Writes 0 into 6B Register to Reset
  Wire.endTransmission(true);                 // Ends I2C transmission

  servo_1.attach(2);   // Forward/Reverse_Motor
  servo_2.attach(3);   // Up/Down_Motor
  servo_3.attach(4);   // Forward/Reverse_Motor
  servo_4.attach(5);   // Left/Right_Motor
}

void loop()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);                           // Start with register 0x3B
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);        // Read 14 Registers

  axis_X = Wire.read() << 8 | Wire.read();     // Reads the MPU6050 X, Y, Z AXIS Value
  axis_Y = Wire.read() << 8 | Wire.read();
  axis_Z = Wire.read() << 8 | Wire.read();

  int xAng = map(axis_X, minVal, maxVal, -90, 90);   // Maps axis values in terms of -90 to +90
  int yAng = map(axis_Y, minVal, maxVal, -90, 90);
  int zAng = map(axis_Z, minVal, maxVal, -90, 90);

  x = atan2(-yAng, -zAng) * RAD_TO_DEG + 180;    // Formula to convert into degree
  y = atan2(-xAng, -zAng) * RAD_TO_DEG + 180;
  z = atan2(-yAng, -xAng) * RAD_TO_DEG + 180;

  // Forward/Backward using mid servos (m1 m3) - ROLL
  if (x >= 0 && x <= 60)
  {
    int mov1 = map(x, 0, 60, 0, 90);
    Serial.print("Movement in Up/Down = ");
    Serial.print(mov1);
    Serial.println((char)176);
    servo_1.write(mov1);
    servo_3.write(mov1);
  }
  else if (x >= 300 && x <= 360)
  {
    int mov1 = map(x, 360, 250, 0, 180);
    Serial.print("Movement in Up/Down = ");
    Serial.print(mov1);
    Serial.println((char)176);
    servo_1.write(mov1);
    servo_3.write(mov1);
  }

  // Up/Down using m2 - PITCH
  if (y >= 0 && y <= 60)
  {
    int mov2 = map(y, 0, 60, 0, 90);
    Serial.print("Movement in F/R = ");
    Serial.print(mov2);
    Serial.println((char)176);
    servo_4.write(mov2);
  }
  else if (y >= 300 && y <= 360)
  {
    int mov2 = map(y, 360, 250, 0, 180);
    Serial.print("Movement in F/R = ");
    Serial.print(mov2);
    Serial.println((char)176);
    servo_4.write(mov2);
  }

  // Left/Right using bottom servo (m4) - YAW
  if (z >= 0 && z <= 60)
  {
    int mov3 = map(z, 0, 60, 90, 180);
    Serial.print("Movement in Left = ");
    Serial.print(mov3);
    Serial.println((char)176);
    servo_2.write(mov3);
  }
  else if (z >= 300 && z <= 360)
  {
    int mov3 = map(z, 360, 300, 90, 0);
    Serial.print("Movement in Right = ");
    Serial.print(mov3);
    Serial.println((char)176);
    servo_2.write(mov3);
  }
}
4

You can measure only two independent tilt angles using the MPU-6050.

Here is the minimum code required to do that, correctly. See the link in the code for the definition of the Euler angles.

// minimal MPU-6050 tilt and roll (sjr). Works with MPU-9250 too.
// works perfectly with GY-521, pitch and roll signs agree with arrows on sensor module 7/2019
//
// Tested with 3.3V eBay Pro Mini with no external pullups on I2C bus (worked with internal pullups)
// Add 4.7K pullup resistors to 3.3V if required. A4 = SDA, A5 = SCL

#include<Wire.h>
const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;

void setup() {

  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);                        //end the transmission
  Serial.begin(9600);
}

void loop() {

  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B);  //send starting register address, accelerometer high byte
  Wire.endTransmission(false); //restart for read
  Wire.requestFrom(MPU_addr1, 6); //get six bytes accelerometer data
  int t = Wire.read();
  xa = (t << 8) | Wire.read();
  t = Wire.read();
  ya = (t << 8) | Wire.read();
  t = Wire.read();
  za = (t << 8) | Wire.read();
// formula from https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
  roll = atan2(ya , za) * 180.0 / PI;
  pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //account for roll already applied

  Serial.print("roll = ");
  Serial.print(roll,1);
  Serial.print(", pitch = ");
  Serial.println(pitch,1);
  delay(400);
}
5

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