俺の備忘録

個人的な備忘録です。

Raspberry PiでWifiラジコンを作ってPS3のコントローラで操作してみた(タンク編)

やったこと

やったことは前回(Raspberry PiでWifiラジコンを作ってPS3のコントローラで操作してみた - 俺の備忘録) とほとんど同様だが、今回は車体をタンクにしてみた。
キャタピラは男のロマン

f:id:magayengineer:20160728233823j:plain

システム構成

システム構成も前回(Raspberry PiでWifiラジコンを作ってPS3のコントローラで操作してみた - 俺の備忘録)とほぼ同一。 - ステアリング操作用に使っていたサーボは今回カメラ台制御に流用。 - 左右キャタピラはで別のモータで制御する。

f:id:magayengineer:20160728233859j:plain

用意したもの

これも大部分は前回(Raspberry PiでWifiラジコンを作ってPS3のコントローラで操作してみた - 俺の備忘録)と同様。

Raspberry Pi 3本体 Raspberry Pi3 Model B ボード&ケースセット (Element14版, Clear)-Physical Computing Lab

②モバイルバッテリー Raspberry Piとモータへの電源供給用として購入。

5V 2.1AでRapberry Piに電源供給し、 5V 1 AでDCモータ・サーボモータに電源供給する。
出力が2口あるため、これだけで電源がまかなえる。

(パワーアド)Poweradd Pilot 2GS 10000mAhモバイルバッテリー 急速充電 2USBポート iPhone6 / iPhone6s / iPhone5 / iPad / Xperia / Nexus等対応(シルバー)

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

④USBカメラ(マイク付き)
ELECOM WEBカメラ 200万画素 1/5インチCMOSセンサ マイク内蔵 コンパクトタイプ ブラック UCAM-C0220FBBK ELECOM WEBカメラ 200万画素 1/5インチCMOSセンサ マイク内蔵 コンパクトタイプ ブラック UCAM-C0220FBBK

サーボモータ・モータドライバ
アマゾンで売っているキットについてたものを利用。
アマゾンのレビューはイマイチだが、パーツを個々に注文しなくて良いので、入門用にある程度部品をまとめて揃えたい人には便利。

Raspberry Piで学ぶ電子工作 専用 実験キット 基本部品セット スターターパック (電子部品関連) Raspberry Piで学ぶ電子工作 専用 実験キット 基本部品セット スターターパック (電子部品関連)

⑦ユニバーサルプレートL
車体として使用。
タミヤ 楽しい工作シリーズ No.172 ユニバーサルプレートL 210×160mm (70172) タミヤ 楽しい工作シリーズ No.172 ユニバーサルプレートL 210×160mm (70172)

⑧シンプルギアボックス × 2
左右のキャタピラでひとつずつ。
楽しい工作シリーズ No.167 シングルギヤボックス 4速タイプ (70167) 楽しい工作シリーズ No.167 シングルギヤボックス 4速タイプ (70167)

⑨キャタピラ × 2
楽しい工作シリーズ No.100 トラック&ホイールセット (70100) 楽しい工作シリーズ No.100 トラック&ホイールセット (70100)

⑩ユニバーサルアーム
キャタピラのシャフトの固定に使用。

楽しい工作シリーズ No.143 ユニバーサルアームセット (70143) 楽しい工作シリーズ No.143 ユニバーサルアームセット (70143)

ホムセンで買ったステーっぽい何か × 4個
キャタピラシャフトの固定に利用。

f:id:magayengineer:20160716220451j:plain

⑫その他(家にあったもの) - USBケーブル
- SDカード
- はんだごて
- 太さ3mmのネジとボルト
- ピンドリル(3mm)
- 両面テープ
- ジャンパーピン(オス・オス、オス・メス)

車体組み立て

ポイントだけ説明。

ユニバーサルプレートにギアボックス×2とステーを取り付け、 キャタピラのシャフトを固定する。

f:id:magayengineer:20160728233940j:plain

横から見るとこんな感じ。足回りパーツは全てユニバーサルプレートの片面に寄せることで、もう片面にブレッドボードやラズパイ本体が余裕を持って搭載できる。 f:id:magayengineer:20160728234012j:plain

 回路

モータドライバは以下のように接続。

1つめのモータドライバは以下のように接続。

