Lesson 9: Everything Together - Distance Lab

/*
  DIY KIT - Arduino Ultrasonic Distance & Parking Learning Kit
  Lesson 9 of 9: Everything Together - Distance Lab  (DistanceParkingLab.ino)

  LIBRARIES: the same as Lesson 8 (Adafruit SSD1306, with GFX and BusIO).
  They are needed to compile even if no OLED is connected.

  WIRING: everything from Lessons 1-8.
    Red LED pin 5, yellow pin 6, green pin 7 (each through 220 ohm to GND)
    Buzzer I/O pin 8, HC-SR04 TRIG pin 9 and ECHO pin 10
    OLED (optional) SDA A4, SCL A5, VCC 5V, GND

  WHAT SHOULD HAPPEN
    Green 50 cm+ (silent), yellow 20-49 cm (silent), red 10-19 cm (slow beeps),
    red under 10 cm (fast beeps). No echo: yellow blinks, buzzer silent.
    The OLED, if fitted, shows the distance, zone and sound setting.
    Serial Monitor at 9600 baud with "New Line": type help for the commands.
    A learning model only - never use it as a vehicle or safety device.
*/

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

// Set to true to test the LEDs, buzzer and OLED without the sensor:
// the distance then steps through 80, 35, 15, 6 cm and "no echo".
#ifndef DISTANCE_DIAGNOSTIC
#define DISTANCE_DIAGNOSTIC false
#endif

// The kit's pin plan - the same in every lesson.
constexpr uint8_t PIN_LED_RED = 5;
constexpr uint8_t PIN_LED_YELLOW = 6;
constexpr uint8_t PIN_LED_GREEN = 7;
constexpr uint8_t PIN_BUZZER = 8;
constexpr uint8_t PIN_TRIG = 9;
constexpr uint8_t PIN_ECHO = 10;

constexpr uint32_t SERIAL_BAUD = 9600;
constexpr uint16_t MEASURE_PERIOD_MS = 80;    // a new ping every 80 ms
constexpr uint16_t REPORT_PERIOD_MS = 250;    // a Serial line 4 times a second
constexpr uint16_t DISPLAY_PERIOD_MS = 250;
constexpr uint32_t ECHO_TIMEOUT_US = 30000UL;
constexpr uint16_t MIN_VALID_CM = 2;
constexpr uint16_t MAX_VALID_CM = 400;
// Zones (the same as Lessons 2 and 3): under 10 critical, under 20 near,
// under 50 caution, 50 or more clear.
constexpr uint16_t CRITICAL_CM = 10;
constexpr uint16_t NEAR_CM = 20;
constexpr uint16_t CLEAR_CM = 50;
constexpr uint8_t FILTER_SIZE = 5;            // median of the last 5 readings (Lesson 7)

// Most buzzer modules beep when the pin is HIGH. If yours beeps in the gaps
// or all the time, change false to true.
constexpr bool BUZZER_ACTIVE_LOW = false;

constexpr uint8_t OLED_WIDTH = 128;
constexpr uint8_t OLED_HEIGHT = 64;
constexpr int8_t OLED_RESET = -1;

enum class Zone : uint8_t {
  Unknown,
  Clear,
  Caution,
  Near,
  Critical
};

Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire, OLED_RESET);

bool oledReady = false;
uint8_t oledAddress = 0;
bool running = true;
bool soundEnabled = true;
bool buzzerState = false;

uint16_t samples[FILTER_SIZE] = {0, 0, 0, 0, 0};
uint8_t sampleCount = 0;
uint8_t sampleIndex = 0;
uint8_t invalidStreak = 0;
uint16_t latestRawCm = 0;
uint16_t filteredCm = 0;
bool measurementValid = false;
Zone currentZone = Zone::Unknown;

uint32_t nextMeasureMs = 0;
uint32_t nextReportMs = 0;
uint32_t nextDisplayMs = 0;
uint32_t nextBuzzerChangeMs = 0;

char commandBuffer[32];
uint8_t commandLength = 0;

