追踪标靶

思路： 控制曝光和增益可以直接识别矩形边框。

云台追踪标靶

import sensor
import time
from pyb import Pin, Timer
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
clock = time.clock()

# 调节曝光和增益，适配环境，非常重要，而且非常有用，让画面可以看到清晰的矩形框
sensor.set_auto_exposure(False, exposure_us=40000)
sensor.set_auto_gain(False, gain_db=10)
sensor.skip_frames(time=2000)

CENTER_X = 150
CENTER_Y = 118

p_dir_motor1 = Pin('P0', Pin.OUT_PP) #步进电机1方向引脚   
p_dir_motor1.high()

p_dir_motor2 = Pin('P1', Pin.OUT_PP) #步进电机2方向引脚   
p_dir_motor2.high()

tim = Timer(4, freq=250)
p_pwm_motor1 = tim.channel(1, Timer.PWM, pin=Pin('P7')) #步进电机1脉冲引脚
p_pwm_motor1.pulse_width_percent(0)


p_pwm_motor2 = tim.channel(2, Timer.PWM, pin=Pin('P8')) #步进电机2脉冲引脚
p_pwm_motor2.pulse_width_percent(0)

laser = Pin('P6', Pin.OUT_PP) #设置激光笔引脚
laser.high()

def motor1_speed(speed):
    if speed >= 0:
        p_dir_motor1.high()
    else:
        p_dir_motor1.low()
    time.sleep_ms(5)
    print(speed)
    p_pwm_motor1.pulse_width_percent(abs(speed))

def motor2_speed(speed):
    if speed >= 0:
        p_dir_motor2.high()
    else:
        p_dir_motor2.low()
    time.sleep_ms(5)
    print(speed)
    p_pwm_motor2.pulse_width_percent(abs(speed))


while True:
    clock.tick()
    img = sensor.snapshot().binary([(0,160)])
    rect = None
    rect_max_mag = 0
    for r in img.find_rects(threshold=10000):
        if r.w() < 30 or r.h() < 30:
            continue
        if r.magnitude() < rect_max_mag:
            continue
        rect = r
        rect_max_mag = r.magnitude()
    if not rect:
        print("no rect")
        motor1_speed(0)
        motor2_speed(0)
        continue


    cx = rect.x() + rect.w()/2
    cy = rect.y() + rect.h()/2

    # 计算偏差的像素
    dx = cx - CENTER_X
    dy = cy - CENTER_Y

    # 通过偏差控制电机速度，P为追踪的参数，P越大，追踪越快，容易抖动
    P = 0.2
    motor1_speed(int(dx*P))
    motor2_speed(-int(dy*P))

    img.draw_rectangle(rect.rect(), color=(255, 0, 0))
    for p in rect.corners():
        img.draw_circle(p[0], p[1], 5, color=(0, 255, 0))
    # print(rect.w)
    # print("FPS %f" % clock.fps())

云台画圆

import time
from pyb import Pin
import math

p_dir_motor1 = Pin('P0', Pin.OUT_PP) #步进电机1方向引脚   
p_dir_motor1.high()

p_dir_motor2 = Pin('P1', Pin.OUT_PP) #步进电机2方向引脚   
p_dir_motor2.high()

p_step_motor1 = Pin('P7', Pin.OUT_PP) #步进电机1脉冲引脚  
p_step_motor1.low()

p_step_motor2 = Pin('P8', Pin.OUT_PP) #步进电机2脉冲引脚    
p_step_motor2.low()

laser = Pin('P6', Pin.OUT_PP) #设置激光笔引脚
laser.high()


def motor1_steps(steps):
    if steps >= 0:
        p_dir_motor1.high()
    else:
        p_dir_motor1.low()

    for i in range(abs(steps)):
        p_step_motor1.high()
        time.sleep_ms(1)
        p_step_motor1.low()
        time.sleep_ms(1)

def motor2_steps(steps):
    if steps >= 0:
        p_dir_motor2.high()
    else:
        p_dir_motor2.low()
    for i in range(abs(steps)):
        p_step_motor2.high()
        time.sleep_ms(1)
        p_step_motor2.low()
        time.sleep_ms(1)

degrees = 0
now_x = 0
now_y = 0

while True:
    degrees += 1
    if degrees > 360:
        degress = 0

    # x, y为目标坐标
    target_x = 60 * math.cos(math.radians(degrees))
    target_y = 60 * math.sin(math.radians(degrees))

    # 计算运动的数量
    dx = int(target_x - now_x)
    dy = int(target_y - now_y)

    # 更新现在的坐标
    now_x += dx
    now_y += dy

    # 控制运动
    motor1_steps(dx)
    motor2_steps(dy)

    time.sleep_ms(1)