Arduinoを使ってボールを追いかける車を作った

MXNet 物体検出 Arduino

はじめに

カメラ画像をWi-FiでPCに送信。
PCでカメラ画像からボールの位置を確認、XBeeを使ってArduinoにモータを動かす指示を送信。
上記を繰り返すことによってボールを追いかけることができた。
ボールの認識はMXNetの物体検出モデルを使用した。（こちらを参照）

購入したもの

Arduino Uno R3（The Arduino Starter Kitとして購入）
Hブリッジモータードライバ [L293DNE] （The Arduino Starter Kitに付属）

サンハヤト SAD-101 ニューブレッドボード 

XBee ZB（S2C）ワイヤアンテナ型		×2
XBee エクスプローラ USB			（Sparkfun Electronics）
Arduino用XBee スタック可能シールド V2	（SeeedStudio）

APEMAN A79 アクションカメラ

Hitecコネクタ付き、6Vニッケル水素2000mAhバッテリーパック（モーター用に購入）
cheero Canvas 3200mAh IoT機器対応 モバイルバッテリー（Arduino用に購入）
変換名人 USB A(オス) -USB B(オス)変換ケーブル [ 約20cm ] 

タミヤ　ロングユニバーサルアームセット （オレンシﾞ）
タミヤ　ユニバーサルプレート用スライドアダプター
タミヤ　スポーツタイヤセット （56mm径）
タミヤ　ボールキャスター （2セット入）
タミヤ　ユニバーサルプレート （2枚セット）
タミヤ　ダブルギヤボックス（左右独立4速タイプ）
タミヤ　1.5A 平行コード （5m）

ダイソー　すべりどめシート

適当なボール
  • Arduinoはユニバーサルプレート用スライドアダプターを使ってユニバーサルプレートに固定した。適当なスペーサーがあればスライドアダプターはいらないと思う。
  • ブレッドボードは両面テープでユニバーサルプレートに張り付けた。
  • バッテリー類はすべりどめシートを下に敷いて置いただけ。平地を走行する分にはこれで十分であった。
  • APEMAN A79のカバーをユニバーサルプレートにボンドで固定した。
  • タミヤ　1.5A 平行コード （5m）の商品説明には「芯線が30本と多く、ハンダを使わずに手でひねった結線でも接触不良になりにくいのもポイントです。」と書かれている。それを信じてはんだ付けはしていないが今のところ問題ない。そもそもはんだごてを持っていない（笑）。

モータードライバの使用法

動作環境

Python 3.7.8

certifi==2020.6.20
chardet==3.0.4
cycler==0.10.0
gluoncv==0.7.0
graphviz==0.8.4
idna==2.6
kiwisolver==1.2.0
matplotlib==3.3.0
mxnet-cu101 @ https://repo.mxnet.io/dist/python/cu101/mxnet_cu101-1.6.0-py2.py3-none-win_amd64.whl
numpy==1.16.6
opencv-python==4.3.0.38
Pillow==7.2.0
portalocker==2.0.0
pyparsing==2.4.7
pyserial==3.4
python-dateutil==2.8.1
pywin32==228
requests==2.18.4
scipy==1.5.2
six==1.15.0
tqdm==4.48.2
urllib3==1.22

Pythonコード

import mxnet as mx
import gluoncv

import serial, time
import cv2, queue, threading

delay = 50

class VideoCapture:

    def __init__(self, name):
        self.cap = cv2.VideoCapture(name)
        self.q = queue.Queue()
        t = threading.Thread(target=self._reader)
        t.daemon = True
        t.start()

  # read frames as soon as they are available, keeping only most recent one
    def _reader(self):
        while True:
            ret, frame = self.cap.read()
            if not ret:
                break
            if not self.q.empty():
                try:
                    self.q.get_nowait()   # discard previous (unprocessed) frame
                except queue.Empty:
                    pass
            self.q.put(frame)

    def read(self):
        return self.q.get()

