/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>
/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

#include <LiquidCrystal.h>

LiquidCrystal lcd(13,12,8,7,4,2);



Adafruit_MPU6050 mpu;



Servo myservo;  // create servo object to control a servo
Servo myservo2;  
int potpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin

void setup() {

  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo2.attach(11);
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
 case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
   /*case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;*/
  }
/*
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  Serial.println("");
  delay(100);
  */

}

void loop() {
 /* val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
  val = map(val, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  myservo.write(val);                  // sets the servo position according to the scaled value
  delay(15);   
  */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);



  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);



  Serial.println("");
  
  
  
  

 
  float val = g.gyro.x;            // reads the value of the potentiometer (value between 0 and 1023)
  val=val* (180 / 3.14159);
    Serial.print("Rotation X: ");
   Serial.print(val);
   
  Serial.println("");
  val = map(val,-250,250,0, 180);     // scale it to use it with the servo (value between 0 and 180)
  //val=map(val,-4.36,4.36,0,180);
  myservo.write(val);                  // sets the servo position according to the scaled value
                          // waits for the servo to get there

 //lcd.setCursor(0,1) ; //sets cursor to second line first row


lcd.begin(16, 2);
  
 // lcd.print(val);
 

float val2;
val2 = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
  val2 = map(val2, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  myservo2.write(val2);      
              // sets the servo position according to the scaled value

     lcd.setCursor(2,2); // cursor to first position first line           
  lcd.begin(16, 2);
  
  lcd.print(val2);
 
}


// Basic demo for accelerometer readings from Adafruit MPU6050