import sensor, image, time
from pid import PID
from pyb import Servo
pan_servo=Servo(1)
tilt_servo=Servo(2)
pan_servo.calibration(500,2500,500)
tilt_servo.calibration(500,2500,500)
red_threshold = (13, 49, 18, 61, 6, 47)
pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.05, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
#tilt_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # use RGB565.
sensor.set_framesize(sensor.QVGA) # use QQVGA for speed.
sensor.set_vflip(True)
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
face_cascade = image.HaarCascade("frontalface", stages=25)
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
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. Don't set "merge=True" becuase that will merge blobs which we don't want here.
####################################################################
# Servo initialization and calibration here
pan = Servo(1) # use PWM on P7, s2 on shield
pan.calibration(540, 2400, 540, 1400, 1500)
pan.angle(90)
tilt = Servo(2) # use PWM on P8, s1 on shield
tilt.calibration(540, 2400, 1200, 1800, 1500)
tilt.angle(90)
delay(1000) # delay to give servos time to get to 90 & 90
#####################################################################
#####################################################################
# DC Motor pin setup
LEFT_MOTOR1 = Pin('P0', Pin.OUT_PP, Pin.PULL_NONE)
LEFT_MOTOR2 = Pin('P1', Pin.OUT_PP, Pin.PULL_NONE)
LEFT_ENABLE = Pin('P2', Pin.OUT_PP, Pin.PULL_NONE) # HI -> ON; LOW -> OFF
# Autonomous rover code by Chris Anderson, 3DR, DIY Robocars # Copyrigbt 2017 # Released under a BSD 2.0 Open Source Licence import sensor, image, pyb, math, time from pyb import Servo from pyb import LED s1 = Servo(1) # P7 Motor s2 = Servo(2) # P8 Steering print (s1.calibration()) # show throttle servo calibration cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 steering_gain = 500 kp = 0.8 # P term of the PID ki = 0 # I term of the PID kd = 0 # D term of the PID red_led = LED(1) green_led = LED(2) blue_led = LED(3) ir_led = LED(4) old_error = 0 measured_angle = 0 set_angle = 90 # this is the desired steering angle (straight ahead) p_term = 0 i_term = 0 d_term = 0 old_time = pyb.millis() def led_control(x):
from pyb import Servo servo = Servo(1) print(servo) servo.angle(0) servo.angle(10, 100) servo.speed(-10) servo.speed(10, 100) servo.pulse_width(1500) print(servo.pulse_width()) servo.calibration(630, 2410, 1490, 2460, 2190) print(servo.calibration())
from pyb import Servo servo = Servo(1) print(servo) servo.angle(0) servo.angle(10, 100) servo.speed(-10) servo.speed(10, 100) servo.pulse_width(1500) print(servo.pulse_width()) servo.calibration(630, 2410, 1490, 2460, 2190) print(servo.calibration())
# Servo Control Example
#
# This example shows how to use your OpenMV Cam to control servos.
import micropython, time
from pyb import Servo
micropython.alloc_emergency_exception_buf(100)
s1 = Servo(1) # P7
#s1.calibration(1000, 1500, 1100)
s1.calibration(640, 2420, 1500)
s1.speed(-100)
time.sleep(2000)
print(s1.pulse_width())
print(s1.calibration())
s1.speed(-14)
time.sleep(5000)
s1.speed(-13)
time.sleep(5000)
i = -20
while (i < 100):
print(i)
s1.speed(i)
print(s1.pulse_width())
time.sleep(1000)
i += 1
