組件：

• 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 sysGPIO.setmode(GPIO.BCM) GPIO.setwarnings(False)gpio_io1 = 18 gpio_io2 = 23GPIO.setup(gpio_io1, GPIO.OUT) GPIO.setup(gpio_io2, GPIO.OUT) def setServoAngle(servo, angle):assert angle >=0 and angle <= 180pwm = GPIO.PWM(servo, 50)pwm.start(8)dutyCycle = 1.8 + angle / 360 * 20pwm.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 sysGPIO.setmode(GPIO.BCM) GPIO.setwarnings(False)gpio_io1 = 18 gpio_io2 = 23GPIO.setup(gpio_io1, GPIO.OUT) GPIO.setup(gpio_io2, GPIO.OUT) def setServoAngle(servo, angle):assert angle >=0 and angle <= 180pwm = GPIO.PWM(servo, 50)pwm.start(8)dutyCycle = 1.8 + angle / 360 * 20pwm.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 = 1500global 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_cntglobal target_valuex, target_valuey, color_x, target_servox,picturet_start = time.time()fps = 0ret, frame = image.read()#frame = cv2.resize(frame, (300, 300))frame = cv2.GaussianBlur(frame,(5,5),0) first_read = 1while_cnt = 0time.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_xxservo_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 = 180if target_servox < 0:target_servox = 0setServoAngle(gpio_io1, target_servox)if math.fabs(150 - color_y) > 10: # 輸入Y軸方向參數PID控制輸入yservo_pid.SystemOutput = color_yyservo_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 = 180if target_servoy < 0:target_servoy = 0#print(target_servoy)setServoAngle(gpio_io2, target_servoy)fps = fps + 1mfps = 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:breakimage.release()cv2.destroyAllWindows()if __name__ == '__main__':test()

PID.py

# PID控制一階慣性系統測試程序 #*****************************************************************# # 增量式PID系統 # #*****************************************************************# class IncrementalPID:def __init__(self, P, I, D):self.Kp = Pself.Ki = Iself.Kd = Dself.PIDOutput = 0.0 #PID控制器輸出self.SystemOutput = 0.0 #系統輸出值self.LastSystemOutput = 0.0 #上次系統輸出值self.Error = 0.0 #輸出值與輸入值的偏差self.LastError = 0.0self.LastLastError = 0.0#設置PID控制器參數def SetStepSignal(self,StepSignal):self.Error = StepSignal - self.SystemOutputIncrementValue = self.Kp * (self.Error - self.LastError) +\self.Ki * self.Error +\self.Kd * (self.Error - 2 * self.LastError + self.LastLastError)self.PIDOutput += IncrementValueself.LastLastError = self.LastErrorself.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 = Pself.Ki = Iself.Kd = Dself.SystemOutput = 0.0self.ResultValueBack = 0.0self.PidOutput = 0.0self.PIDErrADD = 0.0self.ErrBack = 0.0#設置PID控制器參數def SetStepSignal(self,StepSignal):Err = StepSignal - self.SystemOutputKpWork = self.Kp * ErrKiWork = self.Ki * self.PIDErrADDKdWork = self.Kd * (Err - self.ErrBack)self.PidOutput = KpWork + KiWork + KdWorkself.PIDErrADD += Errif self.PIDErrADD > 2000:self.PIDErrADD = 2000if self.PIDErrADD < -2500:self.PIDErrADD = -2500self.ErrBack = Err#設置一階慣性環節系統 其中InertiaTime為慣性時間常數def SetInertiaTime(self, InertiaTime,SampleTime):self.SystemOutput = (InertiaTime * self.ResultValueBack + \SampleTime * self.PidOutput) / (SampleTime + InertiaTime)self.ResultValueBack = self.SystemOutput

總結

以上是生活随笔為你收集整理的Raspberry Pi 4B 同步控制两个舵机 实现颜色跟踪的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。