from lib.marker import collect_markers, use_camera, release_camera, make_shot TARGET = 618 TARGET = None if __name__ == '__main__': clockwise = int(sys.argv[1]) r = redis.Redis() c = frobo_ng() c.debug = True try: use_camera(r, width=320, height=240) c.update_state() dbprint('BEFORE %s m to %s' % (c.state['sonar'], c.heading())) found = c.search_marker(r, clockwise=clockwise, marker_id=TARGET) dbprint('MARKER: %s' % json.dumps(found, indent=2)) dbprint('AFTER %s m to %s' % (c.state['sonar'], c.heading())) json.dump(c.dots, file('dots.json', 'w'), indent=2) finally: c.cmd_mstop() c.wait_until_stop() make_shot(r) release_camera(r) dbprint('EVENTUALLY %d dist=%g' % (c.heading(), c.state['sonar'])) dbprint('TARGET: %s' % found)
use_camera(r) time.sleep(4) min_pwr = 20 max_pwr = 100 for k in range(10): found = c.search_marker(r, marker_id=TARGET) if found: for j in range(10): for i in range(10): off = marker_offset(r, TARGET) if off is None: continue else: aoff = abs(off) dbprint('H offset: %d degree%s' % (aoff, ((' in %s direction' % ('left' if off < 0 else 'right')) if int(off) != 0 else ''))) pwr = min_pwr + (max_pwr - min_pwr) * min( 1, (aoff / 180.)) if int(off) == 0: break if off < 0: c.tick_left(pwr=pwr) else: c.tick_right(pwr=pwr) if off is None: break elif int(off) == 0: dbprint("Move straingt") c.move_straight(fwd=True, max_steps=c.m2steps(0.2),
import sys, time, json from lib.frobo_ng import frobo_ng import picamera from lib.camera import update_img from lib.utils import dbprint dist = float(sys.argv[1]) c = frobo_ng() c.debug = True try: h = c.heading() try: c.update_state() dbprint('BEFORE %d (%d:%d) dist=%g(%g)' % (c.heading(), c.state['lcount'], c.state['rcount'], c.state['sonar'], c.state.get('irdist', -1))) c.move_straight(fwd=True, max_steps=c.m2steps(dist), max_secs=10, power=90, fix_heading=False) dbprint('AFTER %d (%d:%d) dist=%g(%g)' % (c.heading(), c.state['lcount'], c.state['rcount'], c.state['sonar'], c.state.get('irdist', -1))) if not c.hit_warn is None: dbprint("Slide back") c.move_straight(fwd=False, max_steps=c.m2steps(0.2), max_secs=5) dbprint('AFTER slide %d (%d:%d) dist:%g(%g)' % ( c.heading(), c.state['lcount'], c.state['rcount'], c.state['sonar'], c.state.get('irdist', -1))) json.dump(c.dots, file('dots.json', 'w'), indent=2) finally: c.cmd_mstop() c.update_state() change = c.heading() - h dbprint('EVENTUALLY %d (%d(%.2fm):%d(%.2fm)) dist=%g(%g), turn=%d' % ( c.heading(), c.state['lcount'], c.steps2m(c.state['lcount']),
with picamera.PiCamera() as camera: fp = FeatureProcess(camera) try: update_img(camera, html_data_path('pic0.jpg')) c.turn(350) time.sleep(1) update_img(camera, 'images/pic1.jpg')) fp.percent() c.move_straight(fwd=True, max_steps=c.m2steps(0.5), max_secs=1) update_img(camera, html_data_path('pic2.jpg')) time.sleep(1) c.turn(90) time.sleep(1) c.move_straight(fwd=True, max_steps=c.m2steps(0.5), max_secs=1) update_img(camera, html_data_path('pic3.jpg')) time.sleep(1) c.turn(210) time.sleep(1) c.move_straight(fwd=True, max_steps=c.m2steps(0.5), max_secs=1) update_img(camera, html_data_path('pic4.jpg')) time.sleep(1) c.turn(270) time.sleep(1) c.move_straight(fwd=True, max_steps=c.m2steps(0.5), max_secs=1) update_img(camera, html_data_path('pic5.jpg')) dbprint('Matches %s%%' % fp.percent()) json.dump(c.dots, file(html_data_path('dots.json'), 'w'), indent=2) finally: update_img(camera)
import matplotlib as mpl mpl.use('Agg') import picamera, cv2 from lib.utils import dbprint, html_data_path from lib.camera import update_img, ColorFix clrLower = [0, 10, 10] clrUpper = [64, 255, 255] if __name__ == '__main__': with picamera.PiCamera() as camera: ss = ColorFix(camera) try: # data = ss.mask_range(clrLower, clrUpper) data = ss.blob_detection() if data: # cv2.imwrite(html_data_path('frame_%03d.jpg' % i), data['frame']) # cv2.imwrite(html_data_path('iframe_%03d.jpg' % i), data['iframe']) # cv2.imwrite(html_data_path('oframe_%03d.jpg' % i), data['oframe']) cv2.imwrite(html_data_path('frame.jpg'), data['frame']) cv2.imwrite(html_data_path('iframe.jpg'), data['iframe']) cv2.imwrite(html_data_path('oframe.jpg'), data['oframe']) # json.dump(data['hlist'], file('colorfix.json', 'w')) else: dbprint("NOT FOUND") # json.dump({'imgcount': i}, file(html_data_path('frames.json'), 'w'), indent=2) finally: update_img(camera)
#!/usr/bin/env python import sys, time, json from lib.frobo_ng import frobo_ng import picamera from lib.camera import update_img from lib.utils import dbprint dist = float(sys.argv[1]) debug = False c = frobo_ng(debug=debug) cam = picamera.PiCamera() try: azim = c.heading() dbprint('BEFORE %d (%d:%d)' % (azim, c.state['lcount'], c.state['rcount'])) c.move_straight(fwd=True, max_steps=c.m2steps(dist), max_secs=1) dbprint('AFTER FWD %d (%d:%d)' % (c.heading(), c.state['lcount'], c.state['rcount'])) update_img(cam) azim -= 180 dbprint("Turning around to %d" % azim) c.turn(azim, err=5) dbprint('AFTER TURN %d (%d:%d)' % (c.heading(), c.state['lcount'], c.state['rcount'])) json.dump(c.dots, file('dots.json', 'w'), indent=2) finally: update_img(cam) c.update_state() dbprint('EVENTUALLY %d (%d:%d)' %
for tt in [ cv2.THRESH_BINARY, cv2.THRESH_BINARY_INV, cv2.THRESH_TRUNC, cv2.THRESH_TOZERO, cv2.THRESH_TOZERO_INV ]: for t in range(t_start, 255, (t_end - t_start) / cnt): data = ss.find_shapes(threshold=t, threshold_type=tt) if data: if os.environ.get('RASPICAM_ROTATE', ''): angle = int(os.environ['RASPICAM_ROTATE']) rows, cols, depth = data['frame'].shape M = cv2.getRotationMatrix2D((cols / 2, rows / 2), 180, 1) data['frame'] = cv2.warpAffine( data['frame'], M, (cols, rows)) cv2.imwrite(html_data_path('shapes_%03d.jpg' % i), data['frame']) # cv2.putText(data['thresh'], 'Thresh: %d' % t, (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 255), 2) cv2.putText(data['thresh'], 'Thresh: %d, Type: %d' % (t, tt), (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) cv2.imwrite(html_data_path('thresh_%03d.jpg' % i), data['thresh']) i += 1 dbprint(('*' * 10) + ' Image %d written' % i) json.dump({'imgcount': i}, file(html_data_path('shapes.json'), 'w'), indent=2) finally: update_img(camera)
from lib.frobo_ng import frobo_ng if __name__ == '__main__': random.seed() clockwise = int(sys.argv[1]) c = frobo_ng() c.debug = True r = redis.Redis() use_camera(r) time.sleep(4) try: dbprint('BEFORE %s cm to %s' % (c.state['sonar'], c.heading())) for i in range(30): c.update_state() c.move_straight(fwd=True, max_secs=2, max_steps=c.m2steps(random.randint(10, 100)/100.), power=100) c.find_distance(60, clockwise=random.choice((True, False))) markers = collect_markers(r, fpath = os.path.join(html_path('data'), 'pic%d.jpg' % i)) if markers: dbprint('Step %d. Found %d markers' % (i, len(markers))) dbprint('AFTER %s cm to %s' % (c.state['sonar'], c.heading())) json.dump(c.dots, file('dots.json', 'w'), indent=2) finally: make_shot(r) release_camera(r)
import sys, os, time, json import picamera from lib.utils import dbprint from lib.camera import update_img from lib.frobo_ng import frobo_ng if __name__ == '__main__': azim = float(sys.argv[1]) c = frobo_ng() c.debug = True cam = picamera.PiCamera() t = time.time() try: dbprint('BEFORE %d (%d:%d), TARGET: %d' % (c.heading(), c.state['lcount'], c.state['rcount'], azim)) c.turn(azim, err=2) dbprint('AFTER %d (%d:%d), TARGET: %d' % (c.heading(), c.state['lcount'], c.state['rcount'], azim)) c.wait_until_stop() json.dump(c.dots, file('dots.json', 'w'), indent=2) finally: c.cmd_mstop() time.sleep(2) update_img(cam) dbprint('EVENTUALLY %d (%d:%d), TARGET: %d dist:%g, dT:%d' % (c.heading(), c.state['lcount'], c.state['rcount'], azim, c.state['sonar'], time.time()-t))
#!/usr/bin/env python import sys, os, time import picamera from lib.utils import dbprint from lib.camera import update_img from lib.frobo_ng import frobo_ng LEFT = int(sys.argv[1]) dbprint('START') c = frobo_ng() c.debug = True try: h = c.heading() dbprint('BEFORE %.2f (%d:%d)' % (h, c.state['lcount'], c.state['rcount'])) proc = c.tick_left if LEFT else c.tick_right proc(pwr=100) # proc(pwr=100, min_angle=90) new_h = c.heading() dbprint('AFTER %.2f (%d:%d) turned:%.2f' % (new_h, c.state['lcount'], c.state['rcount'], new_h - h)) finally: cam = picamera.PiCamera() time.sleep(2) update_img(cam)
def cb(c): c.update_state() if c.state['sonar'] > 0: ad_data.append((c.heading(), c.state['sonar'])) c = frobo_ng() c.debug = True t = time.time() vname = 'v.h264' with picamera.PiCamera() as camera: camera.resolution = (160, 120) camera.start_recording(vname, format='h264', quality=23) try: dbprint('BEFORE %d (%d:%d)' % (c.heading(), c.state['lcount'], c.state['rcount'])) cnt = 8 delta = -360 / cnt ar = [(v, v + 45, 0) for v in range(0, 360, 45)] diff_acc = 0 old_h = h = c.heading() for i in range(cnt): azim = h + delta dbprint('next azim=%d' % azim) c.simple_turn(azim=azim, pwr=40, cb_func=cb, precise_stop=False) h = c.heading() diff_acc += abs(old_h - h) % 360 dbprint('diff_acc=%d' % diff_acc)
from lib.utils import dbprint from lib.frobo_ng import frobo_ng from lib.camera import update_img import picamera, cv2 if __name__ == '__main__': clockwise = int(sys.argv[1]) with picamera.PiCamera() as camera: c = frobo_ng() c.debug = True try: c.update_state() dbprint('BEFORE %s m to %s' % (c.state['sonar'], c.heading())) t = time.time() c.search_shapes(camera, clockwise=clockwise) dbprint('AFTER %s m to %s' % (c.state['sonar'], c.heading())) json.dump(c.dots, file('dots.json', 'w'), indent=2) except Exception, e: dbprint('Error %s' % e) raise finally: c.cmd_mstop() c.wait_until_stop() update_img(camera) dbprint('EVENTUALLY %d (%d:%d), dist:%g, dT:%d' % (c.heading(), c.state['lcount'], c.state['rcount'], c.state['sonar'], time.time()-t))
import picamera from lib.utils import dbprint from lib.camera import update_img from lib.frobo_ng import frobo_ng if __name__ == '__main__': right = int(sys.argv[1]) c = frobo_ng() c.debug = True with picamera.PiCamera() as camera: h = c.heading() try: update_img(camera, 'pic0.jpg') c.turn((h + (1 if right else -1) * 90) % 360, err=10) c.move_straight(fwd=True, max_secs=20, max_steps=c.m2steps(0.5)) update_img(camera, 'pic1.jpg') c.turn(h, err=5) json.dump(c.dots, file('dots.json', 'w'), indent=2) finally: c.cmd_mstop() c.update_state() update_img(camera) change = c.heading() - h dbprint('EVENTUALLY %d dist=%g, turn=%g' % (c.heading(), c.state['sonar'], change))
import picamera, cv2 from lib.utils import dbprint, html_data_path from lib.camera import update_img, FeatureProcess, capture_cvimage from lib.frobo_ng import frobo_ng if __name__ == '__main__': c = frobo_ng() # c.debug = True with picamera.PiCamera() as camera: fp = FeatureProcess(camera) try: fp.percent() c.tick_left(min_angle=10) data = fp.percent() if data: cv2.imwrite(html_data_path('pic1.jpg'), data['frame']) dbprint('Left=%g' % (data['percent'] if data else data)) cv2.imwrite(html_data_path('frame.jpg'), data['frame']) c.tick_right(min_angle=20) data = fp.percent() if data: cv2.imwrite(html_data_path('pic2.jpg'), data['frame']) dbprint('Right=%g' % (data['percent'] if data else data)) finally: update_img(camera)