Garage Parking Assistant

example.ino

/***********************************************************************
  GARAGE PARKING SENSOR
************************************************************************/

#include <Adafruit_NeoPixel.h>

#define LED_PIN        0
#define LED_COUNT      7
#define LED_BRIGHTNESS 30

#define SENSOR_BAUD    115200 
#define DEBUG_BAUD     9600

Adafruit_NeoPixel pixels(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

const uint8_t RING_LEDS[6] = {0, 5, 4, 3, 2, 1}; 
const uint8_t STOP_LEDS[3] = {2, 6, 5};          

#define DIST_OOR_MM    4950 
#define DIST_ENTRY_MM  4900 
#define DIST_STOP_MM   215  

#define DIST_HYST_MM   50   
#define WAKE_DELTA_MM  20   
#define SLEEP_TIMEOUT  120000UL 

enum State {
  STATE_IDLE,       
  STATE_ENTRY_ANIM, 
  STATE_ACTIVE,     
  STATE_STOP_ANIM,  
  STATE_SLEEP       
};

State currentState = STATE_IDLE;

uint32_t lastMovementTime = 0;
uint16_t lastSavedDist = 5000;
bool stopTriggered = false;
uint32_t animStartTime = 0;

void setup() {
  Serial.begin(DEBUG_BAUD);
  Serial1.begin(SENSOR_BAUD);
  pixels.begin();
  pixels.setBrightness(LED_BRIGHTNESS);
  pixels.clear();
  pixels.show();
  lastMovementTime = millis();
}

void loop() {
  readSensor();     
  runAnimations();  
}

void readSensor() {
  static uint8_t pkt[16];
  static uint8_t pktPos = 0;
  
  while (Serial1.available()) {
    uint8_t b = Serial1.read();
    
    if (pktPos == 0 && b != 0x57) continue;
    if (pktPos == 1 && b != 0x00) { pktPos = 0; continue; }
    if (pktPos == 2 && b != 0xFF) { pktPos = 0; continue; }
    if (pktPos == 3 && b != 0x00) { pktPos = 0; continue; }
    
    pkt[pktPos++] = b;
    
    if (pktPos >= 16) { 
      pktPos = 0;
      uint8_t sum = 0;
      for (int i = 0; i < 15; i++) sum += pkt[i];
      if (sum != pkt[15]) continue; 
      
      uint16_t dist = pkt[8] | ((uint16_t)pkt[9] << 8);
      uint16_t strength = pkt[12] | ((uint16_t)pkt[13] << 8);
      
      // Best possible < 2 or more
      if (strength < 2 || dist == 0) dist = 5000; 
      
      processDistance(dist);
    }
  }
}

uint16_t getMedian(uint16_t newDist) {
  static uint16_t history[3] = {5000, 5000, 5000};
  static uint8_t idx = 0;
  history[idx] = newDist;
  idx = (idx + 1) % 3;
  uint16_t a = history[0], b = history[1], c = history[2];
  if (a > b) { uint16_t t = a; a = b; b = t; }
  if (b > c) { uint16_t t = b; b = c; c = t; }
  if (a > b) { uint16_t t = a; a = b; b = t; }
  return b; 
}

void processDistance(uint16_t rawDist) {
  uint16_t dist = getMedian(rawDist);
  
  if (abs((long)dist - (long)lastSavedDist) > WAKE_DELTA_MM) {
    lastMovementTime = millis();
    lastSavedDist = dist;
    if (currentState == STATE_SLEEP) currentState = STATE_ACTIVE;
  }
  
  if (currentState == STATE_ACTIVE || currentState == STATE_IDLE) {
    if (millis() - lastMovementTime > SLEEP_TIMEOUT) {
      currentState = STATE_SLEEP;
      pixels.clear();
      pixels.show();
    }
  }
  
  if (currentState == STATE_SLEEP) return;
  
  if (dist >= DIST_OOR_MM) {
    if (currentState != STATE_IDLE) {
      currentState = STATE_IDLE;
      pixels.clear();
      pixels.show();
      stopTriggered = false;
    }
  } 
  else if (currentState == STATE_IDLE && dist <= DIST_ENTRY_MM) {
    currentState = STATE_ENTRY_ANIM;
    animStartTime = millis();
  }
  else if (currentState == STATE_ACTIVE) {
    
    static uint8_t spikeCounter = 0;
    
    if (dist <= DIST_STOP_MM && !stopTriggered) {

      if (lastSavedDist > 1000) {
        spikeCounter++;
        if (spikeCounter < 3) return;
      }
      
      currentState = STATE_STOP_ANIM;
      animStartTime = millis();
      stopTriggered = true;
      pixels.clear();
      pixels.show();
      spikeCounter = 0;
    } 
    else {
      if (dist > DIST_STOP_MM + DIST_HYST_MM) {
        stopTriggered = false;
      }
      spikeCounter = 0;
      updateGradient(dist);
    }

  }
}

void runAnimations() {
  if (currentState == STATE_ENTRY_ANIM) {
    uint32_t elapsed = millis() - animStartTime;
    uint8_t step = elapsed / 100; 
    if (step >= 12) {
      currentState = STATE_ACTIVE;
    } else {
      pixels.clear();
      uint8_t limit = step % 6;
      for (uint8_t i = 0; i <= limit; i++) {
        pixels.setPixelColor(RING_LEDS[i], pixels.Color(0, 255, 0));
      }
      pixels.show();
    }
  }
  else if (currentState == STATE_STOP_ANIM) {
    uint32_t elapsed = millis() - animStartTime;
    uint8_t phase = elapsed / 200; 
    if (phase >= 6) {
      currentState = STATE_ACTIVE;
    } else {
      pixels.clear();
      if (phase % 2 == 0) {
        for (int i = 0; i < 3; i++) {
          pixels.setPixelColor(STOP_LEDS[i], pixels.Color(255, 0, 0));
        }
      }
      pixels.show();
    }
  }
}

void updateGradient(uint16_t dist) {
  if (dist <= DIST_STOP_MM) {
    fillAll(pixels.Color(255, 0, 0));
  } 
  else if (dist >= DIST_ENTRY_MM) {
    fillAll(pixels.Color(0, 255, 0));
  } 
  else {
    uint16_t span = DIST_ENTRY_MM - DIST_STOP_MM; 
    uint16_t pos = dist - DIST_STOP_MM;
    uint16_t mid = span / 2;
    uint8_t r, g;
    if (pos <= mid) {
      r = 255;
      g = map(pos, 0, mid, 0, 255);
    } else {
      g = 255;
      r = map(pos, mid, span, 255, 0);
    }
    fillAll(pixels.Color(r, g, 0)); 
  }
}

void fillAll(uint32_t color) {
  for (int i = 0; i < LED_COUNT; i++) {
    pixels.setPixelColor(i, color);
  }
  pixels.show();
}