def motion(r_PWM, l_PWM, delay_time):
    a = r_PWM.to_bytes(1, 'big')
    b = l_PWM.to_bytes(1, 'big')
    c = delay_time.to_bytes(1, 'big')

    ser.write(a)
    ser.write(b)
    ser.write(c)

ser =serial.Serial("COM4", 9600)
time.sleep(2)

ctx = mx.gpu()
# Load the model
classes = ['ball']
net = gluoncv.model_zoo.get_model('ssd_512_mobilenet1.0_custom', classes=classes, pretrained=False, root='./model')
net.load_parameters('ssd_512_mobilenet1.0_ball.params')
net.collect_params().reset_ctx(ctx)
# Compile the model for faster speed
net.hybridize()

cap = VideoCapture('rtsp://192.72.1.1:554/liveRTSP/av4/track0')
time.sleep(1)

#最初にストップした状態を送る
motion(0,0,100)

while True:

    frame = cap.read()
    frame = mx.nd.array(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)).astype('uint8')    
    rgb_nd, frame = gluoncv.data.transforms.presets.ssd.transform_test(frame, short=320)
  
    # Run frame through network
    class_IDs, scores, bounding_boxes = net(rgb_nd.as_in_context(ctx))

    if ser.in_waiting != 0:
        ser.reset_input_buffer()
        
        if scores[0][0] > 0.55:
            x_min = bounding_boxes[0][0][0]
            y_min = bounding_boxes[0][0][1]
            x_max = bounding_boxes[0][0][2]
            y_max = bounding_boxes[0][0][3]

            width = x_max - x_min
            height = y_max - y_min

            if max(width, height) < 135:

                #右旋回
                if x_min > 360:
                    l_PWM = 180
                    if x_min > 480:
                        r_PWM = 110
                    else:
                        r_PWM = 130    
                    motion(r_PWM, l_PWM, delay)

                #左旋回
                elif x_max < 280:
                    r_PWM = 180
                    if x_max <160:
                        l_PWM = 110
                    else:
                        l_PWM = 130    
                    motion(r_PWM, l_PWM, delay)
                
                #前進
                else:
                    r_PWM = 180
                    l_PWM = 180
                    motion(r_PWM, l_PWM, delay)
            else:
                motion(0, 0, 25) #ボールが近いのでストップ
        else:
            motion(0, 0, 25) #ボールを見失ったのでストップ
  
    # Display the result
    img = gluoncv.utils.viz.cv_plot_bbox(frame, bounding_boxes[0], scores[0], class_IDs[0], class_names=net.classes, thresh=0.55)
    gluoncv.utils.viz.cv_plot_image(img)
    
    #time.sleep(0.2)
    
    # escを押したら終了
    if cv2.waitKey(1) == 27:
        break

ser.close()
cv2.destroyAllWindows()

Arduinoスケッチ

const int PIN_1A = 4;
const int PIN_2A = 5;
const int PIN_3A = 6;
const int PIN_4A = 7;

const int motor_right = 9;
const int motor_left = 10;

int delay_time;
int right_pwm;
int left_pwm;

void setup() {
  pinMode(PIN_1A, OUTPUT);
  pinMode(PIN_2A, OUTPUT);
  pinMode(PIN_3A, OUTPUT);
  pinMode(PIN_4A, OUTPUT);
  pinMode(motor_right, OUTPUT);
  pinMode(motor_left, OUTPUT);
  Serial.begin(9600);

  //right_wheel前進
  digitalWrite(PIN_1A, LOW);
  digitalWrite(PIN_2A, HIGH);
  //left_wheel前進
  digitalWrite(PIN_3A, HIGH);
  digitalWrite(PIN_4A, LOW);
}

void loop() { 
  if(Serial.available()>2){
    
    right_pwm = Serial.read();
    left_pwm = Serial.read();
    delay_time = Serial.read()*4;
    
    analogWrite(motor_right, right_pwm);
    analogWrite(motor_left, left_pwm);
    
    delay(delay_time);
    Serial.write(255);
  }
}

このエントリーをはてなブックマークに追加