#include "ultrasonic.h"
#include "ledstrip.h"
#define MAX_DELTA 5 // maximum delta per iteration in cm
#define MIN_DISTANCE 0 // minimum distance in cm
#define MAX_DISTANCE 400 // maximum distance value in cm
#define MIN_LED 0 // minimum LED position
#define MAX_LED 300 // maximum LED position
#define LEDS_PER_CM 0.75 // LEDs per strip cm (e.g, 300 LEDs / 400cm)
enum direction {
NONE = 0,
LEFT = -1,
RIGHT = 1
};
enum direction travelDirection = NONE; // direction measured object travels
int measuredDistance = 0; // normalized measured distance in cm
int prevDistance = 0; // previous distance in cm for LED
int curDistance = 0; // current distance in cm for LED
int delta = 0; // difference between previous and current distance
int activeLED = 0; // index of active led
enum direction getTravelDirection(int distance) {
bool isOutOfBounds = distance < MIN_DISTANCE || distance > MAX_DISTANCE;
if (isOutOfBounds) {
return travelDirection;
}
if (distance < prevDistance) {
return LEFT;
}
if (distance > prevDistance) {
return RIGHT;
}
return NONE;
}
// returns a valid X when the sensor value is out of range
int normalizeDistance(int measuredDistance) {
if (measuredDistance >= MIN_DISTANCE && measuredDistance <= MAX_DISTANCE) {
return measuredDistance;
}
if (travelDirection > 0) {
return MAX_DISTANCE;
}
return MIN_DISTANCE;
}
int getCurrentDistance(int distance) {
delta = abs(prevDistance - distance);
if (delta > MAX_DELTA) {
delta = MAX_DELTA;
}
return prevDistance + (delta * travelDirection);
}
void setActiveLED(int distance) {
int activeLED = round(LEDS_PER_CM * curDistance);
// set leds if it's in range
if (activeLED >= MIN_LED && activeLED < MAX_LED) {
prevDistance = curDistance;
setLEDs(activeLED);
}
}
void logValues() {
Serial.print("Meassured Distance: ");
Serial.print(measuredDistance);
Serial.print(" prevDistance: ");
Serial.print(prevDistance);
Serial.print(" curDistance: ");
Serial.print(curDistance);
Serial.print(" Direction:");
if (travelDirection == LEFT) {
Serial.println(" LEFT ");
} else if (travelDirection == RIGHT) {
Serial.println(" RIGHT ");
} else {
Serial.println(" NONE ");
}
}
void setup() {
Serial.begin(9600);
setupUltraSonic();
setupLEDStrip();
delay(500);
}
void loop() {
// get sensor value
measuredDistance = getDistance();
travelDirection = getTravelDirection(measuredDistance);
curDistance = getCurrentDistance(normalizeDistance(measuredDistance));
setActiveLED(curDistance);
// logValues();
}