树莓派+二自由度云台制作智能小车（四）——二自由度云台

一、双舵机实现云台运动

（1）舵机的运动原理

通常情况下，伺服电机（舵机）是由一个标准的直流系统和一个内部反馈控制装置（一个减速齿轮和电位计）来组成的。伺服电机（舵机）的主要作用是将齿轮轴旋转到一个预定义的方向上。伺服电机（舵机）有3个输入引脚，GND、VCC和Signal。脉冲宽度调制技术（PWM）被应用于舵机的控制，轴的方向由脉冲的持续时间决定。需要记住的是，舵机转动的方向不是由占空比决定的，而是由脉冲长度 t 决定的。有的舵机使用的PWM频率为 fPWM=50HZ，其对应于的PWM周期 T=20 ms。脉冲长度 t 和转动方向之间的关系是线性的，但也取决于电机和齿轮的配合。下面是一个脉冲长度和转动方向的关系图。

想进一步了解伺服电机的同学：点这里
本文选用的舵机为MG90S，数字电机，下面是一些基本参数：
• 产品类型：MG90S舵机
• 产品扭矩：2.0kg/cm(4.8V)，2.8kg/cm(6V)
• 产品速度：0.11秒/60°(4.8V)，0.09秒/60°(6V)
• 转动角度：180°
• 工作电压：4.8 ~ 6V
• 齿轮形式：金属齿轮
• 死区设定：5us (微秒)
• 产品重量：12.2g
• 产品尺寸：22.8mm × 12.2mm × 28.5mm

（2）代码实现

在将舵机装进云台支架里时，一定要注意舵机转轴的初始角度，然后要注意竖直方向上的云台运动时是否会卡住，造成舵机损坏。然后2是代表2%，也就是min_angle,12%是max_angle。
下面的是一个舵机角度的测试代码,可以通过这个测试代码，获得转动角度和占空比：

import RPi.GPIO as GPIO
import time

P_SERVO = 16
fPWM = 50    #hz
a = 10
b = 2

def setup():
    global pwm
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(P_SERVO,GPIO.OUT)
    pwm = GPIO.PWM(P_SERVO,fPWM)
    pwm.start(0)
    
def setDirection(direction):
    duty = a / 180 * direction + b
    pwm.ChangeDutyCycle(duty)
    print("direction =%f , duty = %f "%(direction,duty))
    time.sleep(1)
    
print('starting')
setup()
for direction in range(0,181,10):
    setDirection(direction)
direction = 0
setDirection(0)
GPIO.cleanup()
print("done")

接下来就是舵机代码的编写了。steering.py
写代码时要注意舵机的消抖。舵机有一个死区，消抖就是取消发射脉冲，将占空比设置为0。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 云台舵机的代码
import RPi.GPIO as GPIO
import time
import atexit
# ONE servo class