TA7291Pの1ピン:GND
TA7291Pの2ピン:DCモータと接続
TA7291Pの3ピン:未使用
TA7291Pの4ピン:10KΩの抵抗をつけてTA7291Pの7ピンと接続
TA7291Pの5ピン:GPIO25
TA7291Pの6ピン:GPIO24
TA7291Pの7ピン:10KΩの抵抗をつけてTA7291Pの4ピンと接続
TA7291Pの8ピン:未使用
TA7291Pの9ピン:電源(モバイルバッテリー)の+と接続 TA7291Pの10ピン:DCモータと接続

2つめのモータドライバは1つめのモータドライバと5ピンと6ピンだけ異なる。

TA7291Pの5ピン:GPIO20
TA7291Pの6ピン:GPIO21

サーボモータは以下のように接続。

コネクタ:GND
コネクタ:電源(モバイルバッテリー)の+と接続
コネクタ:GPIO18

タンクの操作方法の検討

キー操作は以下のようにした。 左旋回と右旋回をLボタンとRボタンにマッピングし、十字キーはカメラ制御専用にしたかったが、メインユーザである幼稚園児には難しそうなため、以下のようになった。

上キー:カメラを上に向ける
下キー:カメラを下に向ける
Xボタン:前進
Xボタン + 左キー:左旋回
☓ボタン + 右キー:右旋回

○ボタン:後退
○ボタン + 左キー:右旋回
○ボタン + 右キー:左旋回

クライアント側プログラム

前回(Raspberry PiでWifiラジコンを作ってPS3のコントローラで操作してみた - 俺の備忘録)とほとんど一緒。

#!/usr/bin/python
# -*- coding: utf-8 -*-

import pygame
from pygame.locals import *
import time
from contextlib import closing
import socket


# PS3のコントローラのイベント処理クラス
class Ps3Controller:
    # 初期化
    def __init__(self, function):
        pygame.joystick.init()
        joys = pygame.joystick.Joystick(0)
        joys.init()
        self._function = function

    # 入力受付開始
    def start(self):
        pygame.init()

        while True:
           # イベントをハンドル
           self._function(pygame.event.get())
           time.sleep(0.1)

    # TODO
    def terminate(self):
        pass

# Raspberry PiのIPアドレスまたはホスト名
RASPBERRY_HOST = "192.168.0.30"

# Raspberry側の待ち受けポート
RASPBERRY_PORT = 9999

# pygameの定数とraspberry pi側にTCP/IPで指令を送る際に使用するコマンドのマッピング
TYPE_MAP = {JOYBUTTONDOWN: "DOWN",
            JOYBUTTONUP: "UP"}

BUTTON_MAP = {0: "SELECT",
              1: "ANALOG_LEFT",
              2: "ANALOG_RIGHT",
              3: "START",
              4: "UP",
              5: "RIGHT",
              6: "DOWN",
              7: "LEFT",
              8: "L2",
              9: "R2",
              10: "L1",
              11: "R1",
              12: "TRIANGLE",
              13: "CIRCLE",
              14: "CROSS",
              15: "SQUARE"}

# 扱うイベントのフィルタ
EVENT_FILTER= [(JOYBUTTONUP, 12),
                (JOYBUTTONDOWN, 12),
                (JOYBUTTONUP, 15),
                (JOYBUTTONDOWN, 15),
                (JOYBUTTONUP, 13),
                (JOYBUTTONDOWN, 13),
                (JOYBUTTONUP, 14),
                (JOYBUTTONDOWN, 14),
                (JOYBUTTONUP, 10),
                (JOYBUTTONDOWN, 10),
                (JOYBUTTONUP, 8),
                (JOYBUTTONDOWN, 8),
                (JOYBUTTONUP, 11),
                (JOYBUTTONDOWN, 11),
                (JOYBUTTONUP, 9),
                (JOYBUTTONDOWN, 9),
                (JOYBUTTONUP, 0),
                (JOYBUTTONDOWN, 0),
                (JOYBUTTONUP, 3),
                (JOYBUTTONDOWN, 3),
                (JOYBUTTONUP, 1),
                (JOYBUTTONDOWN, 1),
                (JOYBUTTONUP, 2),
                (JOYBUTTONDOWN, 2),
                (JOYBUTTONUP, 4),
                (JOYBUTTONDOWN, 4),
                (JOYBUTTONUP, 5),
                (JOYBUTTONDOWN, 5),
                (JOYBUTTONUP, 6),
                (JOYBUTTONDOWN, 6),
                (JOYBUTTONUP, 7),
                (JOYBUTTONDOWN, 7)]