const __FlashStringHelper *zoneName(Zone zone) {
  switch (zone) {
    case Zone::Clear: return F("CLEAR");
    case Zone::Caution: return F("CAUTION");
    case Zone::Near: return F("NEAR");
    case Zone::Critical: return F("CRITICAL");
    default: return F("NO_ECHO");
  }
}

bool i2cResponds(uint8_t address) {
  Wire.beginTransmission(address);
  return Wire.endTransmission() == 0;
}

void scanI2c() {
  uint8_t count = 0;
  Serial.println(F("I2C_SCAN_BEGIN"));
  for (uint8_t address = 1; address < 127; ++address) {
    if (i2cResponds(address)) {
      Serial.print(F("I2C_DEVICE,0x"));
      if (address < 16) Serial.print('0');
      Serial.println(address, HEX);
      ++count;
    }
  }
  Serial.print(F("I2C_SCAN_END,count="));
  Serial.println(count);
}

void initDisplay() {
  if (i2cResponds(0x3C)) oledAddress = 0x3C;
  else if (i2cResponds(0x3D)) oledAddress = 0x3D;
  else return;

  oledReady = display.begin(SSD1306_SWITCHCAPVCC, oledAddress);
  if (!oledReady) return;

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(F("Distance Learning Lab"));
  display.println(F("NOT FOR VEHICLE SAFETY"));
  display.println(F("Serial Monitor: 9600"));
  display.display();
}

uint16_t diagnosticDistanceCm() {
  const uint8_t phase = (millis() / 3000UL) % 5;
  if (phase == 0) return 80;
  if (phase == 1) return 35;
  if (phase == 2) return 15;
  if (phase == 3) return 6;
  return 0;
}

uint16_t readDistanceCm() {
  if (DISTANCE_DIAGNOSTIC) return diagnosticDistanceCm();

  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  const uint32_t durationUs = pulseIn(PIN_ECHO, HIGH, ECHO_TIMEOUT_US);
  if (durationUs == 0) return 0;

  // Approximation at room temperature: round-trip microseconds divided by 58.
  const uint16_t distanceCm = static_cast<uint16_t>(durationUs / 58UL);
  if (distanceCm < MIN_VALID_CM || distanceCm > MAX_VALID_CM) return 0;
  return distanceCm;
}

uint16_t medianSample() {
  uint16_t sorted[FILTER_SIZE];
  for (uint8_t i = 0; i < sampleCount; ++i) sorted[i] = samples[i];

  for (uint8_t i = 1; i < sampleCount; ++i) {
    const uint16_t value = sorted[i];
    int8_t j = static_cast<int8_t>(i) - 1;
    while (j >= 0 && sorted[j] > value) {
      sorted[j + 1] = sorted[j];
      --j;
    }
    sorted[j + 1] = value;
  }
  return sorted[sampleCount / 2];
}

Zone classifyDistance(uint16_t distanceCm) {
  if (!measurementValid) return Zone::Unknown;
  if (distanceCm < CRITICAL_CM) return Zone::Critical;
  if (distanceCm < NEAR_CM) return Zone::Near;
  if (distanceCm < CLEAR_CM) return Zone::Caution;
  return Zone::Clear;
}

void updateMeasurement() {
  latestRawCm = readDistanceCm();
  if (latestRawCm == 0) {
    if (invalidStreak < 255) ++invalidStreak;
    if (invalidStreak >= 3) {
      measurementValid = false;
      currentZone = Zone::Unknown;
    }
    return;
  }

  invalidStreak = 0;
  samples[sampleIndex] = latestRawCm;
  sampleIndex = (sampleIndex + 1) % FILTER_SIZE;
  if (sampleCount < FILTER_SIZE) ++sampleCount;
  filteredCm = medianSample();
  measurementValid = true;
  currentZone = classifyDistance(filteredCm);
}

void setBuzzer(bool on) {
  buzzerState = on && soundEnabled && running;
  if (BUZZER_ACTIVE_LOW) digitalWrite(PIN_BUZZER, buzzerState ? LOW : HIGH);
  else                   digitalWrite(PIN_BUZZER, buzzerState ? HIGH : LOW);
}