class Steering:
    max_delay = 0.2
    min_delay = 0.04  
    
    def __init__(self,channel,init_position,min_angle,max_angle,speed):
        self.channel = channel
        self.init_position = init_position
        self.position = init_position
        self.min_angle = min_angle
        self.max_angle = max_angle
        self.speed = speed
        
        atexit.register(GPIO.cleanup)
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.channel,GPIO.OUT,initial = False)
        
        self.pwm = GPIO.PWM(self.channel,50)  #PWM  50hz
        self.pwm.start(2+10*self.position/180) #init_position
        time.sleep(Steering.max_delay)
        self.pwm.ChangeDutyCycle(0)     #clean duty cycle,avoid servo motor jitter
        time.sleep(Steering.min_delay)
        
    def forwardRotation(self):
        print("current position1: "+ str(self.position))
        
        if (self.position+self.speed) <= self.max_angle:
            self.position = self.position + self.speed
            self.pwm.ChangeDutyCycle(2+10*self.position/180)
            time.sleep(Steering.max_delay)
            self.pwm.ChangeDutyCycle(0)   #clean duty cycle,avoid servo motor jitter
            time.sleep(Steering.min_delay)
        print("current position2: "+ str(self.position))    
            
    def reverseRotation(self):
        print("current position1: "+ str(self.position))
        
        if (self.position-self.speed) >= self.min_angle:
            self.position = self.position - self.speed
            self.pwm.ChangeDutyCycle(2+10*self.position/180)
            time.sleep(Steering.max_delay)
            self.pwm.ChangeDutyCycle(0)   #clean duty cycle,avoid servo motor jitter
            time.sleep(Steering.min_delay)
        print("current position2: "+ str(self.position))
    
    def reset(self):
        print("reset to inital position ")
        
        self.position = self.init_position
        self.pwm.ChangeDutyCycle(2+10*self.position/180)
        time.sleep(Steering.max_delay)
        self.pwm.ChangeDutyCycle(0)      #clean duty cycle,avoid servo motor jitter
        time.sleep(Steering.min_delay)
    
    def turnleft(self):
        
        self.position = self.max_angle
        self.pwm.ChangeDutyCycle(2+10*self.position/180)
        time.sleep(Steering.max_delay)
        self.pwm.ChangeDutyCycle(0)      #clean duty cycle,avoid servo motor jitter
        time.sleep(Steering.min_delay)

    def turnrigth(self):
        
        self.position = self.min_angle
        self.pwm.ChangeDutyCycle(2+10*self.position/180)
        time.sleep(Steering.max_delay)
        self.pwm.ChangeDutyCycle(0)      #clean duty cycle,avoid servo motor jitter
        time.sleep(Steering.min_delay)

    def stop(self):
        self.pwm.stop()
        time.sleep(Steering.max_delay)
        GPIO.cleanup()
        
            
if __name__ == "__main__":
    steer = Steering(16,90,0,180,10)
    while True:
        direction = input("Please input direction: ")
        if direction == "F":
            steer.forwardRotation()
        elif direction == "R":
            steer.reverseRotation()
        elif direction == "S":
            steer.stop()

当转动的角度，与预想一样时。进入对云台代码的一个编写，
根据实际情况对云台的旋转角度进行一个限制，对微调的角度进行一个设置。
首先在配置文件config.ini中加入舵机的数据：

[camera]
#BCM
#BOARD 16
#control camera's horizonal direction R:0 L:180 F:90
H_servoNum = 23
H_minPosition = 0    
H_maxPosition = 180  
H_InitPosition = 90  
H_speed =5
#control camera's vertical direction  Top:0 Bottom:130 F:60
#BOARD 18
#BCM
V_servoNum = 24
V_minPosition = 0    
V_maxPosition = 105 
V_InitPosition = 60 
V_speed = 5

然后创建一个camera_control.py，

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 云台完整代码
from steering import Steering
import time
import configparser

class Camera:
    def __init__(self):
        '''
        init camera's parameter
        '''
        config = configparser.ConfigParser()
        config.read("config.ini")
        
        #Horizontal direction control parameters
        H_servoNum = config.getint("camera","H_servoNum")
        H_MinPosition = config.getint("camera","H_minPosition")
        H_MaxPosition = config.getint("camera","H_maxPosition")
        H_InitPosition = config.getint("camera","H_InitPosition")
        H_Speed = config.getint("camera","H_speed")
        #Vertical direction control parameters
        V_servoNum = config.getint("camera","V_servoNum")
        V_MinPosition = config.getint("camera","V_minPosition")
        V_MaxPosition = config.getint("camera","V_maxPosition")
        V_InitPosition = config.getint("camera","V_InitPosition")
        V_Speed = config.getint("camera","V_speed")
        
        #HC= horizontal control VC=vertiacl control
        self.HC = Steering(H_servoNum,H_InitPosition,
                           H_MinPosition,H_MaxPosition,H_Speed)
        self.VC = Steering(V_servoNum,V_InitPosition,
                           V_MinPosition,V_MaxPosition,V_Speed)
        
    def cameraRotation(self,direction):
        '''
        This method is used to control the camera's rotating
        value means:
        HR = turn right
        HL = turn left
        VU = up
        VD = down
        GL = left prospective
        GF = front prospective
        GR = right prospective
        GU = up prospective
        GD = down prospective
        '''
        if direction == "HL":
            self.HC.forwardRotation()
        elif direction == "HR":
            self.HC.reverseRotation()
        elif direction == "VU":
            self.VC.reverseRotation()
        elif direction == "VD":
            self.VC.forwardRotation()
        elif direction == "GL":
 #           self.VC.reset()
            self.HC.turnleft()
        elif direction == "GR":
