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)