Raspberry Pi 4B 同步控制兩個舵機 實作顔色跟蹤

元件：

• Raspberry Pi 4B 2G
• 5V TS90A 舵機 2個

• 轉動角度：0°~180°
• 工作電壓：4.8V-5V
• 控制信号：PWM 50HZ/0.5-2.5MS

• 攝像頭

環境

• Python：3.7.3

TS90A 舵機如下圖：

兩個舵機連接配接樹莓派如下圖：

舵機三條線定義：

• 棕色GND
• 紅色VCC 4.8-7.2V 一般用5V
• 橙色：脈沖輸入接樹莓派的GPIO口

參數：

• 無負載速度：0.09秒/60度(4.8V)
• 從 0 ~180° 大約需要 0.27 s，是以設定 sleep(0.3) 延時 0.3s 完成旋轉度數

公式：

dutycycle = 1.8 + angle / 360 * 20

from time import sleep
import RPi.GPIO as GPIO
import sys

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
 
gpio_io1 = 18
gpio_io2 = 23
 
GPIO.setup(gpio_io1, GPIO.OUT) 
GPIO.setup(gpio_io2, GPIO.OUT) 
 
def setServoAngle(servo, angle):
    assert angle >=0 and angle <= 180
    pwm = GPIO.PWM(servo, 50)
    pwm.start(8)
    dutyCycle = 1.8 + angle / 360 * 20
    pwm.ChangeDutyCycle(dutyCycle)
    sleep(0.3)
    pwm.stop()
 
if __name__ == &#39;__main__':
    if len(sys.argv) == 1:
        setServoAngle(gpio_io1, 90)
        setServoAngle(gpio_io2, 90)
    else:
        setServoAngle(gpio_io1, int(sys.argv[1])) 
        setServoAngle(gpio_io2, int(sys.argv[2])) 
    GPIO.cleanup()      

import numpy as np
import math
global picture
picture = np.ones((240,320,3),dtype =np.uint8)*255
import PID
#顯示攝像頭元件
import cv2
import time
# 線程功能操作庫
import threading
from time import sleep
import RPi.GPIO as GPIO
import sys

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
 
gpio_io1 = 18
gpio_io2 = 23
 
GPIO.setup(gpio_io1, GPIO.OUT) 
GPIO.setup(gpio_io2, GPIO.OUT) 
 
def setServoAngle(servo, angle):
    assert angle >=0 and angle <= 180
    pwm = GPIO.PWM(servo, 50)
    pwm.start(8)
    dutyCycle = 1.8 + angle / 360 * 20
    pwm.ChangeDutyCycle(dutyCycle)
    sleep(0.3)
    pwm.stop()

    
image = cv2.VideoCapture(0)

