Raspberry Piステアリング付ラジコンをM5Stack core2からBLEで操作

前回、ESP32を載せたステアリング付ラジコンをM5stackからUDPで操作する内容についてまとめました。↓

ESP32でDCモーターとサーボモーターをPWM制御、ステアリング付ラジコンの作成（mic...

https://denkenmusic.com/esp32でdcモーターとサーボモーターをpwm制御、ステアリ

　前回、ESP32でDCモーターとサーボモーターの制御を行いました。↓　今回は市販のバギー工作セットにESP32を載せて、ステアリングと前進後退の制御を出来るようにしてみます。コントローラーは前回と同じく、M5Stack core2のスライダーで行います。　　おおまかな概要https://twitter.com/ketunorobio/status/1504741210287603712使う物・ESP32・M5Stack core2・DCモーター・サーボモーター・ブレッドボード、ジャンパーピン・バッテリー・タミヤバギーカークラシックセット・ステアリング固定部品（3Dプリンターで印刷します）　構成M...

今回は、通信にBLEを使ってやってみましたので、その辺りについてまとめようかと思います。

初めは前回と同じ構成、ESP32とM5Stack core2を使ってmicropythonでやってみたのですが、いかんせんmicropythonのBLEライブラリの「ubluetooth」がセントラル側のESP32でうまく機能しなかった（スキャンは出来るがコネクトできない。。）ため、なくなくセントラル側をRaspberryPiに変更しました。bluepyが使えるので、こちらではうまくいきました。

pythonでBLE通信をやってみたい、と思っている方の参考になれば幸いです。

1 概要
- 1.1 使ったもの
- 1.2 構成
2 プログラム
- 2.1 M5Stack core2（micropyton）
- 2.2 Raspberry Pi（python）
3 BLEでラジコン操作

概要

使ったもの

・Raspberry Pi ZERO W(セントラル)

・M5stack core2(ペリフェラル)

・タミヤBUGGY CAR CHASSIS SET

・サーボモーター

・バッテリ

・ブレッドボード、ジャンピングワイヤ

・ステアリング固定治具

※ステアリング固定治具など、冒頭の前回の記事にて詳細をまとめています。

構成

M5Stack ⇒ BLE ⇒ Raspberry Pi ⇒モーター駆動

M5Stack core2のスライダーの値をBLEでRaspberry Piに転送、Raspberry Piで随時値を受信させ、値に応じてモーターを制御。

Raspberry Piはpython、M5Stackはmicropythonでプログラムを作成します。

プログラム

M5Stack core2（micropyton）

from m5stack_ui import *
from socket import socket, AF_INET, SOCK_DGRAM
import time
import network
from m5stack import *
from machine import Pin
from uiflow import *
import ubluetooth

ble = ubluetooth.BLE()
ble.active(True)
 
NUS_UUID = '6E400001-B5A3-F393-E0A9-E50E24DCCA9E'
DC_UUID = "3ef44852-cd43-4da4-8d45-4f9b87fb96c2"
SARVO_UUID = "eb901554-f7ed-4412-bc26-5937e6fb4418"
            
BLE_NUS = ubluetooth.UUID(NUS_UUID)
BLE_DC = (ubluetooth.UUID(DC_UUID), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY)
BLE_SARVO = (ubluetooth.UUID(SARVO_UUID), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY)
            
BLE_UART = (BLE_NUS, (BLE_DC, BLE_SARVO))
SERVICES = (BLE_UART, )
 
ble.gatts_register_services(SERVICES)

ble.gap_advertise(100, "asdf")
            
screen = M5Screen()
screen.clean_screen()
screen.set_screen_bg_color(0xFFFFFF)
    
Slider = M5Slider(50,50,12,150,0,100,0x888888,0x000fff)
Slider2 = M5Slider(150,111,150,12,0,100,0x888888,0x000fff)
Slider.set_bg_color(0x888888)
Slider2.set_bg_color(0x888888)
Slider.set_color(0x000fff)
Slider2.set_color(0x000fff)
Slider.set_range(-100,100)
Slider2.set_range(30, 110)
Slider.set_value(0)
Slider2.set_value(70)

while(True):
    data = str(Slider.get_value())
    ble.gatts_write(0x0015, data)
    print(data)
    Slider2.get_value()
    data2 = str(Slider2.get_value())
    ble.gatts_write(0x0018, data2)
    print(data2)
    time.sleep(0.01)

from m5stack_ui import *

from socket import socket, AF_INET, SOCK_DGRAM

import time

import network

from m5stack import *

from machine import Pin

from uiflow import *

import ubluetooth

ble = ubluetooth.BLE()

ble.active(True)

NUS_UUID = '6E400001-B5A3-F393-E0A9-E50E24DCCA9E'

DC_UUID = "3ef44852-cd43-4da4-8d45-4f9b87fb96c2"

