概要

raspberry pi2 を使ってカメラ付きサーボモータを制御した。 python+OpenCVで特定の色の中心を追っかけるようにした。かくかくしてるけどだいたい良い。

はじめに

学生の頃、ロボットに触る機会はよくあったが、だいたい「これが見えたらあっちに歩け」ぐらいの抽象度で、モータの角度を何度にするとか、電気回路がどうとかはさっぱりわからない。仮にも機械工学科目を修めておいてこれではいけないなと思って、とりあえず部屋に転がっていたサーボモータを動かしてみることにした。

使用したもの

秋月のwebカメラ付きサーボ
raspberry pi2
ブレッドボードなど

画像中の特定の色の中心を求める

OpenCVを使って何も考えずに書くと以下のようになる。今回は青色を追っかけるようにした。

import cv2
import numpy as np

capture = cv2.VideoCapture(0)
capture.set(3, 320)
capture.set(4, 240)

dst = np.zeros((320, 240), dtype=np.uint8)

while True:
    _, img = capture.read()
    dst = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    conditions = (dst[:,:,0]<150)*1 * ((dst[:,:,0]>90)*1) * (dst[:,:,1]>100)

    sum_x = 0
    sum_y = 0
    sum_item = 0
    for x,row in enumerate(conditions):
        for y,item in enumerate(row):
            sum_x += x * item
            sum_y += y * item
            sum_item += item
    mean_x = sum_x / sum_item
    mean_y = sum_y / sum_item

    cv2.circle(img, (mean_y, mean_x),10, (0,0,255), -1)
    cv2.imshow("camera", img)
    cv2.waitKey(1)

capture.release()
cv2.destroyAllWindows()

pythonを使っている人にはわかると思うが、このプログラムはとんでもなく遅い。実際にラズパイで実行してみると体感2fpsぐらいだった。というわけで、以下のように書き換えた。

import cv2
import numpy as np

capture = cv2.VideoCapture(0)
capture.set(3, 320)
capture.set(4, 240)

while True:
    _, img = capture.read()
    dst = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    conditions = (dst[:,:,0]<150)*1 * ((dst[:,:,0]>90)*1) * (dst[:,:,1]>100)

    sum_item = np.sum(conditions)
    t_conditions = np.transpose(conditions)

    temp = range(len(conditions))
    mean_x = np.sum([temp * t_condition for t_condition in t_conditions])/sum_item
    temp = range(len(t_conditions))
    mean_y = np.sum([temp * condition for condition in conditions])/sum_item

    cv2.circle(img, (mean_y, mean_x),10, (0,0,255), -1)
    cv2.imshow("camera", img)
    cv2.waitKey(1)

capture.release()
cv2.destroyAllWindows()

これでまあ満足できる程度の早さになる。実行するとこのようになる。

f:id:ksknw:20160228102336p:plain

サーボモータを制御する

回路を適当に組む。サーボの動作電圧が4.8Vだったので、電池を3つつなげて4.5Vで使用した。サーボの信号線をラズパイのPWMができるピンに接続する。

f:id:ksknw:20160228003450j:plain

ラズパイのピンのレイアウトはこちらを参考にした。 PWMで制御するために今回はGPIO13と18を使う。

こちらを参考にしてモータを動かしてみる。

import wiringpi2

PWM_PIN = 13
#PWM_PIN = 18
DUTY_MAX = 123 # 90°
DUTY_MIN = 26  # -90°
DUTY_HOME = 74 # 0°
duty = 0

wiringpi2.wiringPiSetupGpio()
wiringpi2.pinMode(PWM_PIN, wiringpi2.GPIO.PWM_OUTPUT)
wiringpi2.pwmSetMode(wiringpi2.GPIO.PWM_MODE_MS)
wiringpi2.pwmSetClock(375)

wiringpi2.pwmWrite(PWM_PIN, DUTY_HOME)
wiringpi2.delay(100)

def move(degree):
    duty = int((DUTY_MAX-DUTY_MIN)/180.0 * degree + DUTY_HOME)
    wiringpi2.pwmWrite(PWM_PIN, duty)

for degree in [0,45,90,0,-45,-90,0,30,-30,0]:
    move(degree)
    wiringpi2.delay(500)

実行するとこんな感じで動く。

f:id:ksknw:20160228103343g:plain

青色を追っかける

2つのプログラムを組み合わせる。

import wiringpi2
import cv2
import numpy as np

PIN_X = 13
PIN_Y = 18
DUTY_MAX = 123 # 90°
DUTY_MIN = 26  # -90°
DUTY_HOME = 74 # 0°
duty = 0

wiringpi2.wiringPiSetupGpio()
wiringpi2.pinMode(PIN_X, wiringpi2.GPIO.PWM_OUTPUT)
wiringpi2.pinMode(PIN_Y, wiringpi2.GPIO.PWM_OUTPUT)
wiringpi2.pwmSetMode(wiringpi2.GPIO.PWM_MODE_MS)
wiringpi2.pwmSetClock(375)

wiringpi2.pwmWrite(PIN_X, DUTY_HOME)
wiringpi2.pwmWrite(PIN_Y, DUTY_HOME)
wiringpi2.delay(100)

def move(degree_x, degree_y):
    duty_x = int((DUTY_MAX-DUTY_MIN)/180.0 * degree_x + DUTY_HOME)
    duty_y = int((DUTY_MAX-DUTY_MIN)/180.0 * degree_y + DUTY_HOME)
    wiringpi2.pwmWrite(PIN_X, duty_x)
    wiringpi2.pwmWrite(PIN_Y, duty_y)
    wiringpi2.delay(100)

def detect_blue_center(img):
    dst = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    conditions = (dst[:,:,0]<150)*1 * ((dst[:,:,0]>90)*1) * (dst[:,:,1]>100)
    sum_item = np.sum(conditions)
    t_conditions = np.transpose(conditions)

    temp = range(len(conditions))
    mean_x = np.sum([temp * t_condition for t_condition in t_conditions])/sum_item
    temp = range(len(t_conditions))
    mean_y = np.sum([temp * condition for condition in conditions])/sum_item
    return mean_x, mean_y

capture = cv2.VideoCapture(0)
capture.set(3, 320)
capture.set(4, 240)

now_angle_x = 0
now_angle_y = 0

while True:
    _, img = capture.read()
    mean_x, mean_y = detect_blue_center(img)
    cv2.circle(img, (mean_y, mean_x),10, (0,0,255), -1)

    angle_x = (mean_x - 160)/160.0*7
    angle_y = -(mean_y - 120)/120.0*7
    move(now_angle_y + angle_y, now_angle_x + angle_x)
    now_angle_y = now_angle_y + angle_y
    now_angle_x = now_angle_x + angle_x

    cv2.imshow("camera", img)
    cv2.waitKey(1)

capture.release()
cv2.destroyAllWindows()