M5BALAをMQTT経由で制御する

#概要

M5BALAをMQTT経由で制御する

• MQTT経由で制御コマンドを受信可能とする
• 制御コマンドはバランス用のパラメータ設定、左右車輪の回転制御指示等
• 受信した制御コマンドに従ってM5BALA+M5GOが動く

##環境

• M5BALA+M5GO
• M5UI.Flow (Firmware 0.7.2)
• iMac (macOS Mojave)
• MQTT Broker (mosquitto version 1.4.15)

#実行例

以下のようなShellスクリプトをMac上で実行することで、連続的にMQTT経由で制御コマンドを送信しM5BALAを動かしました。

m5bala_control.sh

mosquitto_pub -t M5BALA -m I
while :
do
    mosquitto_pub -t M5BALA -m -50
    sleep 1
    mosquitto_pub -t M5BALA -m L=40,R=0
    sleep 3
    mosquitto_pub -t M5BALA -m -50
    sleep 1
    mosquitto_pub -t M5BALA -m L=40,R=0
    sleep 3
    mosquitto_pub -t M5BALA -m -50
    sleep 1
    mosquitto_pub -t M5BALA -m L=0,R=40
    sleep 3
    mosquitto_pub -t M5BALA -m -50
    sleep 1
    mosquitto_pub -t M5BALA -m L=0,R=40
    sleep 3
done

M5BALAをMQTT経由で制御しながら動かしてみました。
実装はこちら。https://t.co/QktH07ibup#M5Stack #M5BALA pic.twitter.com/QQhmad7qie

— 稲澤祐一 (@inasawa) 2018年10月22日

以下はバランス関連のパラメータをすべて0に設定し、子供のおもちゃを（無理やり）連結して動かしてみました。

m5bala_control.sh

#バランス関連のパラメータをすべて0に設定
mosquitto_pub -t M5BALA -m I,K1=0,K2=0,K3=0,K4=0
while :
do
  mosquitto_pub -t M5BALA -m L=-80,R=-250
  sleep 7
  mosquitto_pub -t M5BALA -m L=-240,R=-80
  sleep 7
done

こちらもMQTT経由でM5BALAを制御。バランス関連のパラメータを全て0にし、子供のおもちゃを（無理やり）連結して動かしてみました。https://t.co/QktH07ibup#M5Stack #M5BALA pic.twitter.com/EXVhXl7n5t

— 稲澤祐一 (@inasawa) 2018年10月22日

#M5BALA制御用プログラム

以下のプログラムを M5UI.Flow から、あるいは APP.LIST に登録して実行します。

m5bala_mqtt.py

from m5stack import *
from m5bala  import M5Bala
from umqtt.simple import MQTTClient
import i2c_bus
import machine
import wifisetup
import time
import sys
import gc
import ujson as json

MQTT_BROKER = 'MQTTBroker' # MQTT Broker のアドレスを設定
MQTT_TOPIC  = 'M5BALA'
MQTT_CLIENT_ID = ubinascii.hexlify(machine.unique_id())
GYRO_OFFSETS_CONF = 'gyro_offsets.json'

def init_params():
    global m5bala, gyro_offsets
    m5bala.K1 = 40.0
    m5bala.K2 = 40.0
    m5bala.K3 =  6.5
    m5bala.K4 =  5.5
    m5bala.left  = 0
    m5bala.right = 0
    m5bala.angleX_offset = 0
    m5bala.imu.gyroXoffset = gyro_offsets['X']
    
def display_params():
    global m5bala
    lcd.print('X:{:6.2f}  Y:{:6.2f}  Z:{:6.2f}    '.format(
              m5bala.imu.gyroXoffset, m5bala.imu.gyroYoffset, m5bala.imu.gyroZoffset),
              10, 10)
    lcd.print('K1:{:2d} K2:{:2d} K3:{:4.1f} K4:{:4.1f}  '.format(
              int(m5bala.K1), int(m5bala.K2), m5bala.K3, m5bala.K4),
              10, 40)
    lcd.print('L:{:4d}  R{:4d}  '.format(
              int(m5bala.left), int(m5bala.right)),
              10, 70)

def mqtt_callback(topic, msg):
    global init_params, display_params
    global m5bala, ledbar, last_command_list, gyro_offsets
    # コマンド受信を示すためLEDを光らせる
    ledbar.set(1, 0x070707, num=10)
    command_list = msg.decode('utf-8').upper()
    lcd.textClear(100, 160, last_command_list)
    lcd.text(100, 160, command_list)
    last_command_list = command_list
    for command in command_list.split(','):
        action = command.split('=')
        if len(action) == 1:
            if action[0] == 'I':
                # パラメータを初期値に戻す
                init_params()
            else:
                # 数値のみ指定された場合は左右の値として設定
                try:
                    n = float(action[0])
                    m5bala.left  = n
                    m5bala.right = n
                except:
                    pass
        elif len(action) == 2:
            # Key=n の形式で各パラメータ値を指定
            a = action[0]
            try:
                n = float(action[1])
                if   a == 'L':
                    m5bala.left  = n
                elif a == 'R':
                    m5bala.right = n
                elif a == 'A':
                    m5bala.angleX_offset = n
                elif a == 'G':
                    m5bala.imu.gyroXoffset = gyro_offsets['X'] + n
                elif a == 'K1':
                    m5bala.K1 = n
                elif a == 'K2':
                    m5bala.K2 = n
                elif a == 'K3':
                    m5bala.K3 = n
                elif a == 'K4':
                    m5bala.K4 = n
            except:
                pass
    display_params()
    ledbar.set(1, lcd.BLACK, num=10)

