Arduino MPU6050 Digital Spirit Level Measuring Device

Arduino MPU6050 Digital Spirit Level Measuring Device

CIRCUIT DIAGRAM :

MATERIALS :

Arduino NANO
MPU6050
OLED Display
Buzzer
LED
Breadboard

CODE :

//Eliyas Science Info
//MPU6050 LEVEL Device Pitch Roll

#include <Wire.h>
#include <Adafruit_SSD1306.h>
//OLED====================================================================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET 4
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Level LEDs==============================================================
int levelLED_neg2 = 5;  
int levelLED_neg1 = 7;
int levelLED_level = 8; 
int levelLED_pos1 = 9;  
int levelLED_pos2 = 11; 
int buzzer = 13;
//Variables for Gyroscope=================================================
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
 
// Setup timers and temp variables
long loop_timer;
int temp;
 
// Display counter
int displaycount = 0;
//========================================================================
void setup()
{ 
  Wire.begin();
  Serial.begin(9600);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
  {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  display.clearDisplay(); 
  analogReference(INTERNAL); 
  
  pinMode(levelLED_neg2, OUTPUT);
  pinMode(levelLED_neg1, OUTPUT);
  pinMode(levelLED_level, OUTPUT); 
  pinMode(levelLED_pos1, OUTPUT);
  pinMode(levelLED_pos2, OUTPUT); 
  pinMode(buzzer, OUTPUT);
  
  //Setup the registers of the MPU-6050                                                       
  setup_mpu_6050_registers(); 
  //Read the raw acc and gyro data from the MPU-6050 1000 times                                          
  for (int cal_int = 0; cal_int < 1000 ; cal_int ++)
  {                  
    read_mpu_6050_data(); 
    //Add the gyro x offset to the gyro_x_cal variable                                            
    gyro_x_cal += gyro_x;
    //Add the gyro y offset to the gyro_y_cal variable                                              
    gyro_y_cal += gyro_y; 
    //Add the gyro z offset to the gyro_z_cal variable                                             
    gyro_z_cal += gyro_z; 
    //Delay 3us to have 250Hz for-loop                                             
    delay(3);                                                          
  }
  // Divide all results by 1000 to get average offset
  gyro_x_cal /= 1000; gyro_y_cal /= 1000; gyro_z_cal /= 1000;                                             
  Serial.begin(115200);
  loop_timer = micros();                                               
}
//========================================================================
void loop()
{
  // Get data from MPU-6050
  read_mpu_6050_data();
  
  //Subtract the offset values from the raw gyro values
  gyro_x -= gyro_x_cal;                                                
  gyro_y -= gyro_y_cal;                                                
  gyro_z -= gyro_z_cal;                                                
         
  //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
  
  //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_pitch += gyro_x * 0.0000611;  
  //Calculate the traveled roll angle and add this to the angle_roll variable
  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians                                
  angle_roll += gyro_y * 0.0000611; 
                                     
  //If the IMU has yawed transfer the roll angle to the pitch angle
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
  //If the IMU has yawed transfer the pitch angle to the roll angle               
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               
  
  //Accelerometer angle calculations
  
  //Calculate the total accelerometer vector
  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); 
   
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  //Calculate the pitch angle
  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; 
  //Calculate the roll angle      
  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       
  
  //Accelerometer calibration value for pitch
  angle_pitch_acc -= 0.0;
  //Accelerometer calibration value for roll                                              
  angle_roll_acc -= 0.0;                                               
 
  if(set_gyro_angles)
  { 
  //If the IMU has been running 
  //Correct the drift of the gyro pitch angle with the accelerometer pitch angle                      
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; 
    //Correct the drift of the gyro roll angle with the accelerometer roll angle    
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        
  }
  else
  { 
    //IMU has just started  
    //Set the gyro pitch angle equal to the accelerometer pitch angle                                                           
    angle_pitch = angle_pitch_acc;
    //Set the gyro roll angle equal to the accelerometer roll angle                                       
    angle_roll = angle_roll_acc;
    //Set the IMU started flag                                       
    set_gyro_angles = true;                                            
  }
  
  //To dampen the pitch and roll angles a complementary filter is used
  //Take 90% of the output pitch value and add 10% of the raw pitch value
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; 
  //Take 90% of the output roll value and add 10% of the raw roll value 
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; 
  //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop  
  
  // Print to Serial Monitor   
  //Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
  
  
  // Increment the display counter
  displaycount = displaycount +1;
 //OUTPUT==================================================================
  if (displaycount > 100)
  {
    display.setTextColor(WHITE);
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(16,0);
    display.print("MPU-6050");
    display.setCursor(0,12);
    display.print("----------");
    display.setCursor(16,25);
    display.print("P: "); display.print(angle_pitch_output);
    display.setCursor(16,48);
    display.print("R: "); display.print(angle_roll_output);
    display.display();
  
    // Check Angle for Level LEDs
    if (angle_pitch_output < -4.01)
    {
      tone(buzzer, 1000); delay(500);
    }
    
    else if ((angle_pitch_output > -4.00) && (angle_pitch_output < -3.01))
    {
      
      digitalWrite(levelLED_neg2, HIGH);
      
      digitalWrite(levelLED_neg1, LOW);
      digitalWrite(levelLED_level, LOW);
      digitalWrite(levelLED_pos1, LOW);
      
      digitalWrite(levelLED_pos2, LOW);
      
    }
    
    else if ((angle_pitch_output > -2.00) && (angle_pitch_output < -1.01))
    {
      
      digitalWrite(levelLED_neg2, LOW);
      
      digitalWrite(levelLED_neg1, HIGH);
      digitalWrite(levelLED_level, LOW);
      digitalWrite(levelLED_pos1, LOW);
      
      digitalWrite(levelLED_pos2, LOW);
      
    }
    else if ((angle_pitch_output < 1.00) && (angle_pitch_output > -1.00))
    {
      
      digitalWrite(levelLED_neg2, LOW);
      
      digitalWrite(levelLED_neg1, LOW);
      digitalWrite(levelLED_level, HIGH);
      digitalWrite(levelLED_pos1, LOW);
      
      digitalWrite(levelLED_pos2, LOW);
      
    }
    else if ((angle_pitch_output > 1.01) && (angle_pitch_output < 2.00))
    {
      
      digitalWrite(levelLED_neg2, LOW);
      
      digitalWrite(levelLED_neg1, LOW);
      digitalWrite(levelLED_level, LOW);
      digitalWrite(levelLED_pos1, HIGH);
      
      digitalWrite(levelLED_pos2, LOW);
      
    }
    
    else if ((angle_pitch_output > 3.01) && (angle_pitch_output < 4.00))
    {
      
      digitalWrite(levelLED_neg2, LOW);
      
      digitalWrite(levelLED_neg1, LOW);
      digitalWrite(levelLED_level, LOW);
      digitalWrite(levelLED_pos1, LOW);
      
      digitalWrite(levelLED_pos2, HIGH);
      
    }
    else if (angle_pitch_output > 4.01)
    {
      tone(buzzer, 1000); delay(500);
    }  
    displaycount = 0; 
  }
 //========================================================================
 while(micros() - loop_timer < 4000); 
 //Reset the loop timer                                
 loop_timer = micros();
 noTone(buzzer);
}
//FUNCTION=================================================================
void setup_mpu_6050_registers()
{ 
  //Activate the MPU-6050 
  //Start communicating with the MPU-6050
  Wire.beginTransmission(0x68); 
  //Send the requested starting register                                       
  Wire.write(0x6B);  
  //Set the requested starting register                                                  
  Wire.write(0x00);
  //End the transmission                                                    
  Wire.endTransmission(); 
                                              
  //Configure the accelerometer (+/-8g)
  //Start communicating with the MPU-6050
  Wire.beginTransmission(0x68); 
  //Send the requested starting register                                       
  Wire.write(0x1C);   
  //Set the requested starting register                                                 
  Wire.write(0x10); 
  //End the transmission                                                   
  Wire.endTransmission(); 
                                              
  //Configure the gyro (500dps full scale)
  //Start communicating with the MPU-6050
  Wire.beginTransmission(0x68);
  //Send the requested starting register                                        
  Wire.write(0x1B);
  //Set the requested starting register                                                    
  Wire.write(0x08); 
  //End the transmission                                                  
  Wire.endTransmission();                                            
}
//======================================================================== 
void read_mpu_6050_data()
{  
  //Read the raw gyro and accelerometer data
  //Start communicating with the MPU-6050                                          
  Wire.beginTransmission(0x68);  
  //Send the requested starting register                                      
  Wire.write(0x3B);
  //End the transmission                                                    
  Wire.endTransmission(); 
  //Request 14 bytes from the MPU-6050                                  
  Wire.requestFrom(0x68,14);    
  //Wait until all the bytes are received                                       
  while(Wire.available() < 14);
  
  //Following statements left shift 8 bits, then bitwise OR.  
  //Turns two 8-bit values into one 16-bit value                                       
  acc_x = Wire.read()<<8|Wire.read();                                  
  acc_y = Wire.read()<<8|Wire.read();                                  
  acc_z = Wire.read()<<8|Wire.read();                                  
  temp = Wire.read()<<8|Wire.read();                                   
  gyro_x = Wire.read()<<8|Wire.read();                                 
  gyro_y = Wire.read()<<8|Wire.read();                                
  gyro_z = Wire.read()<<8|Wire.read();                                 
}

 ELIYAS SCIENCE INFO ©