SARVO_UUID = "eb901554-f7ed-4412-bc26-5937e6fb4418"

BLE_NUS = ubluetooth.UUID(NUS_UUID)

BLE_DC = (ubluetooth.UUID(DC_UUID), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY)

BLE_SARVO = (ubluetooth.UUID(SARVO_UUID), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY)

BLE_UART = (BLE_NUS, (BLE_DC, BLE_SARVO))

SERVICES = (BLE_UART, )

ble.gatts_register_services(SERVICES)

ble.gap_advertise(100, "asdf")

screen = M5Screen()

screen.clean_screen()

screen.set_screen_bg_color(0xFFFFFF)

Slider = M5Slider(50,50,12,150,0,100,0x888888,0x000fff)

Slider2 = M5Slider(150,111,150,12,0,100,0x888888,0x000fff)

Slider.set_bg_color(0x888888)

Slider2.set_bg_color(0x888888)

Slider.set_color(0x000fff)

Slider2.set_color(0x000fff)

Slider.set_range(-100,100)

Slider2.set_range(30, 110)

Slider.set_value(0)

Slider2.set_value(70)

while(True):

data = str(Slider.get_value())

ble.gatts_write(0x0015, data)

print(data)

Slider2.get_value()

data2 = str(Slider2.get_value())

ble.gatts_write(0x0018, data2)

print(data2)

time.sleep(0.01)

BLE通信の基本は、ペリフェラル側がアドバタイズしてセントラルに存在を知らせて、セントラルがペリフェラルにコネクトし、以後はサービスUUIDを介して情報のやり取りを行います。

今回の場合ですと、M5StackでUUIDを定義し、メインループで定義したUUIDのハンドルを指定し、そこにスライダーの値を随時格納しています。この辺りのBLE通信に関する詳（python）も過去記事でまとめていますので、是非参考にしてください。↓

ヘルニアクソ野郎エンジニアblog

M5stack、micro:bitで測定した温度、湿度、気圧データをBleでRaspberry Piへ送信し...

https://denkenmusic.com/m5stack、microbitで測定した温度、湿度、気圧データをbleでraspberry

　Bleでセンサーデータのやり取りをするのに結構てこずったので備忘録としてまとめておきます。ペリフェラル　⇒　micro:bit(オンボードのセンサで基板温度測定)、M5stack(ENV3にて温度、湿度、気圧測定)セントラル　⇒　Raspberry Pi(ゲートウェイ)グラフ化　⇒　Raspberru Piからambientへ２つのデバイスでセンサーを使って測定したデータをBleでRaspberry Piに送信。Raspberry Piは受信データをWEBサービスのambientへ転送、グラフ化、の流れとなります。なお、プログラム言語はpythonとmicropythonで作成しました。Cならググれば山ほ...

あとはセントラル側で、このUUIDの値をreadすればOKです。

なお、アドバタイズはループに入ると止まるようになっています。

Raspberry Pi（python）

import time
import bluepy
import RPi.GPIO as GPIO

HANDLE_DC = ハンドル１
HANDLE_SARVO = ハンドル２
devadr = "MACアドレス"

peri = bluepy.btle.Peripheral()

frequency = 5000
frequency2 = 50
pin1 = 16
pin2 = 20
pin3 = 21

GPIO.setmode(GPIO.BCM)
GPIO.setup(pin1, GPIO.OUT)
GPIO.setup(pin2, GPIO.OUT)
GPIO.setup(pin3, GPIO.OUT)

p1 = GPIO.PWM(pin1, frequency)
p2 = GPIO.PWM(pin2, frequency)
p3 = GPIO.PWM(pin3, frequency2)

p1.start(0)
p2.start(0)
p3.start(0)

while(True):
    try:
        peri.connect(devadr, bluepy.btle.ADDR_TYPE_PUBLIC)
        print("connnect")
        break
    except:
        print("connect error")
        pass
        time.sleep(1)


try:
    while(True):
        dc = peri.readCharacteristic(HANDLE_DC)
        sarvo = peri.readCharacteristic(HANDLE_SARVO)
        dc = int(dc.decode())
        sarvo = int(sarvo.decode())
        if dc > 20:
            #time.sleep(0.01)
            p2.ChangeDutyCycle(0)
            p1.ChangeDutyCycle(dc)
            print(dc)
        elif dc < -20:
            dc = str(dc)
            dc = dc[1:]
            dc = int(dc)
            p1.ChangeDutyCycle(0)
            p2.ChangeDutyCycle(dc)
            print(dc)
        elif 20 > dc > -20:
            #msgA == 0:
            p1.ChangeDutyCycle(0)
            p2.ChangeDutyCycle(0)
            print(dc)
        if sarvo > 70:
            sarvo = (70 - (sarvo - 70)) / 10
            p3.ChangeDutyCycle(sarvo)
            print(sarvo)
        elif sarvo < 70:
            sarvo = (70 + (70 - sarvo)) / 10
            p3.ChangeDutyCycle(sarvo)
            print(sarvo)
        elif sarvo == 70:
            saro = sarvo / 10
            p3.ChangeDutyCycle(sarvo)
            print(sarvo)
        print(dc)
        print(sarvo)
        time.sleep(0.01)

