'''
OpenCV バージョン 4.6.0
numpy バージョン 1.23.5
USBカメラ モーター2つを使用
プログラムを開始したときにウインドウの中央にあるモノの
色を追うプログラム
'''
# ライブラリ
import numpy as np
import RPi.GPIO as GPIO
import cv2
import time
import math
# モーター上下
mota = 17 # 上
motb = 18 # 下
# モーター左右
motc = 22 # 右
motd = 27 # 左
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
# セットアップ
GPIO.setup(mota,GPIO.OUT)
GPIO.setup(motb,GPIO.OUT)
GPIO.setup(motc,GPIO.OUT)
GPIO.setup(motd,GPIO.OUT)
# PWM
p1 = GPIO.PWM(mota, 50)
p2 = GPIO.PWM(motb, 50)
p3 = GPIO.PWM(motc, 50)
p4 = GPIO.PWM(motd, 50)
# モーター停止
GPIO.output(mota,GPIO.LOW)
GPIO.output(motb,GPIO.LOW)
GPIO.output(motc,GPIO.LOW)
GPIO.output(motd,GPIO.LOW)
'''
カメラの移動を行う関数
'''
def move_up(): # ↑
n = 50
p1.start(0)
p1.ChangeDutyCycle(n)
time.sleep(0.1)
p1.stop()
GPIO.output(mota,GPIO.LOW)
def move_down(): # ↓
n = 50
p2.start(0)
p2.ChangeDutyCycle(n)
time.sleep(0.1)
p2.stop()
GPIO.output(motb,GPIO.LOW)
def move_left(): # ←
n = 50
p4.start(0)
p4.ChangeDutyCycle(n)
time.sleep(0.1)
p4.stop()
GPIO.output(motd,GPIO.LOW)
def move_right(): # →
n = 50
p3.start(0)
p3.ChangeDutyCycle(n)
time.sleep(0.1)
p3.stop()
GPIO.output(motc,GPIO.LOW)
def move_stop(): # 停止
GPIO.output(mota,GPIO.LOW)
GPIO.output(motb,GPIO.LOW)
GPIO.output(motc,GPIO.LOW)
GPIO.output(motd,GPIO.LOW)
#カメラからの映像を読み込み
cap = cv2.VideoCapture(0)
# 画面サイズ(横) デフォルトのサイズのまま
W = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
# 画面サイズ(縦) デフォルトのサイズのまま
H = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
# fpsの取得 フレームレート(1秒間の動画がいくつの画像で構成されているか)
# 60fps は 1秒間に60フレーム(コマ)
fps = cap.get(cv2.CAP_PROP_FPS)
#画面サイズ横幅
cap.set(3, 320)
#画面サイズ縦、高さ
cap.set(4, 320)
#fps
cap.set(5, 15)
#白色
color = (255, 255, 255)
time.sleep(0.5)
# 追跡する枠の座標とサイズ
x, y, w, h = 110, 110, 100, 100
#はじめに指定した枠
track_window = (x, y, w, h)
# はじめの座標を格納 どれくらいずれたかの比較に使用
first_x = track_window[0]
first_y = track_window[1]
# フレームの取得
ret,frame = cap.read()
# 追跡する枠を決定
track = frame[y:y+h, x:x+w]
#はじめの画像 この画像の中央の色を追跡
cv2.imshow('FIRST_IMG', frame)
#追跡する枠の内部を切り抜き、BGR→HSV変換
hsv_track = cv2.cvtColor(track, cv2.COLOR_BGR2HSV)
#マスク画像の作成
# 3チャンネル(R-赤,G-緑,B-青)の画像を2値化(白黒)
# 画像 最低値 # 最高値
img_mask = cv2.inRange(hsv_track, np.array((0., 60.,32.)), np.array((180.,255.,255.)))
# キーボードの入力が発生しない限りウインドウを表示し続ける
# ウインドウ閉じると下のプログラムが実行される
cv2.waitKey(0)
# ヒストグラムの生成 (ばらつきを知る)
# 入力画像(uint8 or uint32),チャンネル(グレースケールは0),マスク画像,サイズ,画像値の範囲
track_hist = cv2.calcHist([hsv_track], [0], img_mask, [180], [0,180])
# 正規化 画像, 出力のサイズ, 正規化する範囲の下限値, 正規化する範囲の上限値, 正規化の種類
cv2.normalize(track_hist, track_hist, 0, 255, cv2.NORM_MINMAX)
# 指定された精度 繰り返しの最大回数 回数 精度
# 精度or回数のどちらかの条件が満たされたときに繰り返しを終了
term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )
while(True):
#画面取得
ret, frame = cap.read()
if ret == True:
#フレームをHSV変換
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
#ヒストグラムを特徴量として、画像の類似度を求める
dst = cv2.calcBackProject([hsv],[0],track_hist,[0,180], 1)
#物体検出
ret, track_window = cv2.meanShift(dst, track_window, term_crit)
#物体検出で取得した座標
x,y,w,h = track_window
#検出したら長方形で囲む
img_dst = cv2.rectangle(frame, (x,y), (x+w, y+h), (255,255,255), 3)
# xの座標と幅を/2 yの座標と高さを/2
img_x = int(x+w/2)
img_y = int(y+h/2)
#物体検出した中心点を丸
cv2.circle(img_dst, (img_x, img_y), 10, (0,0,180), -1)
#画面への描写
cv2.imshow('PREVIEW', img_dst)
# 左右
if (first_x - x) > 30: # ズレが 30より大きいとき
move_right()
elif (first_x - x) < -30: # ズレが 30より大きいとき
move_left()
else:
move_stop()
# 上下
if (first_y - y) > 50: # ズレが 50より大きいとき
move_up()
elif (first_y - y) < -50: # ズレが 50より大きいとき
move_down()
else:
# モーター停止
move_stop()
time.sleep(0.1)
# キーボードの入力を待つ 1ms
k = cv2.waitKey(1)
# キーボードの q を押したらループを抜ける
if k == ord('q'):
# モーター停止
move_stop()
break
else:
# モーター停止
move_stop()
break
# プログラム終了処理
cap.release()
GPIO.cleanup() # GPIO開放
cv2.destroyAllWindows() # すべてのウインドウを閉じる