###前提・実現したいこと
Raspberrypiのカメラモジュールを用いた追跡撮影システムとして以下のソースコードで作成しました。
更なる追加の機能として顔認識をした瞬間にGPIOに接続したLEDが点灯し顔が動いて、カメラの画角から外れ30秒程経てば消灯し他後サーボモーターによりカメラが初期位置に戻るという事を追加しようと考えています。
それらのデータ収集としてモーターの角度情報も出力できればと思っているのですがどのようにしたらいいでしょうか?よろしくお願い致します。
###該当のソースコード
python
1# -*- coding: utf-8 -*- 2import picamera 3import picamera.array 4import cv2 5import math 6import wiringpi2 as wiringpi 7import pygame 8import sys 9 10pygame.init() 11size=(320,240) 12screen = pygame.display.set_mode(size) 13 14def pygame_imshow(array): 15 b,g,r = cv2.split(array) 16 rgb = cv2.merge([r,g,b]) 17 surface1 = pygame.surfarray.make_surface(rgb) 18 surface2 = pygame.transform.rotate(surface1, -90) 19 surface3 = pygame.transform.flip(surface2, True, False) 20 screen.blit(surface3, (0,0)) 21 pygame.display.flip() 22 23cascade_path = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml" 24cascade = cv2.CascadeClassifier(cascade_path) 25 26def getServoDutyHw(id, val): 27 val_min = 0 28 val_max = 4095 29 # デューティ比0%を0、100%を1024として数値を入力 30 servo_min = 36 # 50Hz(周期20ms)、デューティ比3.5%: 3.5*1024/100=約36 31 servo_max = 102 # 50Hz(周期20ms)、デューティ比10%: 10*1024/100=約102 32 if id==1: 33 servo_min = 53 34 servo_max = 85 35 duty = int((servo_min-servo_max)*(val-val_min)/(val_max-val_min) + servo_max) 36 # 一般的なサーボモーターはこちらを有効に 37 #duty = int((servo_max-servo_min)*(val-val_min)/(val_max-val_min) + servo_min) 38 if duty > servo_max: 39 duty = servo_max 40 if duty < servo_min: 41 duty = servo_min 42 return duty 43 44PWM0 = 18 45PWM1 = 19 46 47# wiringPiによるハードウェアPWM 48wiringpi.wiringPiSetupGpio() # GPIO名で番号を指定する 49wiringpi.pinMode(PWM0, wiringpi.GPIO.PWM_OUTPUT) # 左右方向のPWM出力を指定 50wiringpi.pinMode(PWM1, wiringpi.GPIO.PWM_OUTPUT) # 上下方向のPWM出力を指定 51wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS) # 周波数を固定するための設定 52wiringpi.pwmSetClock(375) # 50 Hz。18750/(周波数) の計算値に近い整数 53# PWMのピン番号とデフォルトのパルス幅をデューティ100%を1024として指定。 54# ここでは6.75%に対応する69を指定 55wiringpi.pwmWrite(PWM0, 69) 56wiringpi.pwmWrite(PWM1, 69) 57 58prev_x = 160 59prev_y = 120 60prev_input_x = 2048 61prev_input_y = 2048 62 63with picamera.PiCamera() as camera: 64 with picamera.array.PiRGBArray(camera) as stream: 65 camera.resolution = (320, 240) 66 67 while True: 68 # stream.arrayにBGRの順で映像データを格納 69 camera.capture(stream, 'bgr', use_video_port=True) 70 # 映像データをグレースケール画像grayに変換 71 gray = cv2.cvtColor(stream.array, cv2.COLOR_BGR2GRAY) 72 # grayから顔を探す 73 facerect = cascade.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=2, minSize=(30,30), maxSize=(150,150)) 74 75 if len(facerect) > 0: 76 # 複数見つかった顔のうち、以前の顔の位置に最も近いものを探す 77 mindist = 320+240 78 minindx = 0 79 indx = 0 80 for rect in facerect: 81 dist = math.fabs(rect[0]+rect[2]/2-prev_x) + math.fabs(rect[1]+rect[3]/2-prev_y) 82 if dist < mindist: 83 mindist = dist 84 minindx = indx 85 indx += 1 86 87 # 現在の顔の位置 88 face_x = facerect[minindx][0]+facerect[minindx][2]/2 89 face_y = facerect[minindx][1]+facerect[minindx][3]/2 90 91 # 元の画像(system.array)上の、顔がある位置に赤い四角を描画 92 cv2.rectangle(stream.array, tuple(facerect[minindx][0:2]),tuple(facerect[minindx][0:2]+facerect[minindx][2:4]), (0,0,255), thickness=2) 93 94 dx = face_x-160 # 左右中央からのずれ 95 dy = face_y-120 # 上下中央からのずれ 96 97 # サーボモーターを回転させる量を決める定数 98 ratio_x = 3 99 ratio_y = -3 100 101 duty0 = getServoDutyHw(0, ratio_x*dx + prev_input_x) 102 wiringpi.pwmWrite(PWM0, duty0) 103 104 duty1 = getServoDutyHw(1, ratio_y*dy + prev_input_y) 105 wiringpi.pwmWrite(PWM1, duty1) 106 107 # サーボモーターに対する入力値を更新 108 prev_input_x = ratio_x*dx + prev_input_x 109 if prev_input_x > 4095: 110 prev_input_x = 4095 111 if prev_input_x < 0: 112 prev_input_x = 0 113 prev_input_y = ratio_y*dy + prev_input_y 114 if prev_input_y > 4095: 115 prev_input_y = 4095 116 if prev_input_y < 0: 117 prev_input_y = 0 118 119 # 以前の顔の位置を更新 120 prev_x = face_x 121 prev_y = face_y 122 123 # pygameで画像を表示 124 pygame_imshow(stream.array) 125 126 # "q"を入力でアプリケーション終了 127 for e in pygame.event.get(): 128 if e.type == pygame.KEYDOWN: 129 if e.key == pygame.K_q: 130 pygame.quit() 131 sys.exit() 132 133 # streamをリセット 134 stream.seek(0) 135 stream.truncate() 136
あなたの回答
tips
プレビュー