#            self.VC.reset()
            self.HC.turnrigth()
        elif direction == "GU":
            self.VC.turnrigth()
        elif direction == "GD":
            self.VC.turnleft()
        elif direction == "GF":
            self.VC.reset()
            self.HC.reset()
        elif direction == "S":
            self.VC.stop()
            self.HC.stop()
        else:
            print("input error,please input :HL,HR,VU,VD")
                
if __name__ == "__main__":
    camera = Camera()
    while(True):
        direction = input("Please input direction: ")
        camera.cameraRotation(direction)

这就是云台的控制代码了接下来进行摄像头的设置。

二、摄像头模块

本文选用树莓派CSI的免驱摄像头，插上就可以直接用，配合之前发的app可以直接测试摄像头，无需多余的操作。对树莓派摄像头的操作一般有几种，下面介绍两种，一种适合自己进行Android开发的，另一种就是集成在网页上，比较方便，在网页上可以直接用，如果有不对的地方请指正。

（1）利用MJPG-streamer实现网络监控

这也是本文采用的一种方式，比较简单。
将摄像头模块一端的排线插入树莓派上的CSI摄像头卡槽。使用前需要确定树莓派的系统是否进行了更新并采用了最新固件，输入以下命令：
sudo raspi-config
在出现的界面中选择5 Interfacing Options，进入之后选择P1 Camera，如图所示，使能摄像头功能。

(2) MJPG-Streamer实现网络监控
使能摄像头之后，安装MJPG-Streamer工具来实现小车视角画面的实时网络传输。
安装依赖，输入命令：
sudo apt-get install libjpeg8-dev cmake
下载MJPG-Streamer-Master，输入命令：

wget http://github.com/jacksonliam/mjpg-streamer/archive/master.zip	 
unzip master.zip	

对分辨率及帧率进行修改，修改帧率及分辨率如图，输入命令：

cd mjpg-streamer-master/mjpg-streamer-experimental				 
vim plugins/input_raspicam/input_raspicam.c					 

修改完成后保存退出vim，进行编译生成可执行文件，输入命令：
make clean all
开启摄像头，可通过http://树莓派ip:8080进行访问，输入命令：
./mjpg_streamer -i "./input_raspicam.so" -o "./output_http.so -w ./www"
其他的详细操作，由于树莓派的MJPG-Streamer的一个裁剪性，我并没有在参数和命令里面找到设置访问密码等参数，如果有错误，感谢指正。

（2）motion工具实现网络视频监控

之前使能摄像头的操作和方法（1）一样，
使能摄像头模块之后，安装motion工具软件来实现远程视频监控功能。输入命令：
sudo apt-get install motion
配置motion的守护进程motion daemon，让其能够一直在后台运行，需要修改/etc/default/motion文件。将start_motion_daemon=no修改为yes。执行命令：
sudo vi /etc/default/motion
配置完成之后，可以通过以下下命令来开启和关闭摄像头模块。

sudo motion  开启摄像头命令。
sudo killall motion 关闭摄像头命令。

执行开启命令之后，工作正常时，模块上的LED灯会亮起，此时可以在浏览器中输入网络地址，格式为http://树莓派ip:8081，来查看摄像头模块拍摄的实时画面内容。
(3)配置motion工具
在/etc/motion目录下找到motion.conf配置文件，在该配置文件中可以修改摄像图片的分辨率、端口号、传输速率等等。其中常用的配置如下：
staream_port 配置视频流读取的端口号
width 配置视频流每帧图片的宽度
height 配置视频流每帧图片的高度
framerate 每秒播放的帧数，范围在2-100之间，默认为100。

有兴趣的童鞋可以尝试写Android来进行一个更加舒服摇杆操控。