mirror of
https://github.com/undera/pylgbst.git
synced 2020-11-18 19:37:26 -08:00
16b1612cc6
* Cosmetics * Harmonograph demo * Cleanup * Original * Original * Cosmetics * Original file * Fixes * cosmetics * separate classes * Cosmetics * Cosmetics * fix tests * Remove plotter tests * Add bluegiga * Rename it * Progress * Fix tests * Cosmetics * Found a way for pygatt! * Playing with gatt * Fix hung subscribe * rename class * add test * skeleton for autodetect * safer order * Fix tests * Fix test * Add dbus install * another try * 2 * 3 * 34 * 6 * 7 * Isolate some tests * 8 * back to roots * Try more * 9 * Help * rep * site-packs * Fix? * Py3 come on * dbus * busss * dev null! * Fix test * Cleanup * Fix tests * Fix after review * add package * FIx package paths * Cosmetics * Update * More doc
67 lines
1.5 KiB
Python
67 lines
1.5 KiB
Python
from pylgbst.peripherals import ColorDistanceSensor
|
|
from . import *
|
|
|
|
logging.basicConfig(level=logging.INFO)
|
|
|
|
robot = Vernie()
|
|
running = True
|
|
criterion = min
|
|
|
|
cur_luminosity = 0
|
|
|
|
|
|
def on_change_lum(lumn):
|
|
global cur_luminosity
|
|
cur_luminosity = lumn
|
|
|
|
|
|
lum_values = {}
|
|
|
|
|
|
def on_btn(pressed):
|
|
global running
|
|
if pressed:
|
|
running = False
|
|
|
|
|
|
def on_turn(angl):
|
|
lum_values[angl] = cur_luminosity
|
|
|
|
|
|
robot.button.subscribe(on_btn)
|
|
robot.color_distance_sensor.subscribe(on_change_lum, ColorDistanceSensor.LUMINOSITY, granularity=1)
|
|
robot.motor_A.subscribe(on_turn, granularity=30)
|
|
|
|
# TODO: add bump detect to go back?
|
|
|
|
while running:
|
|
# turn around, measuring luminosity
|
|
lum_values = {}
|
|
robot.turn(RIGHT, degrees=360, speed=0.2)
|
|
|
|
# get max luminosity angle
|
|
amin = min(lum_values.keys())
|
|
lmax = max(lum_values.values())
|
|
almax = amin
|
|
for almax in lum_values:
|
|
if lum_values[almax] == lmax:
|
|
break
|
|
|
|
angle = int((almax - amin) / VERNIE_TO_MOTOR_DEGREES)
|
|
logging.info("Angle to brightest %.3f is %s", lmax, angle)
|
|
|
|
# turn towards light
|
|
if angle > 180:
|
|
robot.turn(LEFT, degrees=360 - angle)
|
|
else:
|
|
robot.turn(RIGHT, degrees=angle)
|
|
|
|
# Now let's move until luminosity changes
|
|
lum = cur_luminosity
|
|
while cur_luminosity >= lum:
|
|
logging.info("Luminosity is %.3f, moving towards it", cur_luminosity)
|
|
robot.move(FORWARD, 1)
|
|
|
|
robot.color_distance_sensor.unsubscribe(on_change_lum)
|
|
robot.button.unsubscribe(on_btn)
|