Raspberry Pi 4B 同步控制两个舵机 实现颜色跟踪

Posted 2021-09-25 ʚVVcatɞ

组件：

• 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__ == '__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('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('capture', frame)
	    # 检测按键
        k = cv2.waitKey(1)	
        if k == 27:
            break
    image.release()
    cv2.destroyAllWindows()
			
if __name__ == '__main__':
    test()

PID.py

#  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