【自動運転#32】追従させる【ソフトウェアPart20】

環境

Raspberry pi zero

iphone8

エディターはラズパイに初めから付いている、Thonnyを使ってます。

今回は

カメラとサーボモータを使って、追従できると知ったのでやってみました。

部品

• ピンソケット
• 基板
• モータドライバ
• Raspberry Pi Zero
• ジャンプワイヤー
• モバイルバッテリ

配線繋げる

サーボモータ	配線と機能	ラズパイGPIO繋ぐ先
茶色	マイナス線　GND	6番ピン(GND)
橙(赤？)	プラス線	4番ピン(5V)
黄色	制御線	32番ピン(GPIOポート番号12) *1kΩ抵抗を噛ませています。

↑こんな感じです。

Pythonプログラムを作る

早速、プログラムを作っていきます。「servo-chase.py」というファイルにしました。

import cv2
import numpy as np
import pigpio
 
# つないだGPIOの番号です
pin = 12 #GPIOピン番号32

# 初期化
pi = pigpio.pi() 
pi.set_servo_pulsewidth(pin, 1500) 

img = 0
 
# サーボの可動範囲を決めます
MAX = 2100
MIN = 900
HOME = 1500

# 表示するイメージサイズです
WIDTH = 640
HEIGHT = 480
FPS = 24

cap= cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, HEIGHT)
cap.set(cv2.CAP_PROP_FPS, FPS)

# 描画サイズとフレームレートです
width = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
fps = cap.get(cv2.CAP_PROP_FPS)
print("fps:{}　width:{}　height:{}".format(fps, width, height))

def move(move):
    pi.set_servo_pulsewidth(pin, move)

# 初期位置の設定します
now_degree,move_degree = HOME,0
move(HOME)

# 追従スタート 

try:
    while True:
        ret,  frame = cap.read()
        hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
 
        # 追従する色(青)
        low_blue = np.array([160, 155, 84])
        high_blue = np.array([180, 255, 255])
        blue_mask = cv2.inRange(hsv_frame, low_blue, high_blue)
        red = cv2.bitwise_and(frame, frame, mask=blue_mask)

        contours, _ = cv2.findContours(blue_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        contours = sorted(contours, key=lambda x:cv2.contourArea(x), reverse=True)
 
        for cnt in contours:
            (x, y, w, h) = cv2.boundingRect(cnt)

            img = int((y + y + h) / 2)
         
            # かけ算の数値を変えると動きが大きくなるらしいです
            move_degree = now_degree - (img-240)*0.3
        
            
            if move_degree > MIN  and move_degree < MAX:
                move(move_degree)
                now_degree = move_degree
         
            print(img)
            break
 
        cv2.line(frame, (0, img), (320, img), (0, 255, 0), 2)
        cv2.imshow("mask", blue_mask)
        cv2.imshow("Frame", frame)

except KeyboardInterrupt:
    move(HOME)
    pi.set_servo_pulsewidth(pin, 0)
    pi.stop()
    print("\nCamera stop")

finally:
    cap.release()
    cv2.destroyAllWindows()

↑こんな感じです。

私は「青色」を認識させますが、ここの数字を変えると、「赤色」だったり、「緑色」だったりにして、色々試せそうです。

やってみよう！

プログラムができたので、実際やってみます。

まず、LXTerminalを開きます。

“pigpiod”を実行するには”pigpiod”を常駐しておく必要があります。なので、コマンドを入力します。

sudo pigpiod

ラズパイを起動するときに”pigpiod”を自動で実行させるには、”nano”で設定ファイル「rc.local」を編集しなければなりません。

sudo nano /etc/rc.local

と入力して、ファイルを開きます。

開いたら、最後の行にある「exit」の前の行に、

sudo pigpiod

と追記して、「Ctrl」+「X」で終了して、

「Y」キーで上書き保存します。

次に、ファイルが実行できるように”cd”で移動します。

cd プログラムが入ってるフォルダのパス

次に”servo-chase.py”プログラムを実行します。

sudo python3 servo-chase.py

すると、数字がLXTerminal上にバァーーーーって出ます。

物をカメラにかざすと、

物を認識して、サーボモータが動いているのがわかります。

できたー。やったー。

でも、実際は車につけてみて、本当に追従するかですね。

使った部品