LoginSignup
29
25

More than 3 years have passed since last update.

@akinkoin株式会社Nextremer

LEGOでカメラアームを作り遠隔でカメラを操作してみた(C言語 + Nuxt.js + Python + Firebase + SkyWay + ラズパイ)

Last updated at Posted at 2021-01-26

はじめに

frontend.pnglego.jpg

以前、WebRTCを使って、ブラウザから自宅のペットの様子を見られるペットカメラを作りました。
WebRTCで常時接続のペットカメラを作ってみた

このペットカメラを遠隔で操縦できたらペットも探しやすいし、楽しい:relaxed:
そして遠隔で物理的に操作するって、なんだか素敵です！
そんなモチベーションで、LEGOでカメラアームを作り遠隔操作してみました。

githubのリポはこちら

全体構成

今回追加した機能は赤線で囲んでいます
jetcam-redline.png

必要なもの

LEGO

LEGO マインドストームEV3
lego
いろいろ使えるLEGOセットで、なかなか楽しいです。
EV3という制御ブロックにモジュールをコードでつないでプログラムで制御できます。
モジュールは、モーター、タッチセンサー、ジャイロセンサー、カラーセンサー、超音波センサーなど。
ギアやベルト、タイヤにシャフトなどもあって機械いじり欲求も満たされます:thumbsup_tone3:

接続アダプタ

Console Adapter for EV3
console-adapter-for-ev3.png
ラズパイのUSBと、LEGO EV3のPortをシリアル接続するための変換コネクター。
現在国内で取り扱っているショップがないので、海外のメーカーから個人輸入しました。

LEGOアーム

先端にWebCamを取り付けてカメラアームにします。
lego.jpg

  • 動き:PAN(旋回)、PEDESTAL(上下高さ)、 Tilt(上下首振り)の3種
  • センサー2つ使って、PAN, PEDESTALの可動域が超えそうになるとモーターが停止
  • センサー1つで、緊急停止ボタン。全モーターが止まります。

LEGO公式サイトの組み立てモデル「ロボットアーム」と「パピー」のパーツを融合して組み立ててます。
基本セットモデル組み立て説明書

LEGOの実行環境

ラズパイから常時シリアル通信でEV3に命令してモジュールを制御したかったので、開発プラットフォームのEV3RTを利用しました。
ちなみに他の開発プラットフォームではEV3DEVが有名です。
今回は、ラズパイと常時連動したかったのでEV3RTを選択しましたが、
スタンドアローンのLEGO上でプログラムを走らせるだけなら、EV3DEVの方が扱いやすそうです。

EV3RT

Mindstorms EV3用の開発プラットフォーム
CやC++などでEV3にモジュールの制御、センサー値取得ができる実行環境です。

EV3RTがどんなものか→TOPPERS/EV3RTとは
C言語で何ができるか→EV3RT C API Reference

EV3で制御する内容をC言語でコンパイルしSDカードに保存。EV3にSDカードを挿して読み込み実行させます。

コンパイルの手順

EV3の環境構築は、全てをイチから構築するとややこしくて長い道のりです。
以下を参考にします。

pfnet-research/chainer-ev3
アフレル社の教材 実践！ Chainerとロボットで学ぶディープラーニング
アフレル社の教材はメールアドレスの登録で無料で優良なPDF教材がDLできます。

アフレル教材の「EV3の実行環境構築」を参考にして、Google Colaboratoryでコンパイルしました。

Google Colaboratory

クラウドでコンパイルすると準備が楽で環境を汚さずに何度でもすぐにやり直せます。
コンパイル環境としてすごくオススメです。:raised_hands_tone3:

大まかにはアフレル教材に沿いますが、EV3へ複数の命令を追加したいので、書き換えたapp.cでコンパイルします。
変更したapp.cをColabノートにマウントさせて、そちらに差し替えてコンパイルすればOKです。

Pythonコードを追加(ラズパイ)

EV3に命令を出すラズパイ側でもPythonでコード追加します。

lego/arm.py

robot.pyから流れてきたdataに従って、LEGOのアームを動かします。

lego/arm.py

import time
from lego.ev3 import EV3