Comments

Post a Comment

Popular posts from this blog

Arduino Timer Control Relay Devices

Image
Arduino Timer Control Relay Devices CIRCUIT DIAGRAM :  MATERIALS : Arduino Nano/UNO LiquidCrystal I2c Display Relay Module Buzzer Push Buttons BreadBoard Bulb Holder Socket Wires CODE : //Eliyas Science Info //Arduino Timer Relay - Triger relay for specific time #include <LiquidCrystal_I2C.h> //https://github.com/fdebrabander/Arduino-LiquidCrystal-I2C-library LiquidCrystal_I2C lcd(0x3F, 16, 2); // LCD HEX address 0x3F -- change according to yours #include "Countimer.h" //https://github.com/inflop/Countimer Countimer tdown; #include <EEPROM.h> #define bt_set A3 #define bt_up A2 #define bt_down A1 #define bt_start A0 int time_s = 0; int time_m = 0; int time_h = 0; int set = 0; int flag1=0, flag2=0; int relay = 5; int buzzer = 6; void setup() { Serial.begin (9600); pinMode(bt_set, INPUT_PULLUP); pinMode(bt_up, INPUT_PULLUP); pinMode(bt_down, INPUT_PULLUP); pinMode(bt_start, INPUT_PULLUP); pinMode(relay, OUTPUT); pinMode(buzze...
Read more

Arduino Tetris Game Max7219 8x8 Matrix Display

Image
Arduino Tetris Game Max7219 8x8 Matrix Display CIRCUIT DIAGRAM : MATERIALS : Arduino NANO Matrix 8x8 Display PushButton Buzzer Breadboard Jumper Wire CODE :  #include <LedControl.h>   LedControl lc = LedControl(12, 11, 10, 2); // (dataPin, clockPin, csPin, totalDevices)   int lc0[] = {0, 0, 0, 0, 0, 0, 0, 0}; int lc1[] = {0, 0, 0, 0, 0, 0, 0, 0}; long active[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; long screen[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int tmpCol = 0;     int figura = 0; int figuraNext = 0; int fromLeft = 0; int fromRight = 0; int angle = 0; int colCheck = 0; int moveCheck = 0; int score = 0; int started = 0; int lcRows = 16; int lcCols = 8; int allLines = 0; int currLines = 0; int brickDelay = 0; int defDelay = 500; int level = 0;   boolean sound = true; //Pinos   int rotate_button = 2; int left_button = 3; int down_button = 4; int right_button = 5; int start_button = 6; int ...
Read more