通过树莓派控制舵机旋转固定角度

最近项目遇到一个小需求：通过转动旋钮对设备进行调焦。傻瓜式操作，只需要一个电机带动旋钮就好了。重点倒是如何识别图像是否达到自己想要的清晰度。不多说，上代码。
整体说明一下：通过相机采集图像，键盘按‘q’,采集一张图像，然后通过SMD2去分析得到图像的清晰度，再根据清晰度去对应调整舵机旋转的角度。这边只是测试舵机控制。

import atexit
import RPi.GPIO as GPIO                 # 引入GPIO模块
import time
import cv2 as cv
import numpy as np
import math

atexit.register(GPIO.cleanup)
#图像采集
def grap_photo():
    cap = cv.VideoCapture(0)
    while True:
        ret,frame = cap.read()
        if ret:
            cv.imshow('test',frame)
            if cv.waitKey(1) & 0xFF == ord('q'):
                # ~ file_name = "xieyang.jpeg"
                # ~ cv2.imwrite(file_name, frame)
                img=cv.cvtColor(frame,cv.COLOR_BGR2GRAY)
                return img
                # ~ break
        else:
            break
    cap.release()
    cv.destroyAllWindows()
#清晰度分析    
def SMD2(img):
    '''
    :param img:narray 二维灰度图像
    :return: float 图像约清晰越大
    '''
    img = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
    img = cv.resize(img,(480,640))
    shape = np.shape(img)
    out = 0
    for x in range(0, shape[0]-1):
        for y in range(0, shape[1]-1):
            out+=math.fabs(int(img[x,y])-int(img[x+1,y]))*math.fabs(int(img[x,y]-int(img[x,y+1])))
    return out
def NEW_SMD2_(img):
    '''
    :param img:narray 二维灰度图像
    :return: float 图像约清晰越大
    '''
    img = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
    img = cv.resize(img,(480,640)) #原来图片的数据类型：uint8
    img = np.float32(img) #转换后：float32
    timeA = time.time()
    h, w = img.shape
    Sharpness = np.sum(np.abs(img[:h-1, :w-1] - img[1:, :w-1]) * np.abs(img[:h-1, :w-1] - img[:h-1, 1:]))
    # print('new: {:.4f}'.format(time.time()-timeA))
    return Sharpness
#该方法(NEW_SMD2_)采用矩阵运算，处理速度比SMD2要快许多。值得注意的是：需要对img的数据类型转换，
#原图数据类型是uint8,计算方差时数据较大，会导致数组越界，最后得到的数值和SMD2不一样。所以需要转换
#可控制300度旋转的舵机
def Control_300(angle=None):
    try:
        while True:
            # ~ # 等待输入一个0到300的角度
            # ~ direction = angle
            direction = float(input("请输入移动角度: "))
            if direction < 0 or direction > 300:
                print("请输入正确的角度")
                continue

            duty = (1/30) * direction + 2.5   # 将角度转换为占空比
            pwm.ChangeDutyCycle(duty)         # 改变PWM占空比
            time.sleep(0.02)
    finally:
        pwm.stop()                      # 停止PWM
        GPIO.cleanup()                  # 清理释放GPIO资源，将GPIO复位


#可控制360度旋转舵机
def Control_360(angle=None):
    try:
        while True:
            # ~ # 等待输入一个0到300的角度
            # ~ direction = angle
            direction = float(input("请输入移动角度: "))
            if direction < 0 or direction > 360:
                print("请输入正确的角度")
                continue

            duty = (1/36) * direction + 2.5   # 将角度转换为占空比
            pwm.ChangeDutyCycle(duty)         # 改变PWM占空比
            time.sleep(0.02)
    finally:
        pwm.stop()                      # 停止PWM
        GPIO.cleanup()                  # 清理释放GPIO资源，将GPIO复位


#不可控360度旋转    
def UnControl_360(angle=None):
    try:
        # 等待输入一个0到300的角度
        direction = angle
        # ~ direction = float(input("请输入移动角度: "))
        if direction < 0 or direction > 360:
            print("请输入正确的角度")
        duty = (1/30) * direction + 2.5   # 将角度转换为占空比
        pwm.ChangeDutyCycle(duty)         # 改变PWM占空比
        time.sleep(0.02)
 
    finally:
        pwm.stop()                      # 停止PWM
        GPIO.cleanup()                  # 清理释放GPIO资源，将GPIO复位 
if __name__ == '__main__':
    ServoPin = 21
    PWMFreq = 50                      # PWM信号频率

    GPIO.setmode(GPIO.BCM)              # 使用BCM编号方式
    GPIO.setup(ServoPin, GPIO.OUT)      # 将GPIO19设置为输出模式
    pwm = GPIO.PWM(ServoPin, PWMFreq)   # 创建PWM对象，并设置频率为50
    pwm.start(0)                        # 启动PWM，并设置初始占空比0
    # ~ UnControl_360()
    Control_360()
    # ~ img = grap_photo()
    # ~ data = SMD2(img)
    # ~ print(data)
    # ~ if data>10000:
        # ~ Control_300(200)
    # ~ else:
        # ~ Control_300(10)