class Lego_arm:
    """レゴアームを制御

    モジュールの指定と、スピード、起動時間をLego EV3に渡す
    """

    def __init__(self):
        # モーターとセンサーの通信ポートの定義.
        self.sensor_pan = EV3.PORT_2
        self.sensor_pedestal = EV3.PORT_3
        self.stop_switch = EV3.PORT_4

        # 回転: pan +時計回り
        self.pan = EV3.PORT_A
        # 上下移動: pedestal +下がる
        self.pedestal = EV3.PORT_B
        # 上下角: tilt -上がる
        self.tilt = EV3.PORT_C

        # モーターとセンサーの初期設定.
        self.ev3 = EV3()
        self.ev3.sensor_config(self.sensor_pan, EV3.TOUCH_SENSOR)
        self.ev3.sensor_config(self.sensor_pedestal, EV3.TOUCH_SENSOR)
        self.ev3.sensor_config(self.stop_switch, EV3.TOUCH_SENSOR)
        self.ev3.motor_config(self.pan, EV3.LARGE_MOTOR)
        self.ev3.motor_config(self.pedestal, EV3.LARGE_MOTOR)
        self.ev3.motor_config(self.tilt, EV3.MEDIUM_MOTOR)

        # 緊急停止の調整値
        self.stop_adjust = {
            'pan': {
                'power': -50,
                'time': 0.1
            },
            'pedestal': {
                'power': 90,
                'time': 0.12
            }
        }

    def move_motor(self, data):
        """lego motorを動かす 調整値
        """

        module = None
        power = int(data['power'])
        interval = float(data['time'])
        if interval < 0.1:
            interval = 0.1
        elif interval > 10:
            interval = 10
        breaking = False

        if data['move'] == 'pan':
            module = self.pan
        elif data['move'] == 'pedestal':
            module = self.pedestal
            breaking = True
        elif data['move'] == 'tilt':
            module = self.tilt
        elif data['move'] == 'stop':
            self.emergency_stop()
            return

        self.ev3.motor_set_power(module, power)

        # 安全装置
        t_end = time.time() + interval
        while time.time() < t_end:
            if self.ev3.touch_sensor_is_pressed(self.stop_switch):
                self.emergency_stop()
                return

            if self.ev3.touch_sensor_is_pressed(self.sensor_pan):
                self.ev3.motor_stop(self.pan, False)
                self.ev3.motor_set_power(self.pan,
                                         self.stop_adjust['pan']['power'])
                time.sleep(self.stop_adjust['pan']['time'])
                print('stop pan')
                break

            if self.ev3.touch_sensor_is_pressed(self.sensor_pedestal):
                self.ev3.motor_stop(self.pedestal, False)
                self.ev3.motor_set_power(self.pedestal,
                                         self.stop_adjust['pedestal']['power'])
                time.sleep(self.stop_adjust['pedestal']['time'])
                print('stop pedestal')
                break

        self.ev3.motor_stop(module, breaking)

    def emergency_stop(self):
        self.ev3.motor_stop(self.pan, False)
        self.ev3.motor_stop(self.pedestal, False)
        self.ev3.motor_stop(self.tilt, False)
        print('Emergency Stop All!')

    def close(self):
        self.ev3.close()
        print("LEGO Close!")

主役のメソッドはmove_motorで、アーム制御のためモーターを動かします。
pedestal(上下高さ)、tilt(上下首振り)は噛ませてるギアの兼ね合いで人間の感覚と値が逆になっています。
(マイナス値を入れると上がるなど)

pan、pedestalはモーターのパワーで壊れないように、可動限界になるとタッチセンサーが信号を送り、停止するように組み込みました。
念の為、全てのモーターが停止する緊急停止用のタッチセンサーも組み込んでいます。
どちらも停止機能が働いた後、ゆっくりと反回転して少しだけ戻るようにしていますが、
これは急停止のみだと、ボタンが押されっぱなしになりシステムが復帰できなくなるケースの回避処置です。

pedestal(アーム上下)だけは、breaking=Trueでモーター停止制御をブレイキングしており、pan,tiltの惰性停止とは異なる方法にしています。
垂直運動でLEGOパーツの自身の重さでアームが垂れてきてしまうのを避けるためです。

lego/ev3

LEGOのモーター、センサーなどのモジュールを制御するために、EV3にシリアル通信で渡すメソッドをまとめています。
プリファード・ネットワークスさんのコードを元にして、使用しないメソッドやクラスは大幅に削除しています。
また、以下の機能を追加しました。

  • motor_set_power: モーターを単体で動かすmethod
  • motor_stop: モーターの停止method
  • motor_rotate: モーターを入力角度分だけ細かく動かすmethod(使わず)

面倒なのが、シリアル通信です。

  • 一度に1倍と(255まで)しか送れない
  • マイナスは使えない

