樹莓派+二自由度雲臺製作智能小車（四）——二自由度雲臺

一、雙舵機實現雲臺運動

（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")

接下來就是舵機代碼的編寫了。
在寫代碼時要注意舵機的消抖。舵機有一個死區，消抖就是取消發射脈衝，將佔空比設置爲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來進行一個更加舒服搖桿操控。