def set_gyro_offsets():
    # デフォルトでは class M5Bala の中で以下の値を設定している。
    #   X=-2.71, Y=-0.01, Z=-0.04
    # この値はM5Stack Fire用なので、M5GOを使用する場合は修正が必要。
    # そもそも個体差がある値なので、最初に設定を行なっている。
    # 
    # ジャイロのオフセット値を取得
    # ・設定ファイルが存在すればファイルの内容を取得
    # ・ファイルが存在しない、あるいはボタンCが押下された場合は
    #   1000回取得して平均値を求め設定ファイルに保存
    global m5bala, gyro_offsets, GYRO_OFFSETS_CONF
    gyro_offsets = None
    try:
        with open(GYRO_OFFSETS_CONF, 'r') as f:
            gyro_offsets = json.loads(f.read())
        print(GYRO_OFFSETS_CONF + ': ' + str(gyro_offsets))
    except OSError:
        print(GYRO_OFFSETS_CONF + ' Not Found')
    if gyro_offsets is None or buttonC.wasPressed():
        lcd.print('Calculate Gyro Offsets',   lcd.CENTER, 40)
        lcd.print('DO NOT MOVE A M5Stack...', lcd.CENTER, 70)
        time.sleep_ms(2000)
        gyroX = 0
        gyroY = 0
        gyroZ = 0
        for i in range(1000):
            gyro = m5bala.imu.gyro
            gyroX += gyro[0]
            gyroY += gyro[1]
            gyroZ += gyro[2]
        gyroX /= 1000
        gyroY /= 1000
        gyroZ /= 1000
        gyro_offsets = {}
        gyro_offsets['X'] = gyroX
        gyro_offsets['Y'] = gyroY
        gyro_offsets['Z'] = gyroZ
        with open(GYRO_OFFSETS_CONF, 'w') as f:
            json.dump(gyro_offsets, f)
        lcd.clear()
    m5bala.imu.setGyroOffsets(gyro_offsets['X'], gyro_offsets['Y'], gyro_offsets['Z'])
    
wifisetup.auto_connect()

lcd.font(lcd.FONT_DejaVu18)
lcd.clear()

ledbar = machine.Neopixel(machine.Pin(15), 10)
ledbar.brightness(0)

try:
    m5bala = M5Bala(i2c_bus.get(i2c_bus.M_BUS))
except Exception as e:
    print(repr(e))
    lcd.print(repr(e), lcd.CENTER, lcd.CENTER, lcd.RED)
    sys.exit()

# ジャイロのオフセット値を取得
set_gyro_offsets()

init_params()
display_params()

# 動作中、一定間隔で AngleX と AngleZ を表示
# AngleY は動作に影響しないのと動作中のオーバーヘッドをなるべく
# 少なくするために表示していない。
# AngleZ は、そのうち角度指定で回転指示をやってみようかと思って
# 表示させている。
lcd.print('AngleX:', 10, 100)
lcd.print('AngleZ:', 10, 130)
lcd.print('Action:', 10, 160)

last_command_list = ''

# MQTT Broker への接続
mqtt = MQTTClient(MQTT_CLIENT_ID, MQTT_BROKER)
mqtt.set_callback(mqtt_callback)
mqtt.connect()
mqtt.subscribe(MQTT_TOPIC)

# mpu6050.py の __init__ の中で「self.preInterval = time.time()」
# となっているのは time.ticks_us() の間違いだと思われるので再設定
m5bala.imu.preInterval = time.ticks_us()
display_interval = 0

while True:
    mqtt.check_msg()
    if time.ticks_diff(time.ticks_ms(), display_interval) > 0:
        # 画面表示間隔を500msとする
        display_interval = time.ticks_add(time.ticks_ms(), 500)
        # 傾き角度を表示
        lcd.print('{:8.3f}  '.format(m5bala.angleX),     100, 100)
        # 水平の回転角度を表示
        lcd.print('{:8.3f}  '.format(m5bala.imu.angleZ), 100, 130)
    
    m5bala.balance()
    gc.collect()

#Mosquitto 立ち上げ

Mosquitto は Mac 上に Docker を使って立ち上げています。

以下から Dockerfile、docker-entrypoint.sh を取得
https://github.com/eclipse/mosquitto/tree/master/docker/1.4.14

Dockerfileを修正し、動作確認のために mosquitto-clients もインストールしています。
（github にある mosquitto の Dockerfile は上記の 1.4.14 が最新ですが、以下のように mosquitto のバージョン指定なしで実際にインストールされたのは 1.4.15 でした）

Dockerfile

FROM alpine

LABEL Description="Eclipse Mosquitto MQTT Broker"

RUN apk --no-cache add mosquitto mosquitto-clients && \
    mkdir -p /mosquitto/config /mosquitto/data /mosquitto/log && \
    cp /etc/mosquitto/mosquitto.conf /mosquitto/config && \
    chown -R mosquitto:mosquitto /mosquitto

COPY docker-entrypoint.sh /
ENTRYPOINT ["/docker-entrypoint.sh"]
CMD ["/usr/sbin/mosquitto", "-c", "/mosquitto/config/mosquitto.conf"]

Dockerイメージの作成および起動

$ docker build -t mosquitto .
$ docker run -d --name mosquitto -p 1883:1883 mosquitto