openmv控制舵机

import sensor image time

from pid import PID
from pyb import ServoUARTPin
import pyb

uart = UART（3 115200） # TX:P4 RX:P5 （板子线反接）
pan_servo=Servo（1） # P7
distance_servo=Servo（2） # P8
key_servo = Servo（3） # P9 -40:开 40：关
shot_flagpin = Pin（‘P1‘ Pin.OUT_PP） # 设置shot_flagpin为输出引脚
shot_flagpin.low（） # 拉高发射控制电平 高电平：发射 低电平：不发射
mode_Highpin = Pin（‘P2‘ Pin.IN Pin.PULL_UP）# 设置flag_Highpin为输入引脚，并开启上拉电阻
mode_Lowpin = Pin（‘P3‘ Pin.IN Pin.PULL_UP）# 设置flag_Lowpin为输入引脚，并开启上拉电阻


distance_dict = {‘15‘:212‘16‘:214‘17‘:229‘18‘:232‘19‘:234‘20‘:242‘21‘:248‘22‘:252‘23‘:260‘24‘:272‘25‘:273‘26‘:270‘27‘:264‘28‘:274‘29‘:296‘30‘:283‘31‘:300}

mode_flag = 0 # 0:等待模式  1:手动模式 2:自动模式 3：摆头模式
uart_flag = False # True:串口接收完成 False: 串口等待接收
shot_flag = False # True:发射炮弹 False: 不发射炮弹
distance_finish = False # True: 调整完成 False:未调整完成
pan_finish = False # True: 调整完成 False:未调整完成
distance_rejustvalue = 0 # 调试上传调整距离，解决阈值变化问题


# 数据包起始标志位
START = b‘（‘
STOP = b‘）‘
mid_Xangel = 30 #X 舵机给定初始转向 30
mid_Yangel = 15#Y 舵机给定初始转向 15
X_range = list（range（（mid_Xangel-30）（mid_Xangel+30））） # 模式三的旋转范围
X_i = 0 # 旋转计数
Aim_threshold = （27 58 46 74 3 59）# （34 61 25 66 -11 47）
red_threshold  = （13 49 18 61 6 47）
pixels_threshold  = 60
K= 4700 #the value should be measured

#pan_pid = PID（p=0.07 i=0 imax=90） #脱机运行或者禁用图像传输，使用这个PID
#distance_pid = PID（p=0.05 i=0 imax=90） #脱机运行或者禁用图像传输，使用这个PID
pan_pid = PID（p=0.1 i=0 imax=90）#在线调试使用这个PID
distance_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.QVGA） # 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 mode_read（）:
    High = mode_Highpin.value（）
    Low = mode_Lowpin.value（）
    modevalue = High * 2 + Low * 1
    return modevalue

# 找最大标志点
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

# 距离角度转换 distabce = angle * -2.1786 + 400.96
def distance_turn2angel（distance）:
    angel = int（（distance + distance_rejustvalue - 139.316）/5） # 乘以一个系数
    return angel

# 发射开关
def is_shot（is_fire）:
    # -40 guang 40 kai
    if is_fire:
        key_servo.angle（10）
    else:
        key_servo.angle（55）

# 转角限幅
def turn_max（）:
    pass

# 初始化
pan_servo.angle（mid_Xangel）# 摆正水平舵机
distance_servo.angle（mid_Yangel） # 摆正竖直舵机
is_shot（False）



while（True）:
    clock.tick（） # Track elapsed milliseconds between snapshots（）.
    mode_flag = mode_read（） # 获取运行模式
    if mode_flag == 0:
        # mode_flag = 0
        pan_servo.angle（mid_Xangel）# 摆正水平舵机
        distance_servo.angle（mid_Yangel） # 摆正竖直舵机
        is_shot（False）
        uart_flag = False
        shot_flag = False #