Jump to content
I2Cdevlib Forums

Recommended Posts

here is my code, i copied this code form 

   , to run this code with mpu-6050 i need to change register set bit values,  form datasheet of mpu6050 i changed the values , but still i did not get the right result.

 

 

 

#include <Wire.h> //Include the Wire.h library so we can communicate with the gyro

 
//Declaring variables
int cal_int;
unsigned long UL_timer;
double gyro_pitch, gyro_roll, gyro_yaw;
double gyro_roll_cal, gyro_pitch_cal, gyro_yaw_cal;
byte highByte, lowByte;
 
//Setup routine
void setup(){
  Wire.begin();                                      //Start the I2C as master
  Serial.begin(9600);                                //Start the serial connetion @ 9600bps
 
  //The gyro is disabled by default and needs to be started
  Wire.beginTransmission(105);                       //Start communication with the gyro (adress 1101001)
  Wire.write(0x20);                                  //We want to write to register 20
  Wire.write(0x0F);                                  //Set the register bits as 00001111 (Turn on the gyro and enable all axis)
  Wire.endTransmission();                            //End the transmission with the gyro
  Wire.beginTransmission(105);                       //Start communication with the gyro (adress 1101001)
  Wire.write(0x23);                                  //We want to write to register 23
  Wire.write(0x80);                                  //Set the register bits as 10000000 (Block Data Update active)
  Wire.endTransmission();                            //End the transmission with the gyro
 
 
  delay(250);                                        //Give the gyro time to start
  
  //Let's take multiple samples so we can determine the average gyro offset
  Serial.print("Starting calibration...");           //Print message
  for (cal_int = 0; cal_int < 2000 ; cal_int ++){    //Take 2000 readings for calibration
    gyro_signalen();                                 //Read the gyro output
    gyro_roll_cal += gyro_roll;                      //Ad roll value to gyro_roll_cal
    gyro_pitch_cal += gyro_pitch;                    //Ad pitch value to gyro_pitch_cal
    gyro_yaw_cal += gyro_yaw;                        //Ad yaw value to gyro_yaw_cal
    if(cal_int%100 == 0)Serial.print(".");           //Print a dot every 100 readings
    delay(4);                                        //Wait 4 milliseconds before the next loop
  }
  //Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset
  Serial.println(" done!");                          //2000 measures are done!
  gyro_roll_cal /= 2000;                             //Divide the roll total by 2000
  gyro_pitch_cal /= 2000;                            //Divide the pitch total by 2000
  gyro_yaw_cal /= 2000;                              //Divide the yaw total by 2000
  
}
//Main program
void loop(){
  delay(250);                                        //Wait 250 microseconds for every loop  
  gyro_signalen();                                   //Read the gyro signals
  print_output();                                    //Print the output
}
 
void gyro_signalen(){
  Wire.beginTransmission(105);                       //Start communication with the gyro (adress 1101001)
  Wire.write(168);                                   //Start reading @ register 28h and auto increment with every read
  Wire.endTransmission();                            //End the transmission
  Wire.requestFrom(105, 6);                          //Request 6 bytes from the gyro
  while(Wire.available() < 6);                       //Wait until the 6 bytes are received
  lowByte = Wire.read();                             //First received byte is the low part of the angular data
  highByte = Wire.read();                            //Second received byte is the high part of the angular data
  gyro_roll = ((highByte<<8)|lowByte);               //Multiply highByte by 256 and ad lowByte
  if(cal_int == 2000)gyro_roll -= gyro_roll_cal;     //Only compensate after the calibration
  lowByte = Wire.read();                             //First received byte is the low part of the angular data
  highByte = Wire.read();                            //Second received byte is the high part of the angular data
  gyro_pitch = ((highByte<<8)|lowByte);              //Multiply highByte by 256 and ad lowByte
  gyro_pitch *= -1;                                  //Invert axis
  if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal;   //Only compensate after the calibration
  lowByte = Wire.read();                             //First received byte is the low part of the angular data
  highByte = Wire.read();                            //Second received byte is the high part of the angular data
  gyro_yaw = ((highByte<<8)|lowByte);                //Multiply highByte by 256 and ad lowByte
  gyro_yaw *= -1;                                    //Invert axis
  if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal;       //Only compensate after the calibration
}
 
void print_output(){
  Serial.print("Pitch:");
  if(gyro_pitch >= 0)Serial.print("+");
  Serial.print(gyro_pitch/57.14286,0);               //Convert to degree per second
  if(gyro_pitch/57.14286 - 2 > 0)Serial.print(" NoU");
  else if(gyro_pitch/57.14286 + 2 < 0)Serial.print(" NoD");
  else Serial.print(" ---");
  Serial.print("  Roll:");
  if(gyro_roll >= 0)Serial.print("+");
  Serial.print(gyro_roll/57.14286,0);                //Convert to degree per second
  if(gyro_roll/57.14286 - 2 > 0)Serial.print(" RwD");
  else if(gyro_roll/57.14286 + 2 < 0)Serial.print(" RwU");
  else Serial.print(" ---");
  Serial.print("  Yaw:");
  if(gyro_yaw >= 0)Serial.print("+");
  Serial.print(gyro_yaw/57.14286,0);                 //Convert to degree per second
  if(gyro_yaw/57.14286 - 2 > 0)Serial.println(" NoR");
  else if(gyro_yaw/57.14286 + 2 < 0)Serial.println(" NoL");
  else Serial.println(" ---");
}
Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Loading...
 Share

×
×
  • Create New...