# TCP/IPでRaspberry PIにコマンドを送る
def send_command(all_events):
    # 扱いたいイベントだけにフィルタする
    # 今回はボタン関連のイベントのみを扱う(コントローラの傾き等は扱わない)
    target_events = filter(lambda e:e.dict.has_key("button") and (e.type, e.dict["button"]) in EVENT_FILTER, all_events)

    command = ""
    for e in target_events:
        command += "(" + BUTTON_MAP.get(e.dict["button"]) + "," + TYPE_MAP.get(e.type) + ")&"

    # (ボタン名,イベント種別)&(ボタン名,イベント種別)&...の形式でRaspberry Piに送るコマンドを生成
    try:
        if len(command) > 0:
            print command

            bufsize = 4096

            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

            with closing(sock):
                sock.connect((RASPBERRY_HOST, RASPBERRY_PORT))
                sock.send(command)
    except Exception, e:
        print "networkerror"

if __name__ == '__main__':
    try:
        # PS3コントローラ初期化
        c = Ps3Controller(send_command)
        c.start()
    except KeyboardInterrupt:
        print "terminated"

Raspberry側プログラム

前回(Raspberry PiでWifiラジコンを作ってPS3のコントローラで操作してみた - 俺の備忘録)とほとんど一緒。 ただし、ボタンの組み合わせ操作に対応など一部改良。

#!/usr/bin/python
# -*- coding: utf-8 -*-

import RPi.GPIO as GPIO
import wiringpi2 as wiringpi
import threading
import socket
from contextlib import closing

# Wifiラジコンカー
class Tank():
    # 最高スピード(Duty70%)
    SPEED_MAX = 70

    # 最低スピード(Duty40%)
    SPEED_MIN = 40

    # サーボのアングル最小
    ANGLE_MIN = 3.5

    # サーボのアングル最大
    ANGLE_MAX = 10

    def __init__(self):
        GPIO.setmode(GPIO.BCM)

        # 左輪
        GPIO.setup(24, GPIO.OUT)
        GPIO.setup(25, GPIO.OUT)
        # 右輪
        GPIO.setup(20, GPIO.OUT)
        GPIO.setup(21, GPIO.OUT)

        self._p0 = GPIO.PWM(25, 50)
        self._p1 = GPIO.PWM(24, 50)
        self._p2 = GPIO.PWM(20, 50)
        self._p3 = GPIO.PWM(21, 50)
        self._p0.start(0)
        self._p1.start(0)
        self._p2.start(0)
        self._p3.start(0)

        # サーボ
        wiringpi.wiringPiSetupGpio()

        # PWM出力ピンを指定
        wiringpi.pinMode(18, wiringpi.GPIO.PWM_OUTPUT)
        # 周波数を固定するための設定
        wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)
         # 50 Hz。 <- 18750/(周波数)
        wiringpi.pwmSetClock(375)

        self._speed = 60
        self._angle = (Tank.ANGLE_MAX + Tank.ANGLE_MIN) / 2

    # 前進
    def go_ahead(self):
        # 左輪
        self._p0.ChangeDutyCycle(0)
        self._p1.ChangeDutyCycle(self._speed)

        # 右輪
        self._p2.ChangeDutyCycle(0)
        self._p3.ChangeDutyCycle(self._speed)

    # 後退
    def go_back(self):
        # 左輪
        self._p0.ChangeDutyCycle(self._speed)
        self._p1.ChangeDutyCycle(0)

        # 右輪
        self._p2.ChangeDutyCycle(self._speed)
        self._p3.ChangeDutyCycle(0)

    # 左旋回
    def turn_left(self):
        # 左輪
        self._p0.ChangeDutyCycle(self._speed)
        self._p1.ChangeDutyCycle(0)

        # 右輪
        self._p2.ChangeDutyCycle(0)
        self._p3.ChangeDutyCycle(self._speed)

    # 右旋回
    def turn_right(self):
        # 左輪
        self._p0.ChangeDutyCycle(0)
        self._p1.ChangeDutyCycle(self._speed)

        # 右輪
        self._p2.ChangeDutyCycle(self._speed)
        self._p3.ChangeDutyCycle(0)


    # カメラを少し上に向ける
    def up_camera(self):
        if self._angle < Tank.ANGLE_MAX:
            self._angle += 0.5
        wiringpi.pwmWrite(18, int(self._angle * 1024 / 100))

    # カメラを少し下に向ける
    def down_camera(self):
        if self._angle > Tank.ANGLE_MIN:
            self._angle -= 0.5
        wiringpi.pwmWrite(18, int(self._angle * 1024 / 100))

    # スピードアップ
    def speed_up(self):
        if self._speed < Tank.SPEED_MAX:
            self._speed += 10

    # スピードダウン
    def speed_down(self):
        if self._speed > Tank.SPEED_MIN:
            self._speed -= 10

    # 停止
    def stop(self):
        self._p0.ChangeDutyCycle(0)
        self._p1.ChangeDutyCycle(0)
        self._p2.ChangeDutyCycle(0)
        self._p3.ChangeDutyCycle(0)

    # 終了処理
    def terminate(self):
        self._p0.stop()
        self._p1.stop()
        self._p2.stop()
        self._p3.stop()
        GPIO.cleanup()

