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

#define LED_BUILTIN 11
#define LED_BUILTIN 12 
#define trigPin 4
#define echoPin 5



Adafruit_MPU6050 mpu;   //This one goes on palm
Servo servo1;
Servo servo2;
Servo servo3;

int pos1 = 90;
int pos2 = 90;
const int ledg (11);
const int ledr (12);
const int def = 0.78;
int val;    
long duration;
int distance;
int sz;
int sx;
int swr;
int rv;
LiquidCrystal lcd(14, 15, 16, 17, 18, 19);
int tick;


void setup(void) {
  servo1.attach(8);
  servo2.attach(9);
  servo3.attach(7);
  Serial.begin(115200);
  pinMode(2, INPUT);   // Connect HX710 OUT to Arduino pin 2
  pinMode(3, OUTPUT);  // Connect HX710 SCK to Arduino pin 3
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input


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

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);


  delay(20);
}

void loop() {
   
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // wait for the current reading to finish
  while (digitalRead(2)) {}

  // read 24 bits
  long result = 0;
  for (int i = 0; i < 24; i++) {
    digitalWrite(3, HIGH);
    digitalWrite(3, LOW);
    result = result << 1;
    if (digitalRead(2)) {
      result++;
    }
  }

  // get the 2s compliment
  result = result ^ 0x800000;

  // pulse the clock line 3 times to start the next pressure reading
  for (char i = 0; i < 3; i++) {
    digitalWrite(3, HIGH);
    digitalWrite(3, LOW);
  }
result = map(result,8388608,8389658,0,5);
//=======================================================
  
if (result>=1) {                                     //system armed
g.gyro.z *= 100;                                        //servo z postion
if(g.gyro.z >= 0 && 314.16 >= g.gyro.z){
  sz = map(g.gyro.z,0,314.16,0,180);
}
if (g.gyro.z < 0){                                            //servo z correction
  sz = 0;}
  if (g.gyro.z > 314.16){
  sz = 180;}

g.gyro.x *= 100;                                      //servo x postion  
 if (g.gyro.x >= -17.45 && 148.35 >= g.gyro.x){
    sx = map(g.gyro.x,-17.45,148.35,100,10);
    }
if (g.gyro.x < -17.45){                                     //servo x correction
  sx = 100;}
  if (g.gyro.x > 148.35){
  sx = 10;}
  servo1.write(sx);
  servo2.write(sz);

}


//======================================================== rgb led color system=======================
if (result < 1){digitalWrite(13, HIGH);
                digitalWrite(12, LOW);
                digitalWrite(11, LOW);}  

if (result >= 1 && result < 2){digitalWrite(13, LOW);
                              digitalWrite(12, HIGH);
                              digitalWrite(11, HIGH);}

if (result >= 2 && result < 3){digitalWrite(13, HIGH);
                               digitalWrite(12, HIGH);
                               digitalWrite(11, LOW);}

if (result >= 3){digitalWrite(13, HIGH);
                 digitalWrite(12, HIGH);
                 digitalWrite(11, HIGH);}


if (result >= 3 && swr!=1){         //relay logic
  digitalWrite(10, HIGH);
  swr = 1;
}
if (swr==1){
  delay(500);
  digitalWrite(10, LOW);
}
if (result < 3){
swr=0;
}
//============================================== Ultrasonic sensor


// Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2;
  // Prints the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print("cm");
  Serial.print("   Force:");
  Serial.print(result);
  Serial.print("kg");
  Serial.print ("   gyro z:");
  Serial.print(g.gyro.z);
  Serial.print("   servo z:");
  Serial.print(sz);
  Serial.print ("   gyro x:");
  Serial.print(g.gyro.x);
  Serial.print("   servo x:");
  Serial.print(sx);
  Serial.print("  ");
  Serial.print(swr);


  val = distance;            // reads the value of the potentiometer (value between 0 and 1023)
  val = map(val, 400, 0, 70, 90);     // scale it to use it with the servo (value between 0 and 180)
  servo3.write(val);   
                 // sets the servo position according to the scaled value

//=============================================================================lcd==========

lcd.begin(16, 2);
  // you can now interact with the LCD, e.g.:
  uint8_t degree[8] = {                                 //custom character                                
  0b11100,
  0b10100,
  0b11100,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  };
  lcd.createChar(1, degree);

  
  tick++;
if (tick >= 100){
  tick=0;
}
  Serial.print("  ");
  Serial.println(tick);

switch (tick){
case 1 ... 33:
  lcd.setCursor(0, 0) ;
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print("cm");
  lcd.setCursor(0, 1);
  lcd.print("Force:    ");
  lcd.print(result);
  lcd.print("kg");
  break;
case 34 ... 66:
  lcd.setCursor(0, 0) ;
  lcd.print("Servo z: ");
  lcd.print(sz);
  lcd.print("\x01");
  lcd.setCursor(0, 1);
  lcd.print("Servo x: ");
  lcd.print(sx);
  lcd.print("\x01");
  break;
case 67 ... 100:
  lcd.setCursor(0, 0);
  lcd.print("Servo auto: ");
  lcd.print(89-val);
  lcd.print("\x01");
  }

  delay(10);
  }