void updateLeds(uint32_t nowMs) {
  bool red = false;
  bool yellow = false;
  bool green = false;

  if (running) {
    if (currentZone == Zone::Critical || currentZone == Zone::Near) red = true;
    else if (currentZone == Zone::Caution) yellow = true;
    else if (currentZone == Zone::Clear) green = true;
    else {
      const uint32_t halfSeconds = nowMs / 500UL;   // no echo: blink yellow
      yellow = (halfSeconds % 2) == 0;
    }
  }

  digitalWrite(PIN_LED_RED, red ? HIGH : LOW);
  digitalWrite(PIN_LED_YELLOW, yellow ? HIGH : LOW);
  digitalWrite(PIN_LED_GREEN, green ? HIGH : LOW);
}

void updateBuzzer(uint32_t nowMs) {
  if (!running || !soundEnabled || !measurementValid ||
      (currentZone != Zone::Near && currentZone != Zone::Critical)) {
    setBuzzer(false);
    nextBuzzerChangeMs = nowMs;
    return;
  }

  if (static_cast<int32_t>(nowMs - nextBuzzerChangeMs) < 0) return;
  setBuzzer(!buzzerState);

  if (currentZone == Zone::Critical) {
    nextBuzzerChangeMs = nowMs + 100;                       // fast beeps: 100 ms on, 100 ms off
  } else {
    nextBuzzerChangeMs = nowMs + (buzzerState ? 120 : 380); // slow beeps: 120 ms on, 380 ms off
  }
}

void printStatus() {
  Serial.print(F("STATUS,running="));
  Serial.print(running ? 1 : 0);
  Serial.print(F(",sound="));
  Serial.print(soundEnabled ? 1 : 0);
  Serial.print(F(",diagnostic="));
  Serial.print(DISTANCE_DIAGNOSTIC ? 1 : 0);
  Serial.print(F(",oled="));
  Serial.print(oledReady ? 1 : 0);
  Serial.print(F(",oled_addr="));
  if (oledAddress == 0) Serial.print(F("NA"));
  else {
    Serial.print(F("0x"));
    Serial.print(oledAddress, HEX);
  }
  Serial.print(F(",trig=D"));
  Serial.print(PIN_TRIG);
  Serial.print(F(",echo=D"));
  Serial.print(PIN_ECHO);
  Serial.print(F(",buzzer=D"));
  Serial.println(PIN_BUZZER);
}

void printHelp() {
  Serial.println(F("Commands:"));
  Serial.println(F("  help     - show this list"));
  Serial.println(F("  status   - report mode, OLED, and pins"));
  Serial.println(F("  scan     - scan the optional OLED I2C bus"));
  Serial.println(F("  run      - start measurements and outputs"));
  Serial.println(F("  pause    - stop measurements and turn outputs off"));
  Serial.println(F("  sound on - enable the active-buzzer cadence"));
  Serial.println(F("  sound off- mute the buzzer; LEDs still operate"));
  Serial.println(F("  test     - brief LED and buzzer output test"));
}

void outputTest() {
  const bool previousRunning = running;
  running = false;
  setBuzzer(false);
  digitalWrite(PIN_LED_RED, HIGH);
  delay(250);
  digitalWrite(PIN_LED_RED, LOW);
  digitalWrite(PIN_LED_YELLOW, HIGH);
  delay(250);
  digitalWrite(PIN_LED_YELLOW, LOW);
  digitalWrite(PIN_LED_GREEN, HIGH);
  delay(250);
  digitalWrite(PIN_LED_GREEN, LOW);
  if (soundEnabled) {
    running = true;
    setBuzzer(true);
    delay(150);
    setBuzzer(false);
  }
  running = previousRunning;
  Serial.println(F("OUTPUT_TEST_COMPLETE"));
}

