#include <LiquidCrystal.h>

#include<Wire.h>
 
#define MPU 0x68


int trigPin=7,echoPin=6;
int yesil=10, sari=9, kirmizi=8;
long sure,uzaklik;

int16_t ivmeX,ivmeY,ivmeZ,IsI,GyX,GyY,GyZ;
int mt1=4 ,mt2=5 ,mt3=3 ,mt4=2;

int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);


void setup() {
  pinMode(trigPin,OUTPUT);
  pinMode(echoPin,INPUT);
  pinMode(yesil,OUTPUT);
  pinMode(sari,OUTPUT);
  pinMode(kirmizi,OUTPUT);

  pinMode(mt1,OUTPUT);
  pinMode(mt2,OUTPUT);
  pinMode(mt3,OUTPUT);
  pinMode(mt4,OUTPUT);
  Serial.begin(9600);

  digitalWrite(mt1,LOW);
  digitalWrite(mt2,LOW);

  digitalWrite(mt3,LOW);
  digitalWrite(mt4,HIGH);
 
 
  lcd.begin(16,2);

  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0); 
  Wire.endTransmission(true);
 
}

void loop() {
  digitalWrite(trigPin,LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin,HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin,LOW); 
  
  sure = pulseIn(echoPin, HIGH, 11600);
  uzaklik= sure*0.0345/2;   
  Serial.print("mesafe:");
  Serial.print(uzaklik);
  Serial.println("  Cmdir.");

  delay(250);

  lcd.clear();                                          
  lcd.setCursor(0, 0);                                      
  lcd.print("Uzaklık:");                                
  lcd.setCursor(0, 1);                                  
  lcd.print(uzaklik);                                   
  lcd.print("cm");
  
  if(uzaklik<=20){

    digitalWrite(kirmizi,HIGH);
    digitalWrite(sari,HIGH);
    digitalWrite(yesil,HIGH);
  }
  
  else if(uzaklik<=30){

    digitalWrite(kirmizi,LOW);
    digitalWrite(sari,HIGH);
    digitalWrite(yesil,HIGH);
  }

  else if(uzaklik<=40){

    digitalWrite(kirmizi,LOW);
    digitalWrite(sari,LOW);
    digitalWrite(yesil,HIGH);
  }

  else {

    digitalWrite(kirmizi,LOW);
    digitalWrite(sari,LOW);
    digitalWrite(yesil,LOW);
  }

  verileriOku();
  
  Serial.print("ivmeX = "); Serial.print(ivmeX);
  Serial.print(" | ivmeY = "); Serial.print(ivmeY);
  Serial.print(" | ivmeZ = "); Serial.print(ivmeZ);
  Serial.print(" | Sicaklik = "); Serial.print(IsI/340.00+36.53);  
  Serial.print(" | GyroX = "); Serial.print(GyX);
  Serial.print(" | GyroY = "); Serial.print(GyY);
  Serial.print(" | GyroZ = "); Serial.println(GyZ);
  delay(510);

  digitalWrite(mt1,LOW);//mt1 sağ      
  digitalWrite(mt2,LOW);//mt2 sağ 

  digitalWrite(mt3,HIGH);//mt3 sol geri 
  digitalWrite(mt4,LOW);//mt4 sağ  ileri 
  delay(40);

}
  void verileriOku(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true); 
  
  ivmeX=Wire.read()<<8|Wire.read();   
  ivmeY=Wire.read()<<8|Wire.read(); 
  ivmeZ=Wire.read()<<8|Wire.read(); 
  IsI=Wire.read()<<8|Wire.read(); 
  GyX=Wire.read()<<8|Wire.read(); 
  GyY=Wire.read()<<8|Wire.read();  
  GyZ=Wire.read()<<8|Wire.read();


}