class CameraAngleControl(threading.Thread):
    def __init__(self):
        super(CameraAngleControl, self).__init__()

    def run(self):
        pass

# クライアントからTCP/IPで送られていたコマンドをハンドルして、
# Carクラスをコントロールするクラス
class TankController:
    def __init__(self, tank):
        self._tank = tank

        self._left = False
        self._right = False
        self._cross = False
        self._circle = False

        # コマンドとTANKの制御メソッドの対応づけ
        self._command_map = {
            "(UP,DOWN)": tank.up_camera,
            "(DOWN,DOWN)": tank.down_camera,
            "(L1,DOWN)": tank.speed_up,
            "(L2,DOWN)": tank.speed_down
        }


        # 状態とTANKの制御メソッドの対応づけ
        self._state_map = {
            0: tank.stop,
            1: tank.go_back,
            2: tank.go_ahead,
            3: tank.stop,
            4: tank.stop,
            5: tank.turn_left,
            6: tank.turn_right,
            7: tank.stop,
            8: tank.stop,
            9: tank.turn_right,
            10: tank.turn_left,
            11: tank.stop,
            12: tank.stop,
            13: tank.stop,
            14: tank.stop,
            15: tank.stop
        }

    def get_state(self):
        state = 0

        if self._left:
            state |= 8

        if self._right:
            state |= 4

        if self._cross:
            state |= 2

        if self._circle:
            state |= 1
        print state
        return state

    def change_flags(self, button, type):
        if button == "LEFT" and type == "UP":
            self._left = False
        elif button == "LEFT" and type == "DOWN":
            self._left = True
        elif button == "RIGHT" and type == "UP":
            self._right = False
        elif button == "RIGHT" and type == "DOWN":
            self._right = True
        elif button == "CROSS" and type == "UP":
            self._cross = False
        elif button == "CROSS" and type == "DOWN":
            self._cross = True
        elif button == "CIRCLE" and type == "UP":
            self._circle = False
        elif button == "CIRCLE" and type == "DOWN":
            self._circle = True

        print self._left, self._right, self._cross, self._circle


    # 送られてきたコマンドに対応するTankクラスの関数を実行する
    def control(self, commands):
        commands = commands.split("&")
        for command in commands:
            if len(command) > 0:
                if self._command_map.has_key(command):
                    # コマンドに対応する制御メソッドをそのまま実行
                    func = self._command_map.get(command)
                    if func != None:
                        func()
                else:
                    # 複数のボタンの押下状態を見て挙動を判定するもの
                    command = command.replace('(', '')
                    command = command.replace(')', '')
                    values = command.split(",")
                    self.change_flags(values[0], values[1])
                    func = self._state_map.get(self.get_state())
                    print func

                    if func != None:
                        func()

    # 終了処理
    def terminate(self):
        self._tank.terminate()

BUFF_SIZE = 4096
QUQUE_SIZE = 50

class CommandReciever():
    def __init__(self, host_name, port, function):
        self._host_name = host_name
        self._port = port
        self._function = function
        self._sock = None

    def start(self):
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self._sock = sock
        with closing(sock):
            sock.bind((self._host_name, self._port))
            sock.listen(QUQUE_SIZE)

            while True:
                conn, address = sock.accept()
                with closing(conn):
                    # コマンド受付
                    msg = conn.recv(BUFF_SIZE)
                    self._function(msg)

    def terminate(self):
        self._sock.close()


HOST_NAME = '192.168.0.30'
PORT = 9999
# メイン処理
if __name__ == '__main__':
    try:
        # Carクラスとコントローラクラスを初期化
        ctrl = TankController(Tank())

        reciever = CommandReciever(HOST_NAME, PORT, ctrl.control)
        reciever.start()
    except KeyboardInterrupt:
        ctrl.terminate()
        reciever.terminate()

今後について

練習や最低限の勉強(リハビリ?)じゃ出来たのでサーボモータを複数使用したロボット制作等をしていきたい! ただし、お小遣いが枯渇してしまったため、しばらくお休み!