void handleCommand(char *command) {
  for (char *p = command; *p; ++p) {
    if (*p >= 'A' && *p <= 'Z') *p = *p - 'A' + 'a';
  }

  if (strcmp(command, "help") == 0) printHelp();
  else if (strcmp(command, "status") == 0) printStatus();
  else if (strcmp(command, "scan") == 0) scanI2c();
  else if (strcmp(command, "run") == 0) {
    running = true;
    Serial.println(F("MODE,RUN"));
  } else if (strcmp(command, "pause") == 0) {
    running = false;
    setBuzzer(false);
    Serial.println(F("MODE,PAUSED"));
  } else if (strcmp(command, "sound on") == 0) {
    soundEnabled = true;
    Serial.println(F("SOUND,ON"));
  } else if (strcmp(command, "sound off") == 0) {
    soundEnabled = false;
    setBuzzer(false);
    Serial.println(F("SOUND,OFF"));
  } else if (strcmp(command, "test") == 0) {
    outputTest();
  } else if (command[0] != '\0') {
    Serial.print(F("UNKNOWN_COMMAND,"));
    Serial.println(command);
    printHelp();
  }
}

void serviceSerial() {
  while (Serial.available() > 0) {
    const char incoming = static_cast<char>(Serial.read());
    if (incoming == '\n' || incoming == '\r') {
      if (commandLength > 0) {
        commandBuffer[commandLength] = '\0';
        handleCommand(commandBuffer);
        commandLength = 0;
      }
    } else if (commandLength < sizeof(commandBuffer) - 1) {
      commandBuffer[commandLength++] = incoming;
    }
  }
}

void reportMeasurement() {
  Serial.print(F("DISTANCE,raw_cm="));
  if (latestRawCm == 0) Serial.print(F("NA"));
  else Serial.print(latestRawCm);
  Serial.print(F(",median_cm="));
  if (!measurementValid) Serial.print(F("NA"));
  else Serial.print(filteredCm);
  Serial.print(F(",zone="));
  Serial.print(zoneName(currentZone));
  Serial.print(F(",buzzer="));
  Serial.println(buzzerState ? 1 : 0);
}

void updateDisplay(uint32_t nowMs) {
  if (!oledReady || static_cast<int32_t>(nowMs - nextDisplayMs) < 0) return;
  nextDisplayMs = nowMs + DISPLAY_PERIOD_MS;

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(F("DISTANCE LEARNING LAB"));
  display.println(F("NOT FOR VEHICLE SAFETY"));

  if (!running) {
    display.println(F("PAUSED"));
  } else if (!measurementValid) {
    display.println(F("Distance: NO ECHO"));
    display.println(F("Check alignment/range"));
  } else {
    display.print(F("Distance: "));
    display.print(filteredCm);
    display.println(F(" cm"));
    display.print(F("Zone: "));
    display.println(zoneName(currentZone));
  }
  display.print(F("Sound: "));
  display.println(soundEnabled ? F("ON") : F("MUTED"));
  display.display();
}

void setup() {
  pinMode(PIN_LED_RED, OUTPUT);
  pinMode(PIN_LED_YELLOW, OUTPUT);
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  digitalWrite(PIN_TRIG, LOW);
  setBuzzer(false);

  Serial.begin(SERIAL_BAUD);
  Wire.begin();
  initDisplay();

  Serial.println(F("Lesson 9: Distance Lab (DistanceParkingLab.ino)"));
  Serial.println(F("LEARNING DEMONSTRATION ONLY - NOT A VEHICLE OR INDUSTRIAL SAFETY DEVICE"));
  Serial.println(F("Distances are uncalibrated and depend on target angle, surface, air temperature, and surroundings."));
  Serial.println(F("DATA_HEADER,raw_cm,median_cm,zone,buzzer"));
  printHelp();
  printStatus();
}

void loop() {
  serviceSerial();
  const uint32_t nowMs = millis();

  if (running && static_cast<int32_t>(nowMs - nextMeasureMs) >= 0) {
    nextMeasureMs = nowMs + MEASURE_PERIOD_MS;
    updateMeasurement();
  }

  updateLeds(nowMs);
  updateBuzzer(nowMs);
  updateDisplay(nowMs);

  if (running && static_cast<int32_t>(nowMs - nextReportMs) >= 0) {
    nextReportMs = nowMs + REPORT_PERIOD_MS;
    reportMeasurement();
  }
  delay(1);   // a short pause each time round
}