Blog

#include <I2Cdev.h>
#include <MPU6050.h>

MPU6050 mpu;

// Timers
unsigned long timer = 0;
float timeStep = 0.01;

// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;

const int RELAY_PIN_1 = 5;
const int RELAY_PIN_2 = 6;

void setup()
{
  pinMode(RELAY_PIN_1, OUTPUT);
  pinMode(RELAY_PIN_2, OUTPUT);
  digitalWrite(RELAY_PIN_1, HIGH);
  digitalWrite(RELAY_PIN_2, HIGH);
  Serial.begin(115200);

  // Initialize MPU6050
  while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }

  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  //  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  mpu.setThreshold(3);
}

void loop()
{
  timer = millis();

  // Read normalized values
  Vector norm = mpu.readNormalizeGyro();

  // Calculate Pitch and Roll
  pitch = pitch + norm.YAxis * timeStep;
  roll = roll + norm.XAxis * timeStep;

  int flag_1 = 0;
  int flag_2 = 0;
  int flag_3 = 0;
  int flag_4 = 0;
  if (pitch > 40 )  {
    digitalWrite(RELAY_PIN_1, LOW);
  }
if(pitch<=-20){
  digitalWrite(RELAY_PIN_1,HIGH);
}
if (roll > 40 )  {
    digitalWrite(RELAY_PIN_2, LOW);
  }
if(roll<=-20){
  digitalWrite(RELAY_PIN_2,HIGH);
}
  /*
    if (pitch  45)
    {
      if (flag_3 == 0)
      {
        flag_3 = 1;
        digitalWrite(RELAY_PIN_1, HIGH);
      }
      else {
        flag_3 = 0;
        digitalWrite(RELAY_PIN_1, LOW);
      }
    }
    if (roll &lt; -45)
    {
      if (flag_4 == 0)
      {
        flag_4 = 1;
        digitalWrite(RELAY_PIN_1, HIGH);
      }
      else {
        flag_4 = 0;
        digitalWrite(RELAY_PIN_1, LOW);
      }
    }
  */
  //print the pitch and roll
  Serial.print(" Pitch = ");
  Serial.print(pitch);
  Serial.print(" Roll = ");
  Serial.println(roll);

  // Wait to full timeStep period
  delay((timeStep * 1000) - (millis() - timer));
}