Blob Detection Example
This example shows off how to use the find_blobs function to find color
blobs in the image. This example in particular looks for dark green objects.
import sensor, image, time
import car
from pid import PID
from pyb import Servo
You may need to tweak the above settings for tracking green things...
Select an area in the Framebuffer to copy the color settings.
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(10)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False,2000)#这里设置曝光时间
#sensor.skip_frames(time = 2000)
s3 = Servo(3) # servo on position 1 (P9)
clock = time.clock()
For color tracking to work really well you should ideally be in a very, very,
very, controlled enviroment where the lighting is constant...
red_threshold = (17, 50, -40, 110, 34, 127)
size_threshold = 2500
x_pid = PID(p=3, i=1, imax=100)
h_pid = PID(p=0.03, i=0.1, imax=50)
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
while(True):
s3.angle(90) # move to 90 degrees
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_blobs([red_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[5]-img.width()/2
h_error = max_blob[2]*max_blob[3]-size_threshold
print("x error: ", x_error)
'''
for b in blobs:
# Draw a rect around the blob.
img.draw_rectangle(b[0:4]) # rect
img.draw_cross(b[5], b[6]) # cx, cy
'''
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
x_output=x_pid.get_pid(x_error,1)
h_output=h_pid.get_pid(h_error,1)
print("h_output",h_output)
car.run(-h_output-x_output-50,-h_output+x_output-50)
else:
s3.angle(-90) # move to -90 degrees
car.run(500,-500)