def main(map_station_mp3_color, station):
global exit_thread
global stop_playing
global start_playing
pr('play_music.main: Starting thread')
setup()
try:
while 42:
while not start_playing:
time.sleep(0.1)
if exit_thread:
pr('play_music.main: Exit thread')
return
try:
pr('Trying to find fn to play %s' % str(start_playing))
fn = get_mp3_filename(map_station_mp3_color, station,
start_playing)
fn = DEF_PATH_MP3 + convert_fn(fn)
play(fn)
if exit_thread:
pr('play_music.main: Exit thread')
return
time.sleep(1)
except MP3FileError as e:
pr(e)
pass
start_playing = False
except KeyboardInterrupt:
pass
def load(fname):
if fname is None:
pr('No config file given. Using default')
return DEF_CONFIG
pr('Trying to load config file: %s' % fname)
with open(fname, 'r') as infile:
return yaml.load(infile)
def setup():
pygame.init()
pygame.mixer.init()
m = alsaaudio.Mixer(DEF_AUDIO_DEVICE)
vol_before = m.getvolume()[0]
m.setvolume(100)
vol_after = m.getvolume()[0]
pr('Setting Alsa volume for %s to %d (was: %d)' %
(DEF_AUDIO_DEVICE, vol_after, vol_before))
def rgb_stable(config_rgb):
rgb_stable_cnt = config_rgb['stable_cnt']
rgb_stable_dist = config_rgb['stable_dist']
pr('Starting rgb_stable with stable count: %5d, stable_dist: %5d' %
(rgb_stable_cnt, rgb_stable_dist))
led_on()
while 42:
res = get_stable_rgb(rgb_stable_cnt, rgb_stable_dist)
prdbg(res)
pr('RGB: %25s' % str(res))
def setup(config):
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_LED, GPIO.OUT)
time.sleep(0.05)
global tcs
integration_time = config['integration_time'][0]
gain = config['gain'][0]
pr('Setting TCS config: Integration time: %5d Gain: %5d' %
(integration_time, gain))
tcs = TCS34725.TCS34725(integration_time=integration_time,
gain=gain,
i2c=1)
def app2(config_det, config_rgb, config_color, map_station_mp3_color):
global tcs
global log_rgb_exit
det_threshold = config_det['threshold']
rgb_stable_cnt = config_rgb['stable_cnt']
rgb_stable_dist = config_rgb['stable_dist']
rgb_max_dist = config_rgb['max_dist']
pr('Starting with detection threshold: %d' % det_threshold)
pr('Starting color detection with stable count: %d, stable_dist: %d using max distance: %d'
% (rgb_stable_cnt, rgb_stable_dist, rgb_max_dist))
check_color_vs_map_color_mp3(config_color, map_station_mp3_color)
check_map_color_mp3_vs_color(config_color, map_station_mp3_color)
check_mp3_files(map_station_mp3_color)
try:
station = get_station()
except UndefinedStation as e:
prerr('UndefinedStationError: %s . Exiting ...' % e)
return
# Start play_music and rbg_log threads
pm = threading.Thread(target=play_music.main,
name='play_music.main',
args=(map_station_mp3_color, station))
pm.start()
rgb_log = threading.Thread(target=log_rgb, name='log_rgb')
rgb_log.start()
try:
while 42:
detect_cube(det_threshold)
led_on()
res = get_stable_rgb(rgb_stable_cnt, rgb_stable_dist)
color = get_color(res, config_color, rgb_max_dist)
if not pm.is_alive():
prerr('Thread %s unexpectedly died. Exiting...' % pm.getName())
break
if not rgb_log.is_alive():
prerr('Thread %s unexpectedly died. Exiting...' %
rgb_log.getName())
break
if color is not None:
play_music.stop_playing = False
play_music.start_playing = color[0]
else:
pr('Max RGB color distance exceeded. Not playing...')
detect_cube_removal(det_threshold)
play_music.stop_playing = True
except KeyboardInterrupt:
pr('Keyboard interrupt detected, Stopping threads ..')
finally:
play_music.exit_thread = True
log_rgb_exit = True
pm.join(3)
rgb_log.join(3)
def check_mp3_files(map_station_mp3_color):
pr('Check mp3 files in map station mp3 color')
errors = 0
for station, fn, color in map_station_mp3_color:
fn = convert_fn(fn)
path = DEF_PATH_MP3 + fn
if not (os.path.isfile(path)):
raise MP3FileError('File %s does not exist' % path)
res = check_valid_mp3_content(path)
if 'result' not in res or res['result'] != 'Ok':
errors += 1
prwarn('File %s has errors: %s' % (path, str(res)))
if errors == 0:
pr('Check passed. OK')
else:
prwarn('%d errors found.' % errors)
return errors
def diff():
global tcs
while 42:
led_on()
r, g, b, c = measure()
prdbg('R: %5d G: %5d B: %5d C: %5d' % (r, g, b, c))
led_off()
r2, g2, b2, c2 = measure()
prdbg('R: %5d G: %5d B: %5d C: %5d' % (r2, g2, b2, c2))
rgb = (r, g, b)
rgb2 = (r2, g2, b2)
rgb_len = get_rgb_length(rgb)
rgb2_len = get_rgb_length(rgb2)
rgb_diff = get_rgb_distance(rgb, rgb2)
clear_diff = abs(c - c2)
pr('Len %5d %5d %5d Clear: %5d %5d %5d' %
(rgb_len, rgb2_len, rgb_diff, c, c2, clear_diff))
def get_station():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
for gpio in DEF_STATION_GPIOS:
GPIO.setup(gpio, GPIO.IN, pull_up_down=GPIO.PUD_UP)
gpios_in = []
for gpio in DEF_STATION_GPIOS:
gpios_in.append(GPIO.input(gpio))
for gpios, station in DEF_STATION_GPIO_MAP:
prdbg('Trying to match stations: %s %s %s' %
(str(gpios_in), str(gpios), str(station)))
if tuple(gpios_in) == gpios:
pr('Found station: %d' % station)
return station
raise UndefinedStation('For GPIOs %s no station defined' % str(gpios_in))
def main():
args = docopt(__doc__, version='LED Sensing')
# prdbg(args)
# Check if we want also to log to a file
if args['-l'] is not None:
filehandler = logging.FileHandler(args['-l'])
filehandler.setFormatter(logFormatter)
rootLogger.addHandler(filehandler)
config = load(args['CONFIG'])
# pprint.pprint(config)
setup(config['sensor'])
try:
if args['app']:
app(config['det'], config['rgb'], config['color'])
elif args['app2']:
app2(config['det'], config['rgb'], config['color'],
config['map_station_mp3_color'])
elif args['cal'] and not args['analysis']:
cal(config['det'], config['rgb'], config['color'],
config['sensor'], args['-c'])
elif args['cal'] and args['analysis']:
cal_analysis(args['FILES'])
elif args['color'] and args['analysis']:
color_analyse(config['color'])
elif args['detect']:
detect(config['det'])
elif args['diff']:
diff()
elif args['meas']:
meas(args['on'], args['toggle'])
elif args['rgb'] and args['stable']:
rgb_stable(config['rgb'])
elif args['save_default']:
save_default()
elif args['test_speed']:
test_speed()
else:
pr('Not implemented')
except KeyboardInterrupt:
endprogram()
def check_color_vs_map_color_mp3(config_color, config_map_color_mp3):
"""
Check if all colors defined in config_color are also available for every station in config_map_color_mp3
:param config_color: configuration (color, RGB)
:param config_map_color_mp3: configuration (color, station, MP3_fn)
:return: number of warnings
"""
pr('Check colors from config colors are in map_color_mp3')
warn = 0
for color_name, color_rgb in config_color:
stations = []
found = 0
for station, mp3_fn, map_color_name in config_map_color_mp3:
# prdbg('Color %s, Map: %s' % (str(color_name), str(map_color_name)))
if color_name == map_color_name:
# prdbg('Found color %s in map: %s for station %d' % (str(color_name), str(map_color_name), station))
stations.append(station)
found += 1
if found == 0:
prwarn('Color %s not found' % (str(color_name)))
warn += 1
else:
pr('Found color %30s for station: %s' %
(str(color_name), str(stations)))
if warn == 0:
pr('Check passed. OK')
return warn
prwarn('Check failed. %d warnings occured' % warn)
return warn
def meas(conf_led_on, toggle):
global tcs
dd = DrawDiagram(40)
if toggle:
while 42:
led_on()
for _ in range(3):
r, g, b, c = measure()
dd.add(r)
pr('R: %5d G: %5d B: %5d C: %5d | %-40s' %
(r, g, b, c, dd.getstr()))
led_off()
for _ in range(3):
r, g, b, c = measure()
dd.add(r)
pr('R: %5d G: %5d B: %5d C: %5d | %-40s' %
(r, g, b, c, dd.getstr()))
led(conf_led_on)
while 42:
r, g, b, c = measure()
pr('R: %5d G: %5d B: %5d C: %5d' % (r, g, b, c))
def get_color(rgb, colors, max_rgb_dist):
""" Takes an RGB list as argument and matches against DEF_COLORS the closest
will be uses as match. Returns None if match distance is larger than
max_rgb_distance.
"""
min_dist = 999999999
match = 0
for color in colors:
color_rgb = color[1]
dist = get_rgb_distance(rgb, color_rgb)
if dist < min_dist:
min_dist = dist
match = color
pr('Found %-15s - Dist %d - Cur. RGB: %-20s RGB %-20s' %
(match[0], min_dist, str(rgb), str(match[1])))
if min_dist > max_rgb_dist:
prdbg('Max RGB color dist: %d Dist Limit: %d Exiting... ' %
(min_dist, max_rgb_dist))
return None
return match[0], match[1], min_dist
def check_map_color_mp3_vs_color(config_color, config_map_color_mp3):
"""
Check if all colors defined in config_map_color_mp3 are also available for every station in config_color
:param config_color: configuration (color, RGB)
:param config_map_color_mp3: configuration (color, station, MP3_fn)
:return: number of warnings
"""
pr('Check colors from map_color_mp3 are in config_color')
warn = 0
# Get all available stations
stations = []
for station, mp3_fn, map_color_name in config_map_color_mp3:
if station not in stations:
stations.append(station)
# pr('Stations: %s' % str(stations))
for station in stations:
config_map_color_mp3_part = []
# Extract config for a single station
for map_station, mp3_fn, map_color_name in config_map_color_mp3:
if map_station == station:
config_map_color_mp3_part.append(map_color_name)
for map_color_name in config_map_color_mp3_part:
found = 0
for color_name, color_rgb in config_color:
# prdbg('Color %s, Map: %s' % (str(color_name), str(map_color_name)))
if color_name == map_color_name:
# prdbg('Found color %s in map: %s for station %d' %
# (str(color_name), str(map_color_name), map_station))
found += 1
if found == 0:
prwarn('Color %30s not found for station: %d' %
(str(map_color_name), station))
warn += 1
if warn == 0:
pr('Check passed. OK')
return warn
prwarn('Check failed. %d warnings occured' % warn)
return warn
def log_rgb():
global log_rgb_exit
log_rgb_exit = False
pr('log_rgb: Starting thread')
while 42:
pr('RGBC : %s' % str(last_rgb_measurement))
# Split wait time into smaller steps to make exiting thread more responsive
steps = 100
for i in range(steps):
sleep_time = (1.0 * LOG_RGB_INT) / (1.0 * steps)
time.sleep(sleep_time)
if log_rgb_exit:
pr('log_rgb: Exit thread')
return
def play(fn):
global stop_playing
pygame.mixer.music.load(fn)
pygame.mixer.music.set_volume(1) # Set to max
pr('Playing %s with volume %d' %
(fn, 100 * pygame.mixer.music.get_volume()))
pygame.mixer.music.play()
while pygame.mixer.music.get_busy():
if stop_playing or exit_thread:
pr('Stopping to play')
pygame.mixer.music.stop()
stop_playing = False
return
pygame.time.Clock().tick(10)
pr('Finished playing')
def app(config_det, config_rgb, config_color):
global tcs
det_threshold = config_det['threshold']
rgb_stable_cnt = config_rgb['stable_cnt']
rgb_stable_dist = config_rgb['stable_dist']
rgb_max_dist = config_rgb['max_dist']
pr('Starting app with detection threshold: %d' % det_threshold)
pr('Strating color detection with stable count: %d, stable_dist: %d using max distance: %d'
% (rgb_stable_cnt, rgb_stable_dist, rgb_max_dist))
while 42:
detect_cube(det_threshold)
led_on()
res = get_stable_rgb(rgb_stable_cnt, rgb_stable_dist)
# print(res)
color = get_color(res, config_color, rgb_max_dist)
pr('%-15s - Distance: %5d - Cur. RGB: %-25s RGB %-20s' %
(color[0], color[2], str(res), str(color[1])))
detect_cube_removal(det_threshold)
def save_default():
pr('Saving default config to %s' % DEF_CONFIG_FN)
with open(DEF_CONFIG_FN, 'w') as outfile:
yaml.dump(DEF_CONFIG, outfile, indent=4)
def cal(config_det, config_rgb, config_color, config_sensor, cnt):
det_threshold = config_det['threshold']
rgb_stable_cnt = config_rgb['stable_cnt']
rgb_stable_dist = config_rgb['stable_dist']
rgb_max_dist = config_rgb['max_dist']
cnt = int(cnt)
pr('Starting with detection threshold: %d' % det_threshold)
pr('Starting color detection with stable count: %d, stable_dist: %d using max distance: %d'
% (rgb_stable_cnt, rgb_stable_dist, rgb_max_dist))
try:
station = get_station()
except UndefinedStation as e:
prerr('UndefinedStationError: %s . Exiting ...' % e)
return
res = {}
# Init dict
for color_name, config_rgb in config_color:
res[color_name] = {}
res[color_name]['config'] = config_rgb
res[color_name]['values'] = []
res[color_name]['mean'] = [0, 0, 0]
res[color_name]['std'] = [0, 0, 0]
# Do calibration loop
first_run = True
last_color_rgb = [0, 0, 0]
for color_name, config_rgb in config_color:
color_changed = False
print('Put color %s on detector' % color_name)
for cycle in range(cnt):
while 42:
detect_cube(det_threshold)
led_on()
rgb = get_stable_rgb(rgb_stable_cnt, rgb_stable_dist)
pr('Remove cube')
detect_cube_removal(det_threshold)
# Check if the cube color has really changed
dist_last_rgb = get_rgb_distance(last_color_rgb, rgb)
if first_run:
first_run = False
color_changed = True
break
if color_changed:
break
# TODO: Adjust threshold
if dist_last_rgb > 300:
color_changed = True
break
prwarn(
'Please use the correct color %s, it seems you still using the old one. Dist: %d'
% (color_name, dist_last_rgb))
dist_config = get_rgb_distance(config_rgb, rgb)
dist_mean = get_rgb_distance(res[color_name]['mean'], rgb)
res[color_name]['values'].append(rgb)
res[color_name]['mean'] = get_rgb_median(res[color_name]['values'])
res[color_name]['std'] = get_rgb_std(res[color_name]['values'])
pr('%-15s Distances: Config %d, Mean %d (%d of %d)' %
(color_name, dist_config, dist_mean, cycle + 1, cnt))
last_color_rgb = res[color_name]['mean']
# Eval result
for color_name, config_rgb in config_color:
config_rgb = res[color_name]['config']
mean_rgb = res[color_name]['mean']
dist = get_rgb_distance(config_rgb, mean_rgb)
std = int(numpy.linalg.norm(res[color_name]['std']))
print('%-35s Distances: Config vs Mean %4d, Std %4s' %
(color_name, dist, str(std)))
# Print YAML file
res_yaml = []
res_yaml_values = []
for color_name, _ in config_color:
rgb = res[color_name]['mean']
values = res[color_name]['values']
res_yaml.append([color_name, rgb])
res_yaml_values.append([color_name, values])
print(80 * '*')
print('Printing YAML config')
print(yaml.dump(res_yaml))
# Rewrite config file with new values
timestamp = str(datetime.datetime.now())
path = DEF_PATH_CAL + '%s_station_%d.yaml' % (timestamp, station)
cfg = {
'desc':
'Automatically created with calibration routine date: %s, station: %d'
% (timestamp, station),
'det':
config_det,
'rgb':
config_rgb,
'sensor':
config_sensor,
'color':
res_yaml,
'values':
res_yaml_values,
'station':
station
}
pr('Also saving to %s' % path)
with open(path, 'w') as outfile:
yaml.dump(cfg, outfile, indent=4)
def cal_analysis(files):
configs = []
for file in files:
configs.append(load(file))
# check for same color count in values and color entries, use config #1 as reference
pr('Checking for same colors in all configs (values and color entry) using %s as reference'
% files[0])
first_run = True
color_cnt = 0
colors = []
for i, config in enumerate(configs):
if first_run:
config_color = config['color']
color_cnt = len(config_color)
for color in config_color:
colors.append(color[0])
first_run = False
cur_color_cnt = len(config['color'])
if cur_color_cnt != color_cnt:
pr('Color count differs in %s ['
'color'
'](%d vs %d) Exiting' % (files[i], cur_color_cnt, color_cnt))
return
cur_values_cnt = len(config['values'])
if cur_values_cnt != color_cnt:
pr('Color count differs in %s ['
'values'
'](%d vs %d) Exiting' % (files[i], cur_color_cnt, color_cnt))
return
cur_colors = config['color']
cur_values = config['values']
for j, color_name in enumerate(colors):
cur_color_name = cur_colors[j][0]
if cur_color_name != color_name:
pr('Color differs in %s ['
'color'
'](%s vs %s) Exiting' %
(files[i], cur_color_name, color_name))
return
cur_values_name = cur_values[j][0]
if cur_values_name != color_name:
pr('Color differs in %s ['
'values'
'](%s vs %s) Exiting' %
(files[i], cur_values_name, color_name))
return
pr('Everything is fine, lets start ...')
# pprint.pprint(colors)
# Calc overall means
over_all_means = []
over_all_std = []
over_all_values = []
for j, color_name in enumerate(colors):
pr('Starting with %s' % color_name)
cur_values_all = []
for i, config in enumerate(configs):
cur_values = config['values'][j][1]
for values in cur_values:
cur_values_all.append(values)
# TODO: if the files contain different numbers of measures values the mean is calculated wrong!!!
# TODO: solution, calc the mean of means on a per station basis
over_all_means.append([color_name, get_rgb_median(cur_values_all)])
over_all_std.append([color_name, get_rgb_std(cur_values_all)])
over_all_values.append([color_name, cur_values_all])
pprint.pprint(over_all_means)
pprint.pprint(over_all_std)
# Calc stats OK/NOK
# Stat1 Stat2
# Color 1 1/10 2/10
# Color 2 9/10 3/10
# ...
# Calc stats dist
# Stat1 Stat2
# avg/max avg/max
# Color 1 1/ 10 2/ 10
# Color 2 9/ 10 3/ 10
# ...
over_all_stats = []
over_all_dists = []
nok_stats = []
for j, color_name in enumerate(colors):
pr('Starting with %s' % color_name)
station_stats = []
station_dists = []
for i, config in enumerate(configs):
ok = 0
cnt = 0
color_values = config['values'][j][1]
station_name = config['station']
print('Station: %s' % station_name)
dists = []
for color_value in color_values:
cnt += 1
color_res, _, dist = get_color(color_value, over_all_means,
100000)
if color_res == color_name:
ok += 1
dists.append(dist)
else:
nok_stats.append([station_name, color_name, color_res])
station_stats.append([ok, cnt])
station_dists.append(dists)
over_all_stats.append([color_name, station_stats])
over_all_dists.append([color_name, station_dists])
# and print it out in a nice table (OK/NOK)
print(80 * '*')
print(35 * ' ', end='')
for config in configs:
print('%9s' % config['station'], end='')
print()
print(35 * ' ', end='')
for config in configs:
print(' OK/CNT', end='')
print()
for color_name, results in over_all_stats:
print('%35s' % color_name, end='')
for result in results:
ok = result[0]
cnt = result[1]
print(' %3d/%3d' % (ok, cnt), end='')
print()
# and print it out in a nice table (distances)
print(80 * '*')
print(35 * ' ', end='')
for config in configs:
print('%10s' % config['station'], end='')
print()
print(35 * ' ', end='')
for config in configs:
print(' AVG/ MAX', end='')
print()
for color_name, results in over_all_dists:
print('%35s' % color_name, end='')
for result in results:
if len(result) == 0:
max_dist = '----'
avg_dist = '----'
else:
max_dist = max(result)
avg_dist = int(numpy.mean(result))
print(' %4s/%4s' % (avg_dist, max_dist), end='')
print()
print(80 * '*')
for nok in nok_stats:
print('Station: %2s: set: %30s - act: %30s' % tuple(nok))
# Create a new config file with calculated values
# Rewrite config file with new values
timestamp = str(datetime.datetime.now())
path = DEF_PATH_CAL + '%s_all.yaml' % timestamp
desc = 'Automatically created with cal analysis routine date: %s ' % timestamp
for i, file in enumerate(files):
desc += 'File %d: ' '%s' ', ' % (i, file)
cfg = {
'desc': desc,
'det': configs[0]['det'],
'rgb': configs[0]['rgb'],
'sensor': configs[0]['sensor'],
'color': over_all_means,
}
pr('Also saving to %s' % path)
with open(path, 'w') as outfile:
yaml.dump(cfg, outfile, indent=4)