[Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車の利用例

BNO055の9軸センサー結果と、レーザー距離結果を、ssd1306ディスプレイに表示

以下で示すコードは、
#!/usr/bin/python3
# -*- coding: utf-8 -*-
#   TCPサーバープログラム（/usr/local/apps/raspiAPume.py）
#  [Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車のサービスから呼び出される

import board
import busio
from adafruit_ssd1306 import SSD1306_I2C # SSD1306ディスプレイ用
import adafruit_vl53l1x # VL53L1X使用 レーザー測距センサーモジュール用
# オープンソースハードウェアの設計・製造・販売を行うアメリカの企業のAdafruit（エイダフルート）モジュール利用
from PIL import Image, ImageDraw, ImageFont
import time

i2c = busio.I2C(board.SCL, board.SDA)# --- I2C初期化 ---

# --- SSD1306ディスプレイ初期化 (128x64の場合) -----------
oled = SSD1306_I2C(128, 64, i2c)
oled.contrast(128) # 0?255

def oled_clear():
   oled.fill(0)   # --- クリア
   oled.show() # ---表示

oled_clear() # --- 画面をクリア ---

# --- Pillowで描画領域を作成 ---
image = Image.new("1", (oled.width, oled.height))
draw = ImageDraw.Draw(image)
font = ImageFont.load_default()# --- フォント設定 ---

def draw_text(txt: str, row=0, font=font, fill=255):
   # --- テキスト描画 (0＝黒、255＝白)上記設定で、横21文字---
   draw.text((0, row*15), txt , font=font, fill=255)
   # --- 画面に表示 ---
   oled.image(image)
   oled.show()

draw_text(f"UMEHOSHI ITA",0)

# 9軸センサー BNO055 制御 ---------------------------------------------------
import smbus # I2C通信をPythonから簡単に扱うためのモジュール

BNO055_ADDRESS = 0x28  # BNO055のI2Cアドレス
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ
BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス

bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定

bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)
# センサーをリセット（0x3FのSYS_TRIGGERレジスタのビット7をセット）
bus.write_byte_data(BNO055_ADDRESS,0x3F, 0x20)
time.sleep(0.7)  # リセット後は再起動まで時間がかかる
# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)

# センサーフュージョンを有効にするNDOFモードに変更
bus.write_byte_data(BNO055_ADDRESS,BNO055_OPR_MODE, 0x0C) # NDOFモードへ
time.sleep(0.05)
print("BNO055をNDOF（Fusion）モードで初期化しました。（自動的にキャリブレーションが実行）")

def to_signed(val):
    """16ビット値を符号付き整数に変換"""
    if val >= 0x8000:
        val -= 0x10000
    return val

def read_euler():
    data = bus.read_i2c_block_data(BNO055_ADDRESS, BNO055_EULER_H_LSB, 6)
    # 各要素が 1 バイト（0?255）の整数を6個のリストで得られる。（1 LSB = 1/16 度）

    # データはリトルエンディアン形式（下位→上位の順）
    heading = (data[1] << 8) | data[0]  # 方位角（北基準のYAW）
    roll    = (data[3] << 8) | data[2]  # ロール角（左右の傾き）
    pitch   = (data[5] << 8) | data[4]  # ピッチ角（前後の傾き）

    # ロールとピッチは符号付き
    roll = to_signed(roll)
    pitch = to_signed(pitch)

    # スケーリング（1 LSB = 1/16 度）
    heading = heading / 16.0 # 
    roll    = roll / 16.0
    pitch   = pitch / 16.0
    return heading, roll, pitch

time.sleep(0.2)
h, r, p = read_euler()
msg_HRP=f"Heading: {h:7.2f}, Roll: {r:7.2f}, Pitch: {p:7.2f}"
print(msg_HRP)
draw_text(msg_HRP,2)

# ----------------------------------
vl53 = adafruit_vl53l1x.VL53L1X(i2c)# VL53L1X使用 レーザー測距センサーモジュール初期化
print("VL53L1X Start measuring...")
vl53.start_ranging()

distance = vl53.distance
time.sleep(0.5)
print(f"Distance: {distance} mm")

# umetcp umeusb 通信関連----------------------------------------------------
import os
import umetcp
from umetcp import send_message, receiveData
import socket
import umeusb
import traceback

sock = None # クライアントと通信するソケット

def my_tcp_receive_file_func(filepath):
    ''' 受信したumehoshiアプリ用「.umh」のファイルより、
     「UME専用Hexコマンド」の文字列をusbへ出力する'''
    name,ext = os.path.splitext(filepath)
    if not ext == ".umh" : return
    with open( filepath ) as f: ss=f.readlines()
    ss="".join(ss[1:])
    print(ss)
    umeusb.send_cmd(ss, quantity=0) # TCPで受信した 「.umh」データを[UMEHOSHI ITA]へ送る

umetcp.tcp_receive_file_func = my_tcp_receive_file_func # tcpファイル受信データの処理を置き換え

def my_tcp_recieve_message(msg):
   umeusb.send_cmd(msg)

umetcp.tcp_receive_message_func=my_tcp_recieve_message  # TCO受信文字列（UME専用Hexコマンド）処理を置き換え

def my_usb_receive_func(bin):
    'usb受信のイベントで実行するデフォルト処理'
    global sock
    #print("-----------",bin)
    ss = bin.decode('utf-8')
    a=ss.split('\n')
    for s in a:
        s = s.strip()
        if sock != None:
            print(s)
            send_message(sock, s) # [UMEHOSHI ITA]からの応答メッセージをTCPで返す
        else: print( s )

umeusb.usb_receive_func = my_usb_receive_func # USB受信データの処理を置き換える。

umeusb.init_sub()
import threading
t_id = threading.Thread(target=umeusb.read_loop)
t_id.start()

#ip="192.168.4.1"
ip=umetcp.get_wlan0_ip() # IPアドレス取得
while ip == None:
   ip=umetcp.get_wlan0_ip()
   if ip : break
   time.sleep(0.1)

umeusb.send_cmdfile("/usr/local/apps/uStartInit.umh") # ロボット初期化
server_addr =(umetcp.get_wlan0_ip(), 59154)
hostname=socket.gethostname()
draw_text(f"{server_addr[0]},{server_addr[1]}",1)

print("Serverの情報：",hostname, server_addr)
serversock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serversock.bind(server_addr)  # IPとポート番号を指定します
print("接続要求を待つ")
serversock.listen()
while True:
   print("接続を待って、接続してきたら許可")
   sock, address = serversock.accept()#サーバの接続受け入れ
   print("接続相手：",address)
   try:
      receiveData( sock ) # 受信ループ
   except Exception as e:
      print(f"例外の種類: {type(e)}")
      print(f"スタックトレース: {traceback.format_exc()}")
      print(e, "sock.close()")
   sock.close()