Ultrasonic Parking Sensor

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define TRIG_PIN 27
#define ECHO_PIN 26
#define GREEN_LED_PIN 14
#define YELLOW_LED_PIN 12
#define RED_LED_PIN 13
#define BUZZER_PIN 25
#define SDA_PIN 21
#define SCL_PIN 22
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

const int SAFE_DISTANCE_CM = 70;
const int CAUTION_DISTANCE_CM = 35;
const int STOP_DISTANCE_CM = 18;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
float smoothedDistance = 100;
unsigned long lastBeepMs = 0;
bool buzzerOn = false;

float readDistanceCm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  if (duration == 0) return smoothedDistance;
  return duration / 58.0;
}

void setOutputs(float distanceCm) {
  bool safe = distanceCm > SAFE_DISTANCE_CM;
  bool caution = distanceCm <= SAFE_DISTANCE_CM && distanceCm > CAUTION_DISTANCE_CM;
  bool warning = distanceCm <= CAUTION_DISTANCE_CM;

  digitalWrite(GREEN_LED_PIN, safe ? HIGH : LOW);
  digitalWrite(YELLOW_LED_PIN, caution ? HIGH : LOW);
  digitalWrite(RED_LED_PIN, warning ? HIGH : LOW);

  if (!warning) {
    noTone(BUZZER_PIN);
    buzzerOn = false;
    return;
  }

  int interval = distanceCm <= STOP_DISTANCE_CM ? 120 : 320;
  if (millis() - lastBeepMs > interval) {
    lastBeepMs = millis();
    buzzerOn = !buzzerOn;
    if (buzzerOn) tone(BUZZER_PIN, distanceCm <= STOP_DISTANCE_CM ? 1800 : 1200);
    else noTone(BUZZER_PIN);
  }
}

void drawDisplay(float distanceCm) {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("Parking sensor");
  display.setTextSize(2);
  display.setCursor(0, 18);
  display.print(distanceCm, 0);
  display.println(" cm");
  display.setTextSize(1);
  display.setCursor(0, 48);
  if (distanceCm <= STOP_DISTANCE_CM) display.println("STOP");
  else if (distanceCm <= CAUTION_DISTANCE_CM) display.println("Slow down");
  else if (distanceCm <= SAFE_DISTANCE_CM) display.println("Getting close");
  else display.println("Clear");
  display.display();
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  Wire.begin(SDA_PIN, SCL_PIN);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
}

void loop() {
  float reading = readDistanceCm();
  smoothedDistance = (smoothedDistance * 0.75) + (reading * 0.25);
  setOutputs(smoothedDistance);
  drawDisplay(smoothedDistance);
  delay(80);
}