かなり制限が多くてめんどくさいです。複数のパケットを分割、受け取り後再構築する方法もあるのですが、今回はそこまでの制御命令を出すことが無かったのでそのまま使ってます。

Webアプリからの送信データの切り分け

WebアプリからのdataConnectionの送信データを2種類に切り分けました。

  • message: LEDの点滅などのGPIO向けの制御と、プログラム停止命令
  • lego: LEGOを動かす制御用

コード追加と、データの流れは以下となります。
webrtc_control.py
LEGO制御用に、lego.armクラスをインスタンス化して、スレッドにして立ち上げる。

robot.py
ソケット通信で受け取ったjsonが'message'なら、今まで同様にGPIOやプログラム停止に利用する。
jsonが'lego'なら、lego.armクラスのmove_motorメソッドにデーターを渡して制御させる。

後は、lego/arm →lego/ev3→シリアル通信でEV3に伝達される流れになります。

Webアプリの操作画面

操作画面であるWebアプリ側も追加機能しました。
default manual
直感的でゆっくりと安全に動くDefault操作と、直接データを入力できるManual操作の2つを用意しました。
Manualは数値直接入力なので、pedestalと、tiltの+-が感覚と逆になります。また、モーターの出力100%出すこともできるので、俊敏に動かせる反面、物理的に壊れる危険性もあります。:scream:

フロントの追加コード

特筆したい追加部分は以下です。

frontend/pages/operate.vue
    optimizeValue(module){
      this.lego.move = module

      //非常停止
      if (this.lego.move === 'stop') {
        this.sendOrder()
        return
      }

      let value = this.ope[module]
      let plusMinus = 1

      //+-を確認
      if (value === 0) {
        return
      } else if (value < 0) {
        plusMinus = -1
        value = Math.abs(value)
      }

      //挙動の調整
      if (value < 40) {
        this.lego.power = plusMinus
        this.lego.time = value / 10

      } else if (40 <= value && value < 70) {
        this.lego.power = 3 * plusMinus
        this.lego.time = value / 15

      } else if (70 <= value) {
        this.lego.power = 9 * plusMinus
        this.lego.time = value / 20

      }

      // pedestal、tilt 動きと値が正負逆なので調整
      if (['pedestal', 'tilt'].includes(module)) {
        this.lego.power = this.lego.power * -1
      }

      this.sendOrder(module)
    }
  • Webアプリ側にも非常停止機能を実装
  • Default操作でpedestal、tiltの+-を反転して人の感覚と合わせてます。
  • LEGOモーターのパワーと作動時間を緩和

LEGOバッテリはすぐ切れる問題

今回の一番大きな問題は、プログラムコードでも仕様でも無く、LEGOバッテリーがすぐに切れてしまう事です。:skull:

LEGO EV3は電池か付属のバッテリーを使用しますが、ともに3時間ほどで切れてしまいます。
バッテリで起動させながら、ACアダプタをバッテリに挿すこともできますが、ACアダプタの電源が直接EV3に送られるわけでなさそうです。単純にバッテリに充電と、バッテリから起動を同時にやってるようで、繋いでも数時間で切れてしまいます。それとバッテリーにすごく悪そうです。

つまり、LEGO EV3はACアダプタで直接起動させることはできず、数時間しか連続起動できないです。

ACアダプタからミノムシクリップで直接EV3につなぐ(自己責任)

解決策です。メーカー保証もないので、あくまで自己責任です。
ACアダプタのプラグからみのむしクリップなので直接EV3の電池接触版とつなぎます。
これで数日でも稼働することができます。
mushipin.jpg

当たり前ですが、接続する際は以下注意です。

  • 電圧を揃えましょう。9Vです
  • テスターを使って+/-をちゃんと確認しましょう:eyes:

LEGO EV3RT参考記事

参考になる記事はあまり出回ってないですが、以下リンクは大変参考になりました。
10回以上のシリーズ記事でEV3に何をやらせたいかで、わかりやすく記事分類されています。

後輩たちのためのEV3rt講座

おわりに

まだEV3ポートも空きがあり、LEGOモジュールも使っていないパーツがあるので時間あればいろいろと機能追加したいです。そもそもラズパイのGPIOもLEDしか使っていないのでこちらも追加し放題ですね。遠隔でいろんなことが操縦できるって夢が膨らみ、考えるだけでも楽しいです。:confetti_ball:

29
25
1

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
29
25