DIY人脸跟踪电风扇送女朋友(2)

接着之前的内容，我们来编写一下云台控制的代码。实际上由于安装了开源的Adafruit_Python_PCA9685模块，操作舵机非常的简单。这里我们把舵机的控制分成x轴和y轴，x轴可以像左或者向由旋转一定的角度。y轴可以向上或者向下旋转一定的角度。所以舵机控制的接口定义为：

# channel 控制x轴还是y轴
# direction 控制方向，对应x轴的left或者right， y轴的up和down
# angle 为旋转的角度，0 - 45 度之间
def ServoControl(channel, direction, angle):
    ...


# 如果想让云台像左转动15度，如下调用方式：
ServoControl('x', 'left', 15)

# 如果想让云台像上转动20度，如下调用方式：
ServoControl('y', 'up', 20)

# 如果想让云台恢复到水平中间位置，如下调用方式：
ServoControl('x', 'left', 0) #'right' 也可以
ServoControl('y', 'up', 0) #'down' 也可以

下面是完整的舵机控制代码：

from __future__ import division
import time
import Adafruit_PCA9685
import threading

plus_y_up = 350
plus_y_down = 550
plus_y_middle = int((plus_y_up + plus_y_down) / 2)
plus_y_half = int((plus_y_down - plus_y_up) / 2)
plus_y_current = plus_y_middle

plus_x_left = 600
plus_x_right = 150
plus_x_middle = int((plus_x_left + plus_x_right) / 2)
plus_x_half = int((plus_x_left - plus_x_right) / 2)
plus_x_current = plus_x_middle

pwm = None

def ServoInit():
    global pwm
    pwm = Adafruit_PCA9685.PCA9685()
    pwm.set_pwm_freq(60)
    pwm.set_pwm(0, 0, plus_x_current)
    pwm.set_pwm(1, 0, plus_y_current)

def ServoControl(channel, direction, angle):
    global pwm 
    angle_max = 45
    if angle > angle_max:
        angle = angle_max
    ratio = float(angle) / angle_max
    
    if channel == 'x':
        if direction == 'right':
            plus = plus_x_middle - int(plus_x_half * ratio)
        if direction == 'left':
            plus = plus_x_middle + int(plus_x_half * ratio)
        print(plus)
        pwm.set_pwm(0, 0, plus)
        
    if channel == 'y':
        if direction == 'up':
            plus = plus_y_middle - int(plus_y_half * ratio)
        if direction == 'down':
            plus = plus_y_middle + int(plus_y_half * ratio)
        print(plus)
        pwm.set_pwm(1, 0, plus)
        
    
if __name__ == '__main__': 
    while True:
        ServoControl('x', 'right', 45)        
        time.sleep(1)
        ServoControl('x', 'right', 0)        
        time.sleep(1)
        ServoControl('x', 'left', 45)
        time.sleep(1)

今天暂时就把云台代码写好吧。