FRC2016-2019-distance_bling.py

FRC2016-2019-distance_bling.py

#!/usr/bin/env python3
import board
import neopixel
import time
import VL53L0X

# Create a VL53L0X object
tof = VL53L0X.VL53L0X()
# Start ranging
tof.start_ranging(VL53L0X.VL53L0X_BETTER_ACCURACY_MODE)

# Setup NeoPixel
pixels = neopixel.NeoPixel(board.D18, 25, brightness=0.2, auto_write=True, pixel_order=neopixel.GRB)

TARGET_DISTANCE=500 # Target distance in mm
TARGET_RANGE = 50 # Allowable target range in mm
MIN_DISTANCE_CLOSE = 0
MAX_DISTANCE_FAR = 1000

TARGET_LED = 13
MAX_LED = 26
MIN_LED = 0

OUT_OF_RANGE_COLOR = (255,255,0)
FAR_COLOR = (0,0,255)
CLOSE_COLOR = (255,0,0)
TARGET_COLOR = (0,255,0)

def light_up_distance(distance):
  # If the distance is too close light up scaled number of pixels to center.
  if distance > MAX_DISTANCE_FAR:
    pixels.fill(OUT_OF_RANGE_COLOR)
    print('distance OORange: {}'.format(distance))
    return
  if distance + TARGET_RANGE < TARGET_DISTANCE:
    x = MAX_LED - int(( ((distance-MIN_DISTANCE_CLOSE) * (MAX_LED-TARGET_LED)) /
                (TARGET_DISTANCE - MIN_DISTANCE_CLOSE) ))
    print('distance NEAR: {}\nx: {}'.format(distance, x))
    pixels[TARGET_LED:x] = [CLOSE_COLOR] * (x - TARGET_LED)
    return
  if distance - TARGET_RANGE > TARGET_DISTANCE:
    x = TARGET_LED - int(( ((distance-TARGET_DISTANCE) * (TARGET_LED)) /
                (MAX_DISTANCE_FAR - TARGET_DISTANCE) ))-1
    print('distance FAR: {}\nx: {}'.format(distance, x))
    pixels[x:TARGET_LED] = [FAR_COLOR] * (TARGET_LED-x)
    return
  print('distance SUCCESS: {}\n'.format(distance))
  pixels.fill(TARGET_COLOR)

if __name__ == '__main__':
  while True:
    d = tof.get_distance()
    pixels.fill((0,0,0))
    light_up_distance(d)
    time.sleep(0.1)