# These pins control our direction while the other PWN pins
# below control the speed. The direction for each motor
# is set by an H-Bridge where A0/1 are the two sides of
# one H-Bridge driver. B0/1 are another H-Bridge.
pinADir0 = pyb.Pin('P3', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinADir1 = pyb.Pin('P2', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinBDir0 = pyb.Pin('P1', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinBDir1 = pyb.Pin('P0', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
# Dir0/1 must be not equal to each other for forward or backwards
# operation. If they are equal then that's a brake operation.
# If they are not equal then the motor will spin one way other the
# other depending on it's hookup and the value of dir 0.
pinADir0.value(0)
pinADir1.value(1)
# Dir0/1 must be not equal to each other for forward or backwards
# operation. If they are equal then that's a brake operation.
# If they are not equal then the motor will spin one way other the
# other depending on it's hookup and the value of dir 0.
pinBDir0.value(0)
pinBDir1.value(1)
# Create a timer object running at 1KHz which which will power the
# PWM output on our OpenMV Cam. Just needs to be created once.
tim = pyb.Timer(4, freq=1000)
# Use the timer object to create two PWM outputs on the OpenMV Cam.
# These timers control the speed of the motors. You will be setting
# the PWM percentage of these timers repeatedly in your loop.
chA = tim.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P7"))
chB = tim.channel(2, pyb.Timer.PWM, pin=pyb.Pin("P8"))
import sensor, image, time
from pid import PID
from pyb import Servo
pan_servo=Servo(1)
pan_servo.calibration(500,2500,500)
red_threshold = (20, 60, 78, -127, 57, 8)
pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输,使用这个PID
#pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
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):
chA.pulse_width_percent(80)
chB.pulse_width_percent(80)
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)
pan_error = max_blob.cx()-img.width()/2
print("pan_error: ", pan_error)
img.draw_rectangle(max_blob.rect()) # rect
img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
pan_output=pan_pid.get_pid(pan_error,1)/2
print("pan_output",pan_output)
pan_servo.angle(pan_servo.angle()+pan_output)