トップ
LSM9DS1(9軸センサ)を付けたRaspberry Pi Pico W より受け取ったTCP受信データで、UnityのGameObjectを動かす
作品の構成
このページは、「
その３」の続きで
Raspberry Pi Pico WとLSM9DS1_I2C以外は、基板に半田付けしてそれをケースへの組み込み、開発する過程を紹介します。
（ケースは、タカチ電機工業 TWN7-3-13Wでサイズ(W)65(H)25.8(D)130です。）
基板には、新たに電源用スライドスイッチと、プルアップで使うSW（GPIO10とGPIO11に繋げた）を追加しています。
Raspberry Pi Pico Wの電源供給はUSBでなくVSYS直接にしています。
3.6Vにするか迷いましたが、ニッケル水素電池単4を４個の4.8Vで供給します。
（Wi-Fiを使用する Pico W は、一時的に高電流を要求するため、入力電圧が低すぎると Wi-Fi の動作が不安定になるからです。）
この場合、フル充電時に電圧が5.6V近くなるため、最大定格5.5Vを超えないようにショットキーダイオードも追加しました。
上記の写真で、左側に上から電源用のスライドスイッチ、GPIO10 に繋がるタクトスイッチ、GPIO11に繋がるタクトスイッチ が並んでいます。
（プルアップなので、押すとLowの入力です。）
赤のLEDはGPIO14に接続されて、Hiで点灯です。
ケースをネジで閉めるとLEDが見えなくなり、USBの接続も出来なくなります。
ですが
このリンク先で紹介したように、
これ自体がWifiアクセスポイントにして、pythonのコード変更を遠隔操作でファイル転送し、実行できるようにしました。
具体的には、次の3つファイルをPico W に置くだけでできます。
ですが、全く異なるネットワーク設定の9軸センサー作品を単純に両立させることができませんでした。
上記setapモジュールの用ネットワーク設定は自身をアクセスポイントにするAP（アクセスポイント）モードです。
対して、9軸センサーの作品は既存のWi-Fiに接続すSTA（ステーション）モードを使うからです。
そこで、「9軸センサー作品用」と「開発環境用」に使うmain.pyの内容の２つを用意して置き換える手法にしました。
「開発環境用」はファイル操作なので、main.pyの内容を置き換えることができます。
しかし、「9軸センサー作品用モジュール」で不具合があった場合は、元の「開発環境用」の内容に変更する手法が必要です。
（それが出来ないと、ネジでケースを開いて、USBで接続してThonnyの開発環境でファイル転送をしなければなりません。）
そこで、「9軸センサー作品用」のmain.pyの先頭で、main.pyを変更する次のモジュール（chg_main_ap.py）をimportすることにしました。
# chg_main_ap.py : GPIO10のボタンを起動時に押すと、main.py の内容を"main_ap.py"の内容に変更
from machine import Pin
def fcopy(spath, dpath): # spathのファイルの内容でdpathファイルを生成
with open(spath, "rb") as f:
bin=f.read()
with open(dpath, "wb") as f:
f.write(bin)
SW_PIN=10 # GPIO10 ( 14Pin )
sw=Pin( SW_PIN, Pin.IN, Pin.PULL_UP ) # ボタンに繋がるGPIO10を、プルアップで使うように変更
if sw.value() == 0: # ボタン押された?
fcopy("main_ap.py","main.py") # main.pyをmain_ap.pyの内容で生成
このモジュールを起動時に実行させることで、起動時にGPIO10のボタンを押すとmain.py の内容を
"main_ap.py"の内容に入れ替えできます。
（この仕組みを応用することでケースで閉じた状態でも、起動時のボタン操作で次に起動する作品を変更する芸当ができるようになりました。）
なお、上記の場合はボタン操作で切り替え対象となる
"main_ap.py"を別途にpiocWに配置しておく必要があります。
この
"main_ap.py"の内容は、ファイル操作用の
main.pyと同じ内容で、現在(202505)は次のようにしています。
# "main_ap.py"
import setap # 自身をアクセスポイントにして'192.168.4.1'のIPにする。
import server # 192.168.4.1:59154 のファイル操作用サーバー（開発時に使うサーバー）
# （ここで 上記モジュール実行後に実行させたいモジュールのimport記述も可能）
そして、このLSM9DS1(9軸センサ)用作品の
main.pyは次の内容で、piocWに配置します。
import chg_main_ap # GPIO10のボタンを起動時に押すと、main.py の内容を"main_ap.py"の内容に変更するモジュール
import sensor2 # LSM9DS1(9軸センサ)作品モジュールでローカルWifiに接続して、ブロードキャストに応答したPCへ、データをUDPで送り続けるモジュール
このモジュール内容は、起動時にGPIO10のボタンを押すと、入れ替わってしまうので別途にローカルマシンでバックアップ(main_sensor2.py)しています。
そして、このモジュールで実行させる
sensor2.pyは、
このリンク先で作った>sensor1.pyを次のように大きく変更しています。
上記で使う現時点(202506)のsensor2.pyの内容
sensor2.pyの動作概要
- GP14接続のLEDを点灯
- Wifiのアクセスポイント（picowtcpモジュールにSSIDとパスワードが記述）に接続するまで繰り返す。
- GP14接続のLEDを消灯
- "255.255.255:59001"のブロードキャストの送信で、センサー側のIPアドレスとポート番号を知らせる繰り返し
繰り返し内で、PC側のIPアドレスとUDPユニキャストポート番号を受信できれば、繰り返し終了。
- LSM9DS1の初期化、レジスタなどの設定
- GP14接続のLEDを点灯
- 次の繰り返し
- センサー情報が得られたら次の情報をUDP送信し、WifiのオンボードのLED点灯状態を逆転させ、GP14接続のLEDを消灯（節電）。
Accelerometerの'A'、またはgyroscopeの'G'、
またはmagneticの'M'、またはgyroscopeの'S'の1byte
（'S'の時は,YとZが無くなって,XにSW情報が記憶） |
Xの2byte情報
0x100:GPIO10ボタン
0x200:GPIO11ボタン
|
Yの2byte情報 |
Zの2byte情報 |
このソース（sensor2.py）は、下記の内容です。
# sensor2.py
import socket
import network
import struct
from machine import Pin
import utime
import gc
sw10=Pin( 10, Pin.IN, Pin.PULL_UP )
sw11=Pin( 11, Pin.IN, Pin.PULL_UP )
def print_pass(*args, **kwargs):
pass
#print=print_pass
target_pin = Pin(14,Pin.OUT) # 使用LED(GPIO14接続)利用の準備
onboard_led = Pin( "LED", Pin.OUT )
SSID='〇〇'# デフォルトのWifiアクセスポイント用
PW = '〇〇〇'# 上記に接続するためのパスフレーズ
wifi_s = [ # 複数のWifiのSSIDとパスフレーズのタプルが並ぶリスト
(SSID,PW), # デフォルトアクセスポイント
('1F-AP','abc1Fxyz'), # 1階のアクセスポイント
('2F-AP','abc2Fxyz'), # 2階のアクセスポイント
('3F-AP','abc3Fxyz'), # 3階のアクセスポイント
]
def wifi__connect():# STA（ステーション）モードで既存のWi-Fiに接続して、確定したらリターン
try_numb=3
idx_ap=0
while True:
if idx_ap >= len(wifi_s):
idx_ap=0
try_numb+=1
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
SSID = wifi_s[idx_ap][0]
PW = wifi_s[idx_ap][1]
wlan.connect(SSID, PW)
try_count=1
target_pin.high()# 外付けLED点灯
while wlan.isconnected() == False and try_count <= try_numb:
print(f'Connecting to Wi-Fi {SSID} AP :{try_count}')
utime.sleep(1)
try_count += 1
if wlan.isconnected() : break
utime.sleep(0.2)
target_pin.low()# 外付けLED消灯
idx_ap+=1
#
wlan_status = wlan.ifconfig()
return wlan_status
target_pin.high()# 外付けLED点灯
wlan_status = wifi__connect()# Wifi接続
print( f'''Connected!
Pico IP Address:{wlan_status[0]} Netmask:{wlan_status[1]}
Default Gateway:{wlan_status[2]} Name Server: {wlan_status[3]}''' ) # Wifi接続情報表示
utime.sleep(1)
target_pin.low()# 外付けLED消灯
udp_portNo=59154 # UDP ユニキャスト受信用-----------------------------
udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_sock.bind(('0.0.0.0', udp_portNo))
udp_sock.setblocking(False) # ソケットをノンブロッキングモードに設定
print(f"UDP 受信待機中 (ポート {udp_portNo})...")
BROADCAST_PORT = 59001 # ブロードキャスト送信用-----------------------
MESSAGE = f"{wlan_status[0]},{udp_portNo}"
MESSAGE = MESSAGE.encode('utf-8')
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)# ブロードキャストを有効にする
# 自身picoWのIPアドレスをブロードキャストで配信しる繰り返し
count=0
while True:
try:
sock.sendto(MESSAGE, ('255.255.255.255', BROADCAST_PORT)) # ブロードキャスト送信
print(f"[{count}] BROADCAST Sent: {MESSAGE}")
except OSError as e:
print(f"{e}: Error BROADCAST socket")
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # エラーでソケットを作り直す。
sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)# ブロードキャストを有効にする
try:
data, udp_addr = udp_sock.recvfrom(1024) # PCの宛先アドレス受信
print(f"受信: {data.decode('utf-8')} from {udp_addr}")
break # 受信したらループ抜ける
except OSError:# 受信なしの時はここに来る
print("まだ接続なし。処理続行。")
utime.sleep(2)
if count % 2 == 0:target_pin.high()# 外付けLED点灯
else: target_pin.low()# 外付けLED消灯
count+=1
#
target_pin.low()# 外付けLED消灯
# I2CのLSM9DS1モジュールの初期化------------------------------------
from machine import Pin, I2C
import utime
I2C_SDA=16# GP16のジェネラル端子をSDA「Serial Data」に指定用変数
I2C_SCL=17# GP17のジェネラル端子をSCL「Serial Clock」に指定用変数
I2C_CH=0# I2Cのチャンネル指定用変数（Picoでは0と1のチャンネルがある）
I2C_M_ADDR=0x1C#磁力センサのアドレス
I2C_AG_ADDR=0x6a#加速センサとジャイロセンサのアドレス
# SCL はスタンダードモードの400KHzの通信モードで、I2Cを初期化
i2c=I2C( I2C_CH, scl=Pin(I2C_SCL), sda=I2C_SDA, freq=400000)#★
for addr in i2c.scan(): print( f"enabled address:{addr:02x}" )
# 磁気センサー関連のレジスタ番号と設定値(lsm9ds1.pdf参照)
CTRL_REG1_M_N=0x20# レジスタ番号
CTRL_REG1_M=0b10010000# 温度補正 Low-power mode ODR:10Hz デフォルト:00010000
CTRL_REG2_M_N=0x21
CTRL_REG2_M=0b00000000# Full scale:±4gauss デフォルト:00000000
CTRL_REG3_M_N=0x22
CTRL_REG3_M=0b00000000# I2Cのenable Continuous conversion mode デフォルト:00000011
CTRL_REG4_M_N=0x23
CTRL_REG4_M=0b00000000# Z-axis:low power, data LSb at lower address, デフォルト: 00000000
CTRL_REG5_M_N=0x24
CTRL_REG5_M=0b00000000# continuous update デフォルト: 00000000
STATUS_REG_M=0x27# 状態
OUT_X_L_M=0x28 # このアドレスより、OUT_X_H_M、OUT_Y_L_M、OUT_Y_H_M、OUT_Z_L_M、OUT_Z_H_Mレジスタが並ぶ
OFFSET_X_REG_L_M=0x05
# ジャイロのセンサー関連のレジスタ番号と設定値
CTRL_REG1_G_N=0x10# レジスタ番号
CTRL_REG1_G=0b11010000# 角速度センサ制御レジスタ デフォルト:00000000
#CTRL_REG1_G[7〜5bit]のODR_G:110 測定周期952Hz
#CTRL_REG1_G[4〜3bit]のFS_G:01 Gyroscope full-scaleを 58dps
#CTRL_REG1_G[1〜0bit]のBW_G:00 帯域幅
CTRL_REG2_G_N=0x11
CTRL_REG2_G=0b00000000# デフォルト:00000000
CTRL_REG3_G_N=0x12
CTRL_REG3_G=0b00000000# デフォルト:00000000
#CTRL_REG3_G でLow-power disabled、High-pass filter desable,Gyroscope high-pass filter cutoff frequency set
STATUS_REG=0x17# 状態
OUT_X_G=0x18 # このアドレスより、OOUT_Y_G (1Ah - 1Bh)、OUT_Z_G (1Ch - 1Dh)のレジスタが並ぶ
# 加速度のセンサー関連のレジスタ番号と設定値
CTRL_REG4_N=0x1E
CTRL_REG4=0b00111000#0,0,Zen_G,Yen_G,Xen_G,0,LIR_XL1,4D_XL1 #デフォルト値:00111000
CTRL_REG5_N=0x1F
CTRL_REG5=0b00111000#0,0,Zen_G,Yen_G,Xen_G,0,LIR_XL1,4D_XL1 #デフォルト値:00111000
CTRL_REG6_XL_N=0x20
CTRL_REG6_XL=0b00000000#デフォルト値:00000000
#CTRL_REG6_XL[7〜5bit]はジャイロスコープ利用時無効?で、ジャイロスコープと同じ測定周期
#CTRL_REG6_XL[4〜3bit]はAccelerometer full-scale：±2g
#CTRL_REG6_XL[4〜3bit]
OUT_X_L_XL=0x28#OUT_X_H_XL,OUT_Y_L_XL,OUT_Y_H_XL,OUT_Z_L_XL,OUT_Z_H_XL
#磁力センサのアドレスを指定して、各制御レジスタを設定
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG1_M_N, bytes([CTRL_REG1_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG2_M_N, bytes([CTRL_REG2_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG3_M_N, bytes([CTRL_REG3_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG4_M_N, bytes([CTRL_REG4_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG5_M_N, bytes([CTRL_REG5_M]))
#各ジャイロセンサレジスタをを上記の通り設定
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG1_G_N, bytes([CTRL_REG1_G]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG2_G_N, bytes([CTRL_REG2_G]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG3_G_N, bytes([CTRL_REG3_G]))
#各加速度のセンサーレジスタを上記の通り設定
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG4_N, bytes([CTRL_REG4]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG5_N, bytes([CTRL_REG5]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG6_XL_N, bytes([CTRL_REG6_XL]))
target_pin.high() # 外付けLED点灯
#-----------------------------I2CのLSM9DS1モジュールの初期化の終了
def send_data(sock:socket, prefix:str, d16s:list, udp_addr): # 引数のセンサー情報送信
'''prefix はセンサー情報識別の1文字'''
bin=prefix.encode('utf-8') + int(d16s[0]).to_bytes(2,'big')
bin+=int(d16s[1]).to_bytes(2,'big')+int(d16s[2]).to_bytes(2,'big')
udp_sock.sendto(bin, udp_addr)
print(f"{prefix}:{d16s[0]:6}{d16s[1]:6}{d16s[2]:6}")
def send_sensor(client, udp_addr): # センサー情報を取得して送信
try:
statu=i2c.readfrom_mem( I2C_M_ADDR, STATUS_REG_M,1)
#print(f"statu:{statu[0]:08b}", end=" ")
if statu[0] & 0x80 :
bin=i2c.readfrom_mem( I2C_M_ADDR, OUT_X_L_M, 6) # X,Y,Z軸の磁気センサー情報取得
#print( [v for v in bin] )
magbin=[bin[n] + (bin[n+1]<<8) for n in range(0,len(bin),2)] # 2byteからuint16へ変換
magbin=[v if v <= 32767 else v - 65536 for v in magbin] # uint16からint16へ変換
#deg = math.atan(magbin[1]/magbin[0]) * 360 / math.pi / 2 # 角度算出
#print( f"M:{magbin[0]:6}, Y:{magbin[1]:6}, Z:{magbin[2]:6} , deg={deg:5.2f}" )
send_data(client, 'M', magbin, udp_addr)
if statu[0] & 0x02 : # ジャイロセンサー情報あり?
bin=i2c.readfrom_mem( I2C_AG_ADDR, OUT_X_G, 6) # X,Y,Z軸のジャイロセンサー情報取得
#print( [v for v in bin] )
gyro=[bin[n] + (bin[n+1]<<8) for n in range(0,len(bin),2)]
gyro=[v if v <= 32767 else v - 65536 for v in gyro]
send_data(client, 'G', gyro, udp_addr)
if statu[0] & 0x01: # 加速度情報情報あり?
bin=i2c.readfrom_mem( I2C_AG_ADDR, OUT_X_L_XL, 6) # X,Y,Z軸の加速度情報取得
#print( [v for v in bin] )
accel=[bin[n] + (bin[n+1]<<8) for n in range(0,len(bin),2)]
accel=[v if v <= 32767 else v - 65536 for v in accel]
#print( f"Accelerometer:{accel}" )
send_data(client, 'A', accel, udp_addr)
if (statu[0] & 0x80) or (statu[0] & 0x02) or (statu[0] & 0x01):
if onboard_led.value(): onboard_led.low()
else: onboard_led.high()
target_pin.low()# 外付けLED消灯
#utime.sleep(0.5)
except Exception as e:
print( e )
client = None
utime.sleep(0.5)
print(f"送信先アドレス：{udp_addr}が確定")
udp_sock.setblocking(True) # ソケットをブロッキングモードに戻す
count=0
#nextTime = utime.time()+0.01
while True:
try:
send_sensor(udp_sock, udp_addr) # センサー情報の送信
# udp_sock.sendto(b'\x01\x02\x03\x04\x05\x06\x07', udp_addr) # テスト用の7バイト送信
# if nextTime < utime.time():
# nextTime = utime.time()+0.01
cmd3=bytearray(3)
cmd3[0]=ord('S')
if sw10.value()==0: cmd3[1] |= 1 # SW10 が押された?
if sw11.value()==0: cmd3[1] |= 2 # SW11 が押された?
udp_sock.sendto(cmd3, udp_addr) # SW情報送信
#
except OSError as e:
udp_sock.close() # 上記の送信で OSError: [Errno 113] があったら、ソケットを閉じる
print(e)
udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # ソケットを作り直す。
udp_sock.bind(('0.0.0.0', udp_portNo))
utime.sleep(0.01) # 送信間隔
count+=1
print(f" count:{count}") # 送信回数の表示
#--------- プログラム終了
上記をUSB通電で実行して、センサー情報をUDP送信する繰り返し状態で、計測した電流は約60mAでした。
上記の受信処理で使うUnity側のプログラム
前述のセンサープログラム（sensor2.py）に変更したため、対応するUnity側プログラムも大きく変更したたためソース名もSensorNet2.csからSensorNet2B.csに変更した。
リアルタイムで増えるデータ群を管理し、データがある程度へ変化したことを判定する方法として
「データと、そのデータまでの平均との差を監視して、ゼロからある程度大きく変わった時に変化したと判定する」
手法を使っています。累積和（CUSUM）法という技法で、RingBufferクラスで実現してます。
SensorNet2B.csの内容を以下に示します。
using UnityEngine;
using System;
using System.IO;
using System.Text;
using System.Collections;
// Cubeにアタッチする。(SensorNet2.cs)
// センサー情報受信で、Cubeを傾ける。
// 起動時にPicoWからのUDP受信処理があれば、そのままセンサー情報の受信処理ループが始まる。
// このUDP受信処理が無ければ、PicoWからのブロードキャスト受信で、PicoWの宛先を取得し、
// その宛先のPicoWに自身の宛先を知らせるUDP送信を行う。
// その後にセンサー情報の受信が確認後、通常のセンサー情報の受信処理ループを始める。
// なお、センサー情報の受信デリゲート関数の、Receiveを定義している
public class SensorNet2B : MonoBehaviour
{
public static StreamWriter writer;
float testVal = 0;
public int fixedUpdateCount = 0;
MyNet2 myNet2;// ネット管理（UDP受信）
Collider myCollider;// プレート用のコライダ
GameObject sphere; // ボールオブジェクト
Collider sphereCollider; // 上記コライダ
Rigidbody sphereRigidbody; //上記の物理エンジン
bool toggleM = true, toggleG = true,toggleA = true;// OnGUIトグルボタン状態
bool receiveFlagM = false, receiveFlagG = false, receiveFlagA = false;// 受信制御（受信でTrue、その処理が終わったらFalse）
bool receiveFlagS = true;
Vector3 v3M = Vector3.zero;// Magnetometerセンサーの受信情報
float degreeZ; // 上記から計算したZ軸の角度
GameObject compass;// 方位磁気コンパス用ゲームオブジェクト
Vector3 v3G = Vector3.zero;// Gyroscopeセンサーの受信情報
float timeG; // Gyroscopeセンサー取得時の時間(ミリ秒)
float elapsedTimeG;// Gセンサー取得時の時間(秒)
//ジャイロセンサーの時間と共に蓄積しまう情報で、勝手に傾く対策
//この蓄積情報を一定秒間取得して、その平均を逆回転量で与える。
Vector3 v3A = Vector3.zero;// Accelerometerセンサーの受信情報
short xS; // スイッチ状態
RingBuffer ringBuffer = new RingBuffer(20);//加速度センサーのデータを蓄えて、停止状態を検出
void Start()
{
this.compass = GameObject.Find("compass"); // コンパス
this.sphere = GameObject.Find("Sphere"); // ボール
this.sphereCollider = this.sphere.GetComponent<SphereCollider>();
this.sphereRigidbody = this.sphere.GetComponent<Rigidbody>();
this.myCollider = GetComponent<MeshCollider>();// プレイト用のコライダ
writer = new StreamWriter("DebugSensorNet2B.txt");
myNet2 = new MyNet2();// 通信用オブジェクト生成
myNet2.myReceive = Receive; //受信で実行するdelegateの関数設定
// PC側の再起動時で、センサーのデータ送信繰り返しが既に始まっている場合、
// デフォルトポートで受信ループが開始できるようにする処理がtryReceiveLoopメソッド内で行っている。
if (myNet2.tryReceiveLoop() == false) // UDPが一定時間で受信がなかった？
{
myNet2.ReceiveBroadcastAsync();
// 上記非同期のブロードキャストで、センサー端末のIPとポート知る。
// その後、自身の宛先をセンサー端末へUDPで知らせ、応答のデータ受信を繰り返す。
// 上記で受信が出来たら、自身の宛先をセンサー端末へUDPで知らせる送信処理を止める。
// データ受信の繰り返しだけ永続するが、この受信データは、MyReceive myReceiveのdelegateで処理する
}
}
// デリゲート関数 myReceiveに設定する受信関数 （ bufは受信したバイナリ7byteまで読み取る ）
void Receive(byte[] buf)
{
byte[] buffer2 = new byte[sizeof(UInt16)];// バイナリから整数に変換用バッファ
short x = 0, y = 0, z = 0;
if (buf.Length >= 3) // 変更点 bufのサイズに応じで、buf[1]以降をshortに変換する
{
Buffer.BlockCopy(buf, 1, buffer2, 0, buffer2.Length);
x = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
}
if (buf.Length >= 5)
{
Buffer.BlockCopy(buf, 1 + 2, buffer2, 0, buffer2.Length);
y = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
}
if (buf.Length >= 7)
{
Buffer.BlockCopy(buf, 1 + 4, buffer2, 0, buffer2.Length);
z = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
}
// 受信データの判定処理
if (buf[0] == 'M' && this.receiveFlagM == false)// 磁気センサー
{
v3M = new Vector3(x, y, z);
v3M = this.norm3Magnetic(v3M);// 正規化
float offset = 185.0f;
this.degreeZ = Mathf.Atan2(v3M.y , v3M.x) * Mathf.Rad2Deg + offset; // 角度算出
this.degreeZ = Mathf.Repeat(this.degreeZ + 360f, 360f);
if (this.degreeZ < 0)
{
//this.degreeZ += 360.0f;
}
this.receiveFlagM = true;
}
else if (buf[0] == 'G' && this.receiveFlagG == false)
{
if (timeG != 0) elapsedTimeG = Time.time - timeG;
timeG = Time.time;
v3G.x = x; v3G.y = y; v3G.z = z;
this.receiveFlagG = true;
}
else if (buf[0] == 'A' && this.receiveFlagA == false)
{
v3A.x = x; v3A.y = y; v3A.z = z;
this.receiveFlagA = true;
}
else if (buf[0] == 'S' && this.receiveFlagS == false)
{
xS = x;
this.receiveFlagS = true;
}
}
void FixedUpdate()
{
fixedUpdateCount++;
// 以下は、このアプリのTCPに接続してきたクライアントと接続状態で実行--------
while (this.receiveFlagM || this.receiveFlagG || this.receiveFlagA) {
if (!this.toggleM) this.receiveFlagM = false;
if (!this.toggleG) this.receiveFlagG = false;
if (!this.toggleA) this.receiveFlagA = false;
if (this.receiveFlagM)
{// 磁気センサーの情報で、このGameObjectを姿勢（rotation）を設定
// Quaternion参考：https://manabu.quu.cc/up/unity/cs_quaternion.html
//Quaternion rotation = Quaternion.LookRotation(v3M);//方向を、回転情報に変換 // y と xを、逆に指定
//if(this.v3M_rest != Vector3.zero)
//{
// Quaternion rotation2 = Quaternion.LookRotation(this.v3M_rest);//方向を、回転情報に変換
// rotation = rotation * Quaternion.Inverse(rotation2);//Quaternionの引き算に相当する計算
//}
//transform.rotation = rotation;
this.compass.transform.eulerAngles = new Vector3(0,0, this.degreeZ);
writer.WriteLine($"{this.v3M.x},{this.v3M.y}");//デバック用記録
this.receiveFlagM = false;
}
if (this.receiveFlagA)// 加速度センサーの情報で、このGameObjectを移動
{
if( ringBuffer.addA( this.v3A ) ) // 加速度センサー情報をバッファに記憶させ、停止中かを判定
{
Debug.Log("-----------------停止状態");
}
else
{
Debug.Log("-----------------移動状態");
}
this.receiveFlagA = false;
}
if (this.receiveFlagG)
{// ジャイロセンサーの情報で、このGameObjectを制御。
this.receiveFlagG = false;
Vector3 d3G = Vector3.zero; // センサー情報
d3G.y = -this.v3G.z * elapsedTimeG * 0.1f * 135 / 360; // センサーとオブジェクトを連動させる計算
d3G.z = -this.v3G.x * elapsedTimeG * 0.1f * 140 / 360; // センサーとオブジェクトを連動させる計算
d3G.x = -this.v3G.y * elapsedTimeG * 0.1f * 135 / 360; // センサーとオブジェクトを連動させる計算
this.ringBuffer.addG(d3G);
d3G -= this.ringBuffer.correctionG;// ドリフト補正(停止状態判断で設定)
// Debug.Log($"----------------------------fixedUpdateCount:{fixedUpdateCount}");
this.transform.Rotate(d3G);// 回転させる
StartCoroutine(SubCollision()); // 衝突処理
}
if (this.receiveFlagS)
{
if( (0x200 & xS) != 0)
{
Debug.Log($"Switch ------------------ {xS,04:X}");
this.transform.position = Vector3.zero;
this.transform.rotation = Quaternion.identity;
}
this.receiveFlagS = false;
}
}
}
// プレートの動かし方で、ボールがプレートをすり抜けて落ちる現象に対する強制移動処理
// プレートのコライダとボールのコライダの衝突を判定し、めり込みが判断出来た場合、強制的に上へ移動する処理
IEnumerator SubCollision()
{
yield return null;
//1フレーム停止してから行う処理
Debug.Log($"----------------------{this.myCollider}, {this.sphereCollider}");
if (this.myCollider.bounds.Intersects(sphereCollider.bounds))
{
// Debug.Log("指定のコライダーと重なっています！");
bool isOverlapping = Physics.ComputePenetration(
myCollider, transform.position, transform.rotation,
this.sphereCollider, this.sphereCollider.transform.position, this.sphereCollider.transform.rotation,
out Vector3 direction, out float distance );
// ２つの Collider が重なっているかどうかを判定し、重なっている場合は「押し出す方向」と「深さ」を返す
// directionは、プレート を ボール から押し出す方向の単位ベクトル
if (isOverlapping)
{
if(Mathf.Abs(distance) > 0.01) {//めり込み具合で、上に押し出す移動
Debug.Log($"重なり検出！方向: {direction}, 深さ: {distance}");
this.sphereRigidbody.MovePosition(this.sphere.transform.position + new Vector3(0, distance, 0));
StartCoroutine(SubCollision()); // 衝突処理
}
}
}
}
private void OnGUI() // 参照：https://docs.unity3d.com/ja/2022.3/Manual/gui-Controls.html
{
GUI.color = new Color(0.9f, 0.0f, 0.9f); //共有スタイルで文字色設定
GUIStyle labelStyle = new GUIStyle(GUI.skin.label);
labelStyle.normal.textColor = Color.cyan;
labelStyle.fontSize = 26;
GUIStyle toggleStyle;
toggleStyle = new GUIStyle(GUI.skin.toggle);
toggleStyle.normal.textColor = Color.grey;
toggleStyle.onNormal.textColor = Color.yellow;// Toggle Onの色を変更
toggleStyle.fontSize = 26;
float whidth = 720;
float height = 100;
float xPos = 150;
float yPos = 0;
GUI.Label(new Rect(0, yPos, whidth, height), MyNet2.status, labelStyle);
yPos += 50;
GUI.Label(new Rect(0, yPos, whidth, height), MyNet2.sensorInf, labelStyle);
yPos += 50;
toggleM = GUI.Toggle(new Rect(0, yPos, 120, 50), toggleM, " Magnetometer", toggleStyle);
string msg = $"x={v3M.x,6:F}, y={v3M.y,6:F}, z={v3M.z,6:F} degZ={degreeZ,6:F2}";//方位 受信データの確認
GUI.Label(new Rect(xPos, yPos, whidth, height), msg, labelStyle);
yPos += 50;
toggleG = GUI.Toggle(new Rect(0, yPos, 120, 50), toggleG, " Gyroscope", toggleStyle);
msg = $"x={v3G.x,6:F}, y={v3G.y,6:F}, z={v3G.z,6:F} ";//ジャイロ 受信データの確認
GUI.Label(new Rect(xPos, yPos, whidth, height), msg, labelStyle);
yPos += 50;
toggleA = GUI.Toggle(new Rect(0, yPos, 120, 50), toggleA, " Accelerometer", toggleStyle);
msg = $"x={v3A.x,6:F}, y={v3A.y,6:F}, z={v3A.z,6:F} ";//加速度 受信データの確認
GUI.Label(new Rect(xPos, yPos, whidth, height), msg, labelStyle);
yPos += 50;
msg = $"testVal : count:{testVal}";//実験データの確認
GUI.Label(new Rect(xPos, yPos, whidth, height), msg, labelStyle);
yPos += 50;
GUI.color = new Color(1f, 1f, 0.0f); //文字色設定
if (GUI.Button(new Rect(xPos + 100, yPos, 100, 60), $"リセット"))
{
this.transform.position = Vector3.zero;
this.transform.rotation = Quaternion.identity;
}
}
private void OnApplicationQuit()
{
writer.Close();
}
// 磁気素データから正規化（正解値）したデータに変換
// オフセットやスケールをを、別途の計測データから求めて、
// 一回転の回転が-180〜180になるX,Yになる変換をしたVector3を求めている
public Vector3 norm3Magnetic(Vector3 v3M)
{
float xMin = -370;// このセンサーを水平で一回転させた最小値、最大値を求めて、それを利用
float xMax = 4140;
float yMin = -710;
float yMax = 3130;
float xOffset = (xMax + xMin) / 2f;
float yOffset = (yMax + yMin) / 2f;
float xScale = (xMax - xMin) / 2f;
float yScale = (yMax - yMin) / 2f;
float correctedX = (v3M.x - xOffset) / xScale;
float correctedY = (v3M.y - yOffset) / yScale;
Vector3 Corrected = new Vector3(correctedX, correctedY, 0);
return Corrected;
}
}
class RingBuffer // リアルタイムで増えるデータ群を管理し、データがある程度へ変化したこと判定するためのクラス
{
Vector3[] buf = new Vector3[50]; // バッファの要素数を指定
int index = 0;// 上記配列への記憶位置
internal int count = 0;// 上記配列への記憶数で、最大はbufの初期配列の要素数
internal Vector3 subtotal = Vector3.zero;
int countNotMoving = 0;
Vector3 v3gSubtotal = Vector3.zero;// ジャイロセンサー計測値の加算値
int v3gSubtotalCount = 0; // 上記測定計測加算回数
public Vector3 correctionG = Vector3.zero; // ジャイロセンサーの補正値(上記の停止状態判断で更新)
public RingBuffer() { }
public RingBuffer(int size)
{
this.buf = new Vector3[size]; // バッファの要素数を指定
}
//リングバッファ内の平均値を算出
Vector3 average()
{
if (this.count == 0) return Vector3.zero;
return this.subtotal / this.count;
}
// valをリングバッファに追加して、この個数とその合計をthis.subtotalに設定し、
// その結果としてセンサーユニットを移動していないと判断できた場合にtrueを返す。
// 判定方法は「追加データと、これまでの平均の差」がthresholdValue以下で、
// その状態が、thresholdCount回数以上続いている時、trueになる。
public bool addA(Vector3 val, float thresholdValue = 600f, int thresholdCount = 50)
{
int size = this.buf.Length;
if (this.count == size) this.subtotal -= this.buf[this.index];
this.subtotal += val;
this.buf[this.index] = val;
this.index += 1;
if (this.index >= size) this.index = 0;
if (this.count < size) this.count += 1;
float diff = (val - this.average()).magnitude;
//Debug.Log($"diff:{diff}, count:{this.count}");
//SensorNet2B.writer.WriteLine($"{val.magnitude},{diff}");
if (diff < thresholdValue)
{
countNotMoving += 1;
if (countNotMoving > thresholdCount && this.v3gSubtotalCount > 0)
{
this.correctionG = this.v3gSubtotal / this.v3gSubtotalCount;
return true;// 止まっていると判断
}
}
else
{
countNotMoving = 0;
this.v3gSubtotal = Vector3.zero;
this.v3gSubtotalCount = 0;
}
return false;// 止まっていないと判断
}
// ジャイロセンサの素データを加算
public void addG(Vector3 valG)
{
this.v3gSubtotal += valG;
this.v3gSubtotalCount++;
}
}
上記で使っているネットワーク関連のユーティリティクラス（MyNet2）は次の内容です。
using UnityEngine;
using System.Net;
using System.Net.Sockets;
using System;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
// SensorNet2で利用するネットワーク関連のメソッドを定義したユーティリティクラス
public class MyNet2
{
public static string status = "ブロードキャスト(センサーのIP)受信状態";
public static string sensorInf = "";
private const int BROADCAST_PORT = 59001;
private CancellationTokenSource cts;// 非同期処理を強制するためのオブジェクト
private UdpClient udpClient; // センサー端末に自身をアドレスを知らせる時に使う送信用UDP
private IPEndPoint remoteIpEndPoint;// 上記で使う端末送信時の宛先用
private string receivedUdpIp;// センサー端末のIPアドレス(ブロードキャストで取得)
private int receivedUdpPortNo=59154;// センサー端末からデータ受信で使うポート番号(ブロードキャストで取得)
public delegate void MyReceive(byte[] receivedData);//delegate型を定義
public MyReceive myReceive;//受信データを処理するdelegate
public MyNet2()
{
this.myReceive = dummy;// 受信のデリゲート関数（テスト用の下記ダミー関数）
}
void dummy(byte[] receivedData)// 初期のデリゲート関数myReceiveに設定するダミー関数
{
//Debug.Log($"受信データ：{Encoding.UTF8.GetString(receivedData)}");
StringBuilder stringBuilder = new StringBuilder();
foreach( byte c in receivedData)
{
stringBuilder.Append($"{c:X} ");
}
Debug.Log($"受信データ：{stringBuilder}");
}
// 非同期で、portNoのブロードキャストで受信後、StartUnicastCommunicationメソッドを起動する
public async void ReceiveBroadcastAsync()
{
try
{
using (UdpClient broadcastReceiver = new UdpClient(BROADCAST_PORT))
{
Debug.Log("Waiting for broadcast...");
// ブロードキャストの受信
UdpReceiveResult result = await broadcastReceiver.ReceiveAsync();
byte[] receivedBytes = result.Buffer;
IPEndPoint sender = result.RemoteEndPoint;
Debug.Log($"Received: {Encoding.UTF8.GetString(receivedBytes)} from {sender}");
string recData = Encoding.UTF8.GetString(receivedBytes);
string[] ipPortArray = recData.Split(',');
if (ipPortArray.Length == 2)
{
receivedUdpIp = ipPortArray[0];
if (int.TryParse(ipPortArray[1], out receivedUdpPortNo))
{
sensorInf = $"ReceiveBroadcast SensorIP: {receivedUdpIp}, Port: {receivedUdpPortNo}";
Debug.Log(sensorInf);
cts = new CancellationTokenSource();// 非同期を終了するためのオブジェクト生成
StartUnicastCommunication(cts.Token);// 非同期のユニキャスト処理を開始
}
else
{
Debug.LogError("Invalid port number received in broadcast.");
}
}
else
{
Debug.LogError("Invalid data format received in broadcast.");
}
}
}
catch (Exception e)
{
Debug.LogError($"Broadcast reception error: {e.Message}");
}
}
// UDP送受信関数（上記ReceiveBroadcastAsync関数から呼び出しされる）
private async void StartUnicastCommunication(CancellationToken cancellationToken)
{
try
{
udpClient = new UdpClient();
udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, receivedUdpPortNo));
this.remoteIpEndPoint = new IPEndPoint(IPAddress.Parse(receivedUdpIp), receivedUdpPortNo);
Debug.Log($"UDP 受信待機中 (ポート {receivedUdpPortNo})...");
string localIp = GetLocalIPAddress(); // 実際のIPアドレスを取得する場合
if(localIp=="")localIp = "192.168.0.110"; // インターネットが使えない場合に使う固定値
string sendMessageStr = $"{localIp},{receivedUdpPortNo}";
byte[] sendBytes = Encoding.UTF8.GetBytes(sendMessageStr);
// 自身の情報を送信し、返信を受信するまで繰り返す（送信と受信のループ）
status = $"センサーへ{sendMessageStr}をユニキャスト送信する繰り返し";
while (!cancellationToken.IsCancellationRequested)
{
await udpClient.SendAsync(sendBytes, sendBytes.Length, remoteIpEndPoint);
Debug.Log($"sendto( {sendMessageStr}, {remoteIpEndPoint} )");
int timeoutMilliseconds = 1000; // 1.0秒 タイムアウトを設定
Task<UdpReceiveResult> receiveTask = udpClient.ReceiveAsync();
Task timeoutTask = Task.Delay(timeoutMilliseconds, cancellationToken); // キャンセル可能にする
Task completedTask = await Task.WhenAny(receiveTask, timeoutTask);
if (completedTask == receiveTask)
{
// データが受信された場合
UdpReceiveResult result = await receiveTask; // ReceiveAsyncの結果を取得
byte[] receivedData = result.Buffer;
Debug.Log($"1回目の受信データ：{Encoding.UTF8.GetString(receivedData)}");
myReceive(receivedData); // 受信で実行するdelegate
break;
}
else if (completedTask == timeoutTask)
{
// タイムアウトした場合
Debug.LogWarning($"UDP receive timed out after {timeoutMilliseconds / 1000} seconds.");
}
else if (cancellationToken.IsCancellationRequested)
{
Debug.Log("Unicast receive loop cancelled during await.");
break;
}
}
status = $"{sendMessageStr}でユニキャスト受信を開始";
// 受信だけループを開始
int count = 2;
while (!cancellationToken.IsCancellationRequested)
{
UdpReceiveResult result = await udpClient.ReceiveAsync();
byte[] receivedData = result.Buffer;
Debug.Log($"{count}回目の受信データ");
myReceive(receivedData); // 受信で実行するdelegate
count++;
}
}
catch (OperationCanceledException)
{
Debug.Log("Unicast communication cancelled.");
}
catch (Exception e)
{
Debug.LogError($"Unicast communication error: {e.Message}");
}
finally
{
if (udpClient != null)
{
udpClient.Close();
udpClient = null;
Debug.Log("UDP Client closed.");
}
}
}
// インスタンス変数 udpClientが生成済みにおいて、その受信処理のループ
private async void ReceiveRataLoop(CancellationToken cancellationToken)
{
// 受信だけループを開始
int count = 2;
while (!cancellationToken.IsCancellationRequested)
{
UdpReceiveResult result = await this.udpClient.ReceiveAsync();
byte[] receivedData = result.Buffer;
Debug.Log($"{count}回目の受信データ");
myReceive(receivedData); // 受信で実行するdelegate
count++;
}
}
// 再起動時に、センサー側が既にUDP送信ループにある場合の受信処理のループ
// 2秒間で、UDP受信が無ければ、falseを返す。
// 2秒間内でUDP受信があれば、trueを返す。その場合、上記ReceiveRataLoopの非同期受信ループを起動
public bool tryReceiveLoop()
{
udpClient = new UdpClient();
udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, receivedUdpPortNo));//ポートはデフォルト値
udpClient.Client.ReceiveTimeout = 2000;// 2秒タイムアウト設定
this.remoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0);
try
{
byte[] receivedBytes = udpClient.Receive(ref this.remoteIpEndPoint);
myReceive(receivedBytes); // 受信で実行するdelegate
udpClient.Client.ReceiveTimeout = 0; // タイムアウトの設定を無効
cts = new CancellationTokenSource();// 非同期を終了するためのオブジェクト生成
status = $"{localIp}：{receivedUdpPortNo}で受信中";
ReceiveRataLoop(cts.Token);// 非同期のユニキャスト受信ループ処理を開始;
return true;// 受信できた。
}
catch (SocketException ex)
{
if (ex.SocketErrorCode == SocketError.TimedOut)
{
Console.WriteLine("受信タイムアウトが発生しました。");
}
else
{
Console.WriteLine($"ソケットエラーが発生しました: {ex.Message}");
}
}
udpClient.Close();
udpClient = null;
return false;
}
private string GetLocalIPAddress()// 自身のローカルIPアドレスの文字列を取得
{
string localIP = "";
using (Socket socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp))
{
socket.Connect("8.8.8.8", 65530); // Google Public DNSに接続してみる
IPEndPoint endPoint = socket.LocalEndPoint as IPEndPoint;
localIP = endPoint.Address.ToString();
}
return localIP;
}
}
上記で使っているボールにアタッチしたコード（SphereScript.cs）は次の内容です。
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class SphereScript : MonoBehaviour
{
Rigidbody rb;
float NextWakeUpTime = 0f;
void Start()
{
Texture2D texture = Resources.Load<Texture2D>("Sprites/格子5_5");
if (texture != null)
{
Renderer renderer = GetComponent<Renderer>();
renderer.material.mainTexture = texture;
}
else
{
Debug.LogWarning("テクスチャが見つかりませんでした");
}
// Rigidbody を追加（重力や物理挙動を有効にする）
rb = gameObject.AddComponent<Rigidbody>();
rb.mass = 1.0f; // 質量（kg）
rb.drag = 0.5f; // 空気抵抗
rb.angularDrag = 0.05f; // 回転抵抗
rb.useGravity = true; // 重力を有効にする
// ボールがすり抜ける対策（高速で動くオブジェクトに適用）
rb.collisionDetectionMode = CollisionDetectionMode.ContinuousDynamic;
rb.interpolation = RigidbodyInterpolation.Interpolate;
// rb.sleepMode = RigidbodySleepMode.NeverSleep;
// sleepMode は Unity 2023以降の一部バージョンで導入されたプロパティです。
// 20250830に追加-----------------------------転がらない対策で追加
// Physic Material を新規作成
PhysicMaterial mat = new PhysicMaterial("LowFrictionMaterial");
// 摩擦と反発係数を設定
mat.dynamicFriction = 0.0f;
mat.staticFriction = 0.0f;
mat.bounciness = 0.0f;
// 摩擦と反発の合成方法を設定
mat.frictionCombine = PhysicMaterialCombine.Minimum;
mat.bounceCombine = PhysicMaterialCombine.Minimum;
// Sphere Collider を取得して、作成したマテリアルを割り当て
SphereCollider sphereCol = GetComponent<SphereCollider>();
if (sphereCol != null)
{
sphereCol.material = mat;
}
else
{
Debug.LogWarning("SphereCollider が見つかりません。");
}
}
void Update()
{
// 下に落ちたボールを、上から再び落下させる。
Vector3 v3 = this.gameObject.transform.position;
if (v3.y < -50)
{
Rigidbody rb = GetComponent<Rigidbody>();
rb.velocity = Vector3.zero; // 移動速度をゼロに
rb.angularVelocity = Vector3.zero; // 回転速度をゼロに
v3 = new Vector3(0,2,0);
this.gameObject.transform.position = v3;
}
// 初期で、転がらない状態を防ぐ対策
if (NextWakeUpTime < Time.time && rb.IsSleeping())
{
NextWakeUpTime = Time.time + 0.5f; // 0.5秒後
rb.WakeUp();// スリープ解除
}
}
}
Unity側の実験プログラム検証
SensorNet2B.csのSensorNet2B.writer.WriteLine($"{val.magnitude},{diff}");で、
加速度の素データ（Vector3）と、その加速度データまでの平均との差のベクトル長(magnitude)を可視化したグラフです。
これは、前述の
# SensorNet2B.writer.WriteLine($"{val.magnitude},{diff}");// 検証用にファイルに保存
のコードを有効して得られたファイルを読み取って表示した次のコードで得られたイメージです。
import numpy as np
import matplotlib.pyplot as plt # pltを使うこと
class RingBuffer:
'リングバッファ内の平均値を算出'
def __init__(self):
self.buf = [0] * 20 # バッファの要素数を指定
self.index = 0
self.count = 0
self.subtotal=0
#
def average(self):
if self.count == 0: return 0
return self.subtotal/self.count
#
def add(self, val):
size=len(self.buf)
if self.count == size: self.subtotal -= self.buf[self.index]
self.subtotal += val
self.buf[self.index] = val
self.index+=1
if self.index >= size: self.index = 0
if self.count < size: self.count += 1
#
#
with open('../DebugSensorNet2B.txt', 'rb') as fr:
#with open('../DebugSensorNet2B_20250910.txt', 'rb') as fr:
data = fr.read() # 全てを読む
strYs=data.decode()
print(strYs)
yy=[dy.strip().split(",") for dy in strYs.split("\r\n")]
y=[float(dy[0]) for dy in yy[0:-1]]
x=[dx for dx in range(len(y))]
plt.plot(x,y)
plt.show() #画面に表示して閉じるまで待つ（表示表示されると、plt内容はクリア）
y2=[float(dy[1]) for dy in yy[0:-1]]
plt.plot(x,y2)
plt.show() #画面に表示して閉じるまで待つ（表示表示されると、plt内容はクリア）
plt.plot(y,y2)
plt.show() #画面に表示して閉じるまで待つ（表示表示されると、plt内容はクリア）
