// Define the pins for the ultrasonic sensor
const int trig = 3;  // Trigger pin
const int echo = 2; // Echo pin

bool breakalert = false;

void setup() {
  // Initialize Serial communication
  Serial.begin(9600);

  // Define the trig as an OUTPUT
  pinMode(trig, OUTPUT);
  // Define the echo as an INPUT
  pinMode(echo, INPUT);
}

void loop() {
  float distance = measureDistance();
  if (distance<=50){
    stopMotors();
    HextoPC(0xFB);
    Serial.println("Auto Breaking Enabled");
  }
  else if (distance>50 && !breakalert){
    HextoPC(0xFC);
    Serial.println("Auto Breaking Disabled");
    breakalert=true;
  }
}


float measureDistance() {
  // pulse to trigger the ultrasonic sensor
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  // Measure the time it takes for the echo to return
  float duration = pulseIn(echo, HIGH);

  float distance = duration/58;
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
  delay(1000);
  return distance;
}

void stopMotors() {
 Serial.println("Motors Stopped");
}

void HextoPC(byte hexValue) {
  Serial.println(hexValue, HEX);
}