Arduino spirit Level using gyroscope with LED

Arduino spirit Level using gyroscope with 11-LED

Arduino spirit Level using gyroscope with LED


Parts list:


Circuit:
Arduino spirit Level using gyroscope with LED


Video:



Code:

#include <Wire.h>

//Declaring some global variables

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;


long loop_timer;

int temp;


void setup() {

  pinMode(3,OUTPUT);
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(11,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);

  Wire.begin();                                                        //Start I2C as master

  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050

  for (int cal_int = 0; cal_int < 1000 ; cal_int ++){                  //Read the raw acc and gyro data from the MPU-6050 for 1000 times

    read_mpu_6050_data();                                           

    gyro_x_cal += gyro_x;                                              //Add the gyro x offset to the gyro_x_cal variable

    gyro_y_cal += gyro_y;                                              //Add the gyro y offset to the gyro_y_cal variable

    gyro_z_cal += gyro_z;                                              //Add the gyro z offset to the gyro_z_cal variable

    delay(3);                                                          //Delay 3us to have 250Hz for-loop

  }


  // divide by 1000 to get avarage offset

  gyro_x_cal /= 1000;                                               

  gyro_y_cal /= 1000;                                               

  gyro_z_cal /= 1000;                                               

  Serial.begin(115200);

  loop_timer = micros();                                               //Reset the loop timer

}


void loop(){


  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)

  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable

  angle_roll += gyro_y * 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_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel

  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel



  //Accelerometer angle calculations

  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector

  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians

  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;       //Calculate the pitch angle

  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       //Calculate the roll angle



  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch

  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll


  if(set_gyro_angles){                                                 //If the IMU is already started

    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle

    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle

  }

  else{                                                                //At first start

    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle

    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle

    set_gyro_angles = true;                                            //Set the IMU started flag

  }



  //To dampen the pitch and roll angles a complementary filter is used

  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value

  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

  Serial.print(" | Angle  = "); Serial.println(angle_pitch_output);
if(angle_pitch_output > 0 && angle_pitch_output < 5)
{
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);

}
else if(angle_pitch_output >= 5 && angle_pitch_output <= 20)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,HIGH);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output >= 20 && angle_pitch_output <= 35)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,HIGH);
  digitalWrite(7,HIGH);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output >= 35 && angle_pitch_output <= 50)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);
  digitalWrite(6,HIGH);
  digitalWrite(7,HIGH);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output >= 50 && angle_pitch_output <= 65)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,HIGH);
  digitalWrite(5,HIGH);
  digitalWrite(6,HIGH);
  digitalWrite(7,HIGH);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output >= 65 && angle_pitch_output <= 90)
  {
  digitalWrite(3,HIGH);
  digitalWrite(4,HIGH);
  digitalWrite(5,HIGH);
  digitalWrite(6,HIGH);
  digitalWrite(7,HIGH);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }

  else if(angle_pitch_output <= 0 && angle_pitch_output >= -5)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }

else if(angle_pitch_output <= -5 && angle_pitch_output >= -20)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,HIGH);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output <= -20 && angle_pitch_output >= -35)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,HIGH);
  digitalWrite(10,HIGH);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output <= -35 && angle_pitch_output >= -50)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,HIGH);
  digitalWrite(10,HIGH);
  digitalWrite(11,HIGH);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output <= -50 && angle_pitch_output >= -65)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,HIGH);
  digitalWrite(10,HIGH);
  digitalWrite(11,HIGH);
  digitalWrite(12,HIGH);
  digitalWrite(13,LOW);
  }
else if(angle_pitch_output <= -65 && angle_pitch_output >= -90)
  {
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(9,HIGH);
  digitalWrite(10,HIGH);
  digitalWrite(11,HIGH);
  digitalWrite(12,HIGH);
  digitalWrite(13,HIGH);
  }


 while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop

 loop_timer = micros();//Reset the loop timer



}





void setup_mpu_6050_registers(){

  //Activate the MPU-6050

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x6B);                                                    //Send the requested starting register

  Wire.write(0x00);                                                    //Set the requested starting register

  Wire.endTransmission();                                           

  //Configure the accelerometer (+/-8g)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1C);                                                    //Send the requested starting register

  Wire.write(0x10);                                                    //Set the requested starting register

  Wire.endTransmission();                                           

  //Configure the gyro (500dps full scale)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1B);                                                    //Send the requested starting register

  Wire.write(0x08);                                                    //Set the requested starting register

  Wire.endTransmission();                                           

}



void read_mpu_6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x3B);                                                    //Send the requested starting register

  Wire.endTransmission();                                              //End the transmission

  Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050

  while(Wire.available() < 14);                                        //Wait until all the bytes are received

  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();    

                    
}

Komentar

Posting Komentar

Postingan populer dari blog ini

Using ultrasonic sensor without arduino

Gambar
Using ultrasonic sensor without arduino Parts list: IC 555 IC KA2284 LED(5pcs) Ultrasonic receiver & transmitter Resistor 1K Resistor 2.2K Resistor 10K Resistor 100 ohm Potentiometer 10K (2pcs) Capacitor 10uF (4pcs) Capacitor 3.3nF Transistor 2N2222 Buzzer There are 2 ways to using Ultrasonic sensor without microcontroller: First way, the buzzer turns ON when you put your hand near the sensor   Second way the LED bar show you the distance level between the sensor and the objects. Video: More information in the video
Baca selengkapnya

Power saving relay driver

Gambar
Relay coil after switch ON needs to voltage much lower than its nominal voltage,  We can use this property of relay to saving 85% of the power to increase the battery life in battery powered circuits. To reduce the current / power consumption we can put a resistor through the relay coil, but since this low current not enough to running the relay at the beginning so we should short out the resistor for 1 second, then the relay switch keep going.  You have to use this formula to calculate the suitable resistor value: For more explanations please watch this video:   
Baca selengkapnya

Electronic load using Op-Amp IC 358

Gambar
Electronic load using Op-Amp IC 358 In this circuit i using IC 358 and some few components to make a simple constant load.  The trick here is connecting a dc voltmeter to the non-inverting input of the op-amp to preset the current before connect it to the battery or the super cap. under test. For more explanation you can watch the video below. Parts list: LM358 : https://www.banggood.com/custlink/Dvv3hO51Df High power resistor: https://www.banggood.com/custlink/K3GvYjHwRm Digital DC Voltmeter: https://www.banggood.com/custlink/3vmDYL5TDU Copper board: https://www.banggood.com/custlink/3KDDmMsPSc MOSFET IRFZ44N : https://goo.gl/35n5Tb   Potentiometer 100K : https://goo.gl/dKnWCj Resistor kit (10 Ohm): https://goo.gl/TgSaff Heatsink: https://www.banggood.com/custlink/D3KGdOSmzf   Circuit:   Gerber file: Download the Gerber file from here:  https://drive.google.com/open?id=1Lx8xjfkyYPvvzAbA0ponFBYBTQDqrMvU Video: For more explanation you can watch the video below 
Baca selengkapnya