image.set(3, 320)
image.set(4, 240)
image.set(5, 30)  #設定幀率
image.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc(&#39;M', 'J', 'P', 'G'))
image.set(cv2.CAP_PROP_BRIGHTNESS, 62) #設定亮度 -64 - 64  0.0
image.set(cv2.CAP_PROP_CONTRAST, 63) #設定對比度 -64 - 64  2.0
image.set(cv2.CAP_PROP_EXPOSURE, 4800) #設定曝光值 1.0 - 5000  156.0
ret, frame = image.read()

global g_mode
g_mode = 0
global color_x, color_y, color_radius
color_x = color_y = color_radius = 0
global target_valuex
target_valuex = 1500
global target_valuey
target_valuey = 1500

global color_lower
#color_lower = np.array([0, 43, 46])
global color_upper
#color_upper = np.array([10, 255, 255])

color_lower = np.array([0,70,72])
color_upper = np.array([7, 255, 255])

xservo_pid = PID.PositionalPID(1.1, 0.2, 0.8)
yservo_pid = PID.PositionalPID(0.8, 0.2, 0.8)

setServoAngle(gpio_io1, 90)
setServoAngle(gpio_io2, 90)

def test():
    global color_lower, color_upper, g_mode, first_read, while_cnt
    global target_valuex, target_valuey, color_x, target_servox,picture
    t_start = time.time()
    fps = 0
    ret, frame = image.read()
    #frame = cv2.resize(frame, (300, 300))
    frame = cv2.GaussianBlur(frame,(5,5),0) 
    first_read = 1
    while_cnt = 0
    time.sleep(1)
    while True:
        ret, frame = image.read()  
        #frame = cv2.resize(frame, (300, 300))
        #frame = cv2.GaussianBlur(frame,(3,3),0)  
        hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv,color_lower,color_upper)   
        mask = cv2.erode(mask,None,iterations=2)
        mask = cv2.dilate(mask,None,iterations=2)
        mask = cv2.GaussianBlur(mask,(5,5),0)
        cnts = cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2] 
        if len(cnts) > 0:
            cnt = max (cnts, key = cv2.contourArea)
            (color_x,color_y),color_radius = cv2.minEnclosingCircle(cnt)

            if color_radius > 10:
                # 将檢測到的顔色用原形線圈标記出來
                cv2.circle(frame,(int(color_x),int(color_y)),int(color_radius),(255,0,255),2)  
                if math.fabs(150 - color_x) > 10:
                    xservo_pid.SystemOutput = color_x
                    xservo_pid.SetStepSignal(150)
                    xservo_pid.SetInertiaTime(0.01, 0.1)
                    target_valuex = int(1500+xservo_pid.SystemOutput)
                    target_servox = int((target_valuex-500)/10) 
                    # 将雲台轉動至PID調校位置
                    if target_servox > 180:
                        target_servox = 180
                    if target_servox < 0:
                        target_servox = 0
                    setServoAngle(gpio_io1, target_servox)

                if math.fabs(150 - color_y) > 10:  
                    # 輸入Y軸方向參數PID控制輸入
                    yservo_pid.SystemOutput = color_y
                    yservo_pid.SetStepSignal(150)
                    yservo_pid.SetInertiaTime(0.01, 0.1)
                    target_valuey = int(1500-yservo_pid.SystemOutput)
                    target_servoy = int((target_valuey-500)/10)                        
                    if target_servoy > 180:
                        target_servoy = 180
                    if target_servoy < 0:
                        target_servoy = 0
                    #print(target_servoy)

                    setServoAngle(gpio_io2, target_servoy)

        fps = fps + 1
        mfps = fps / (time.time() - t_start)
        cv2.putText(frame, "FPS " + str(int(mfps)), (40,40), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,255), 3)
        # 實時傳回圖像資料進行顯示
        cv2.imshow(&#39;capture', frame)
        # 檢測按鍵
        k = cv2.waitKey(1)    
        if k == 27:
            break
    image.release()
    cv2.destroyAllWindows()
            
if __name__ == &#39;__main__':
    test()      

#  PID控制一階慣性系統測試程式
#*****************************************************************#
#                      增量式PID系統                              #
#*****************************************************************#
class IncrementalPID:
    def __init__(self, P, I, D):
        self.Kp = P
        self.Ki = I
        self.Kd = D
 
        self.PIDOutput = 0.0             #PID控制器輸出
        self.SystemOutput = 0.0          #系統輸出值
        self.LastSystemOutput = 0.0      #上次系統輸出值
 
        self.Error = 0.0                 #輸出值與輸入值的偏差
        self.LastError = 0.0
        self.LastLastError = 0.0
 
    #設定PID控制器參數
    def SetStepSignal(self,StepSignal):
        self.Error = StepSignal - self.SystemOutput
        IncrementValue = self.Kp * (self.Error - self.LastError) +\
        self.Ki * self.Error +\
        self.Kd * (self.Error - 2 * self.LastError + self.LastLastError)

        self.PIDOutput += IncrementValue
        self.LastLastError = self.LastError
        self.LastError = self.Error

    #設定一階慣性環節系統  其中InertiaTime為慣性時間常數
    def SetInertiaTime(self,InertiaTime,SampleTime):
        self.SystemOutput = (InertiaTime * self.LastSystemOutput + \
            SampleTime * self.PIDOutput) / (SampleTime + InertiaTime)

        self.LastSystemOutput = self.SystemOutput
 
 
# *****************************************************************#
#                      位置式PID系統                              #
# *****************************************************************#
class PositionalPID:
    def __init__(self, P, I, D):
        self.Kp = P
        self.Ki = I
        self.Kd = D
 
        self.SystemOutput = 0.0
        self.ResultValueBack = 0.0
        self.PidOutput = 0.0
        self.PIDErrADD = 0.0
        self.ErrBack = 0.0
    
    #設定PID控制器參數
    def SetStepSignal(self,StepSignal):
        Err = StepSignal - self.SystemOutput
        KpWork = self.Kp * Err
        KiWork = self.Ki * self.PIDErrADD
        KdWork = self.Kd * (Err - self.ErrBack)
        self.PidOutput = KpWork + KiWork + KdWork
        self.PIDErrADD += Err
        if self.PIDErrADD > 2000:
            self.PIDErrADD = 2000
        if self.PIDErrADD < -2500:
            self.PIDErrADD = -2500
        self.ErrBack = Err
        

    #設定一階慣性環節系統  其中InertiaTime為慣性時間常數
    def SetInertiaTime(self, InertiaTime,SampleTime):
           self.SystemOutput = (InertiaTime * self.ResultValueBack + \
           SampleTime * self.PidOutput) / (SampleTime + InertiaTime)

           self.ResultValueBack = self.SystemOutput