except KeyboardInterrupt:
    pass

peri.disconnect()
GPIO.cleanup()

import time

import bluepy

import RPi.GPIO as GPIO

HANDLE_DC = ハンドル１

HANDLE_SARVO = ハンドル２

devadr = "MACアドレス"

peri = bluepy.btle.Peripheral()

frequency = 5000

frequency2 = 50

pin1 = 16

pin2 = 20

pin3 = 21

GPIO.setmode(GPIO.BCM)

GPIO.setup(pin1, GPIO.OUT)

GPIO.setup(pin2, GPIO.OUT)

GPIO.setup(pin3, GPIO.OUT)

p1 = GPIO.PWM(pin1, frequency)

p2 = GPIO.PWM(pin2, frequency)

p3 = GPIO.PWM(pin3, frequency2)

p1.start(0)

p2.start(0)

p3.start(0)

while(True):

try:

peri.connect(devadr, bluepy.btle.ADDR_TYPE_PUBLIC)

print("connnect")

break

except:

print("connect error")

pass

time.sleep(1)

try:

while(True):

dc = peri.readCharacteristic(HANDLE_DC)

sarvo = peri.readCharacteristic(HANDLE_SARVO)

dc = int(dc.decode())

sarvo = int(sarvo.decode())

if dc > 20:

#time.sleep(0.01)

p2.ChangeDutyCycle(0)

p1.ChangeDutyCycle(dc)

print(dc)

elif dc < -20:

dc = str(dc)

dc = dc[1:]

dc = int(dc)

p1.ChangeDutyCycle(0)

p2.ChangeDutyCycle(dc)

print(dc)

elif 20 > dc > -20:

#msgA == 0:

p1.ChangeDutyCycle(0)

p2.ChangeDutyCycle(0)

print(dc)

if sarvo > 70:

sarvo = (70 - (sarvo - 70)) / 10

p3.ChangeDutyCycle(sarvo)

print(sarvo)

elif sarvo < 70:

sarvo = (70 + (70 - sarvo)) / 10

p3.ChangeDutyCycle(sarvo)

print(sarvo)

elif sarvo == 70:

saro = sarvo / 10

p3.ChangeDutyCycle(sarvo)

print(sarvo)

print(dc)

print(sarvo)

time.sleep(0.01)

except KeyboardInterrupt:

pass

peri.disconnect()

GPIO.cleanup()

ペリフェラルのUUIDとハンドルは下記プログラムで調べることができます。↓

import sys
import bluepy
 
def main():
    try:
        peri = bluepy.btle.Peripheral()
        peri.connect("MACアドレス", bluepy.btle.ADDR_TYPE_RANDOM)  # addrTypeがpublic なら、ADDR_TYPE_PUBLICを指定
    except:
        print("device connect error")
        sys.exit()
 
    charas = peri.getCharacteristics()
    for chara in charas:
        print("======================================================")
        print(" UUID : %s" % chara.uuid )
        print(" Handle %04x: %s" % (chara.getHandle(), chara.propertiesToString()))
 
    peri.disconnect()
 
if __name__ == "__main__":
    if len(sys.argv) == 1:
      print('Usage: getHandle.py BLE_DEVICE_ADDRESS')
      sys.exit()
    devadr = sys.argv[1]
 
    main()

import sys

import bluepy

def main():

try:

peri = bluepy.btle.Peripheral()

peri.connect("MACアドレス", bluepy.btle.ADDR_TYPE_RANDOM) # addrTypeがpublic なら、ADDR_TYPE_PUBLICを指定

except:

print("device connect error")

sys.exit()

charas = peri.getCharacteristics()

for chara in charas:

print("======================================================")

print(" UUID : %s" % chara.uuid )

print(" Handle %04x: %s" % (chara.getHandle(), chara.propertiesToString()))

peri.disconnect()

if __name__ == "__main__":

if len(sys.argv) == 1:

print('Usage: getHandle.py BLE_DEVICE_ADDRESS')

sys.exit()

devadr = sys.argv[1]

main()

プログラム名　MACアドレス　を実行すればOKです。

MACアドレスについては、Raspberry Piからbluetoothctlでscan on して確認するか、下記プログラムをペリフェラル側で実行することでも確認する事ができます。

import ubluetooth
import utime

ble=ubluetooth.BLE()
ble.active()== True
# macアドレスを十六進数で表示
def form_mac_address(addr: bytes) -> str:
    return ":".join('{:02x}'.format(b) for b in addr)



    #print(ble.active())
    #macアドレスを表示する
print("BLE current mac address: {}".format(form_mac_address(ble.config("mac"))))