kriselosning

# Single Color RGB565 Blob Tracking Example
#
# This example shows off single color RGB565 tracking using the OpenMV Cam.

import sensor, image, time

threshold_index = 1 # 0 for red, 1 for green, 2 for blue

# Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max)
# The below thresholds track in general red/green/blue things. You may wish to tune them...
thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds
              (27, 100, -74, -25, 13, 62), # generic_green_thresholds
              (0, 30, 0, 64, -128, 0)] # generic_blue_thresholds

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()

# Only blobs that with more pixels than "pixel_threshold" and more area than "area_threshold" are
# returned by "find_blobs" below. Change "pixels_threshold" and "area_threshold" if you change the
# camera resolution. "merge=True" merges all overlapping blobs in the image.

from pyb import Servo

x_pos = 1500 # default
y_pos = 1500 # default

x_min = 640
x_max = 2500
y_max = 2500
y_min = 640

x_gain = -.75 # You have to tweak this value to stablize the control loop.
               # You also may need to invert the value if the system goes
               # in the wrong direction.
y_gain = -.75 # You have to tweak this value to stablize the control loop.
               # You also may need to invert the value if the system goes
               # in the wrong direction.
xServo = Servo(1)
yServo = Servo(2)
xServo.pulse_width(x_pos)
yServo.pulse_width(y_pos)

while(True):
    clock.tick()
    img = sensor.snapshot()
    for blob in img.find_blobs([thresholds[threshold_index]], pixels_threshold=200, area_threshold=200, merge=True):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())

        x = blob.cx()
        y = blob.cy()

        x_error = x - (img.width()/2)
        y_error = y - (img.height()/2)

        x_pos += x_error * x_gain
        y_pos += y_error * y_gain

        # Clamp output between min and max
        if (x_pos > x_max):
            x_pos = x_max
        if (x_pos < x_min):
            x_pos = x_min

        # Clamp output between min and max
        if (y_pos > y_max):
            y_pos = y_max
        if (y_pos < y_min):
            y_pos = y_min

        xServo.pulse_width(int(x_pos))
        yServo.pulse_width(int(y_pos))
#        print(x)
#        print(y)

    # Draw FPS
    img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))