black_grayscale_line_following#
# Black Grayscale Line Following Example
#
# Making a line following robot requires a lot of effort. This example script
# shows how to do the machine vision part of the line following robot. You
# can use the output from this script to drive a differential drive robot to
# follow a line. This script just generates a single turn value that tells
# your robot to go left or right.
#
# For this script to work properly you should point the camera at a line at a
# 45 or so degree angle. Please make sure that only the line is within the
# camera's field of view.
from media.camera import *
from media.display import *
from media.media import *
import time, os, gc, sys, math
DISPLAY_WIDTH = ALIGN_UP(1920, 16)
DISPLAY_HEIGHT = 1080
SCALE = 4
DETECT_WIDTH = DISPLAY_WIDTH // SCALE
DETECT_HEIGHT = DISPLAY_HEIGHT // SCALE
# Tracks a black line. Use [(128, 255)] for a tracking a white line.
GRAYSCALE_THRESHOLD = [(0, 64)]
# Each roi is (x, y, w, h). The line detection algorithm will try to find the
# centroid of the largest blob in each roi. The x position of the centroids
# will then be averaged with different weights where the most weight is assigned
# to the roi near the bottom of the image and less to the next roi and so on.
ROIS = [ # [ROI, weight]
(0, 100, 160, 20, 0.7), # You'll need to tweak the weights for your app
(0, 50, 160, 20, 0.3), # depending on how your robot is setup.
(0, 0, 160, 20, 0.1)
]
# Compute the weight divisor (we're computing this so you don't have to make weights add to 1).
weight_sum = 0
for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.
def camera_init():
# use hdmi for display
display.init(LT9611_1920X1080_30FPS)
# config vb for osd layer
config = k_vb_config()
config.max_pool_cnt = 1
config.comm_pool[0].blk_size = 4*DISPLAY_WIDTH*DISPLAY_HEIGHT
config.comm_pool[0].blk_cnt = 1
config.comm_pool[0].mode = VB_REMAP_MODE_NOCACHE
# meida buffer config
media.buffer_config(config)
# init default sensor
camera.sensor_init(CAM_DEV_ID_0, CAM_DEFAULT_SENSOR)
# set chn0 output size
camera.set_outsize(CAM_DEV_ID_0, CAM_CHN_ID_0, DISPLAY_WIDTH, DISPLAY_HEIGHT)
# set chn0 output format
camera.set_outfmt(CAM_DEV_ID_0, CAM_CHN_ID_0, PIXEL_FORMAT_YUV_SEMIPLANAR_420)
# create meida source device
globals()["meida_source"] = media_device(CAMERA_MOD_ID, CAM_DEV_ID_0, CAM_CHN_ID_0)
# create meida sink device
globals()["meida_sink"] = media_device(DISPLAY_MOD_ID, DISPLAY_DEV_ID, DISPLAY_CHN_VIDEO1)
# create meida link
media.create_link(meida_source, meida_sink)
# set display plane with video channel
display.set_plane(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT, PIXEL_FORMAT_YVU_PLANAR_420, DISPLAY_MIRROR_NONE, DISPLAY_CHN_VIDEO1)
# set chn1 output nv12
camera.set_outsize(CAM_DEV_ID_0, CAM_CHN_ID_1, DETECT_WIDTH, DETECT_HEIGHT)
camera.set_outfmt(CAM_DEV_ID_0, CAM_CHN_ID_1, PIXEL_FORMAT_RGB_888)
# media buffer init
media.buffer_init()
# request media buffer for osd image
globals()["buffer"] = media.request_buffer(4 * DISPLAY_WIDTH * DISPLAY_HEIGHT)
# start stream for camera device0
camera.start_stream(CAM_DEV_ID_0)
def camera_deinit():
# stop stream for camera device0
camera.stop_stream(CAM_DEV_ID_0)
# deinit display
display.deinit()
os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
time.sleep_ms(100)
# release media buffer
media.release_buffer(globals()["buffer"])
# destroy media link
media.destroy_link(globals()["meida_source"], globals()["meida_sink"])
# deinit media buffer
media.buffer_deinit()
def capture_picture():
# create image for drawing
draw_img = image.Image(DISPLAY_WIDTH, DISPLAY_HEIGHT, image.ARGB8888)
# create image for osd
buffer = globals()["buffer"]
osd_img = image.Image(DISPLAY_WIDTH, DISPLAY_HEIGHT, image.ARGB8888, alloc=image.ALLOC_VB, phyaddr=buffer.phys_addr, virtaddr=buffer.virt_addr, poolid=buffer.pool_id)
osd_img.clear()
display.show_image(osd_img, 0, 0, DISPLAY_CHN_OSD0)
fps = time.clock()
while True:
fps.tick()
try:
os.exitpoint()
rgb888_img = camera.capture_image(CAM_DEV_ID_0, CAM_CHN_ID_1)
img = rgb888_img.to_grayscale()
camera.release_image(CAM_DEV_ID_0, CAM_CHN_ID_1, rgb888_img)
draw_img.clear()
centroid_sum = 0
for r in ROIS:
blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True) # r[0:4] is roi tuple.
if blobs:
# Find the blob with the most pixels.
largest_blob = max(blobs, key=lambda b: b.pixels())
# Draw a rect around the blob.
draw_img.draw_rectangle([v*SCALE for v in largest_blob.rect()])
draw_img.draw_cross(largest_blob.cx()*SCALE, largest_blob.cy()*SCALE)
centroid_sum += largest_blob.cx() * r[4] # r[4] is the roi weight.
center_pos = (centroid_sum / weight_sum) # Determine center of line.
# Convert the center_pos to a deflection angle. We're using a non-linear
# operation so that the response gets stronger the farther off the line we
# are. Non-linear operations are good to use on the output of algorithms
# like this to cause a response "trigger".
deflection_angle = 0
# The 80 is from half the X res, the 60 is from half the Y res. The
# equation below is just computing the angle of a triangle where the
# opposite side of the triangle is the deviation of the center position
# from the center and the adjacent side is half the Y res. This limits
# the angle output to around -45 to 45. (It's not quite -45 and 45).
deflection_angle = -math.atan((center_pos-80)/60)
# Convert angle in radians to degrees.
deflection_angle = math.degrees(deflection_angle)
# Now you have an angle telling you how much to turn the robot by which
# incorporates the part of the line nearest to the robot and parts of
# the line farther away from the robot for a better prediction.
print("Turn Angle: %f" % deflection_angle)
draw_img.copy_to(osd_img)
del img
gc.collect()
print(fps.fps())
except KeyboardInterrupt as e:
print("user stop: ", e)
break
except BaseException as e:
sys.print_exception(e)
break
def main():
os.exitpoint(os.EXITPOINT_ENABLE)
camera_is_init = False
try:
print("camera init")
camera_init()
camera_is_init = True
print("camera capture")
capture_picture()
except Exception as e:
sys.print_exception(e)
finally:
if camera_is_init:
print("camera deinit")
camera_deinit()
if __name__ == "__main__":
main()
具体接口使用请参考相关文档说明：