このページで示した[Raspberry Pi]のサービスのstart-app-select.pyから,
DIPスイッチ0b11??の状態で呼び出されるraspiAPume.pyのコードです。
また、これで使うハードとその動作確認コードは、このリンク先でで紹介しています。
#!/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
oled.fill(0) # --- クリア
oled.show() # ---表示
# --- Pillowで描画領域を作成 ---
image = Image.new("1", (oled.width, oled.height))
draw = ImageDraw.Draw(image)
font = ImageFont.load_default()# --- フォント設定 ---
def draw_text(txt: str, row=0, showFlag = True, newImageFlag = False, font=font, fill=255):
global image,draw
if newImageFlag:
image = Image.new("1", (oled.width, oled.height)) # イメージ作り直し（全体クリア）
draw = ImageDraw.Draw(image)
# --- テキスト描画 (0＝黒、255＝白)上記設定で、横21文字---
draw.text((0, row*15), txt , font=font, fill=255)
# --- 画面に表示 ---
oled.image(image)
if showFlag: oled.show()
draw_text(f"UMEHOSHI ITA",0)
# 9軸センサー BNO055 制御 ---------------------------------------------------
import smbus # I2C通信をPythonから簡単に扱うためのモジュール
import os
# BNO055 の初期化
BNO055_ADDRESS = 0x28 # BNO055のI2Cアドレス(ADRピンがGNDなら0x28、VDDなら0x29になります)
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ
BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス
bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定
bno055_calib_bin_path="/usr/local/apps/bno055_calib.bin" # キャリブレーションデータファイルパス
# このキャリブレーションデータファイルが存在しなければ、作成する。
try:
os.stat(bno055_calib_bin_path) # ファイルが存在しない場合は、エラー
except OSError:
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)
while True: # NDOFモードで全てのセンサー（SYS, GYR, ACC, MAG）が 3 になるまで動かす。
cal = bus.read_byte_data(BNO055_ADDRESS, 0x35)
sys = (cal >> 6) & 0x03
gyr = (cal >> 4) & 0x03
acc = (cal >> 2) & 0x03
mag = (cal >> 0) & 0x03
draw_text(f"bno055 NDOF Calibrating...",0, showFlag = False,newImageFlag = True) # キャリブレーションの開始
draw_text(f"SYS:{sys}, GYR:{gyr}, ACC:{acc}, MAG:{mag}", 1)
time.sleep(0.5)
if sys == 3 and gyr == 3 and acc == 3 and mag == 3 : break # キャリブレーション完了？
# 各値が 3 になれば完全キャリブレーション完了です
#
calib_data = bus.read_i2c_block_data(BNO055_ADDRESS, 0x55, 22)
with open(bno055_calib_bin_path, "wb") as f:
f.write(bytearray(calib_data)) # オフセット値を読み出して保存
#
draw_text(f"End Calibration",2) # キャリブレーションデータファイル作成終了
# オフセット調整（センサーの取り付け位置や方向を自動補正）
bno055_offset_path="/usr/local/apps/bno055_offset.txt"
# 上記ファイル内に X軸が北を向く時にHeading、水平に置いた時にRollと Pitchが記憶される
make_offset_mode=False # "bno055_offset.txt"オフセット調整ファイル作成モード
try:
with open(bno055_offset_path, "r") as fr:
s = fr.readline() # 一行読み取り(改行を含めて)
draw_text(f"bno055 offset setting",0,newImageFlag = True)
a = s.split(",")
heading_offset = float(a[0]) # オフセット調整値取得
roll_offset = float(a[1])
pitch_offset = float(a[2])
print(f"offset value heading:{heading_offset}, roll:{roll_offset}, pitch:{pitch_offset}")
except:
make_offset_mode = True # bno055_offset.txtのオフセット調整作成モード
bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)
with open(bno055_calib_bin_path, "rb") as f:
calib_data = list(f.read(22)) # 別途bno055_calib_write.pyで行ったキャリブレーションの記憶ファイルを読む
# 設定モードへ
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x00)
time.sleep(0.025)
# キャリブレーションデータを書き込んで、調整情報を復元
bus.write_i2c_block_data(BNO055_ADDRESS, 0x55, calib_data)
# NDOFモードに戻す
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x0C)
time.sleep(0.05)
# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)
def to_signed(val):
"""16ビット値を符号付き整数に変換"""
if val >= 0x8000:
val -= 0x10000
return val
def read_euler():
''' make_offset_modeがTrueの場合は、現在の
heading：左右への向き, roll：左右の傾き, pitch：上下の傾きを返す。
make_offset_modeがFalseの場合は、調整過程のheading, roll, pitchを返す'''
#
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
if make_offset_mode == False: #オフセット調整処理
# X軸が北を向く時にHeadingが0、水平に置いた時にRollと Pitchが0になるオフセット調整
heading = heading - heading_offset # headingズレ補正
heading = heading if heading >= 0 else heading + 360
roll = roll - roll_offset # rollズレ補正
if roll > 90:
roll = -(90 - roll)
elif roll < -90:
roll = -(-90 - roll)
pitch = pitch - pitch_offset # pitchズレ補正
if pitch > 180:
pitch = -(360 - pitch)
elif pitch < -180:
pitch = -(-360 - pitch)
#
return heading, roll, pitch # 左右への向き, 左右の傾き, 上下の傾きを返す
if make_offset_mode: #オフセット調整処理
count = 100 # ロボットを水平にしてはX軸が北を向いてから安定に必要な予想回数
while True:
h, r, p = read_euler()
print(f"Heading: {h:7.2f}, Roll: {r:7.2f}, Pitch: {p:7.2f}")
count -= 1
print(f" {count}が0になるまでに、ロボットを水平にしてはX軸が北を向くように置いてください。")
draw_text(f"Before CountReaches 0",0,showFlag = False, newImageFlag = True)
draw_text(f"Place it horizontally",1,showFlag = False)
draw_text(f" and point it north",2,showFlag = False)
draw_text(f" count:{count}",3)
time.sleep(0.2)
if count <= 0:
with open(bno055_offset_path, "w") as fw:
fw.write(f"{h},{r},{p}\n") # オフセット調整データ書き込み
heading_offset, roll_offset,pitch_offset=h,r,p
break # 上記でX軸が北を向く時にHeading、水平に置いた時にRollと Pitchを記憶
# --------------------------------------------------------------------------------
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()
print("Serverの情報：",hostname, server_addr)
# 測定結果表示用関数---------------------------------------------------------
def info_show(measure_count):
''' IPアドレスとポート番号のタプルのserver_addrの情報と
read_euler()の測定結果と、VL53L1Xの距離測定し、その結果をSSD1306ディスプレイに表示する'''
h, r, p = read_euler()
distance = vl53.distance
#print(f"Heading: {h:7.2f}, Roll: {r:7.2f}, Pitch: {p:7.2f}")
draw_text(f"{server_addr[0]},{server_addr[1]}",0,showFlag = False, newImageFlag = True)
draw_text(f"Heading: {h:8.2f}",1,showFlag = False)
draw_text(f"Roll:{r:5.1f}Pitch:{p:5.1f}",2,showFlag = False)
draw_text(f"Dist.:{distance:7.1f} {int(measure_count):6}",3)
return h, r, p, distance
measure_loop_flag=True
def measure_loop():
measure_count=0
next_measure_time=0
while measure_loop_flag:
if next_measure_time < time.time():
next_measure_time = time.time() + 0.05
measure_count += 0.05
info_show(measure_count)
time.sleep(0.001)
#
#info_show() # ディスプレイへ表示
t_id2 = threading.Thread(target=measure_loop)
t_id2.start()
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:
measure_loop_flag=False
print(f"例外の種類: {type(e)}")
print(f"スタックトレース: {traceback.format_exc()}")
print(e, "sock.close()")
sock.close()
使用している９軸センサー（ BNO055 ）ではセンサーチップ固有のキャリブレーションや、その素子の配置に対するオフセット調整が必要です。