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)
