ラズパイのBLEでtoioと遊ぶ (3) モータ②加速度センサ
toioには、外部の情報を知るには、読み取りセンサもしくはモーション(加速度)センサが利用できます。超音波などを利用して進行方向などの障害物を検知するセンサは搭載されていません。
モーション・センサには、読み取ったデータに「衝突」という項目があります。ここでは、モータを駆動して前進し、壁にぶつかったら引き返すという動作を考えます。
●イベント駆動
衝突するという現象は、いつも監視していなくてはなりません。二つの方法があります。
RPi.GPIOライブラリを使ったとき、GPIOの設定したピンがLow->Highに変化すると、何かの処理を行う、というのは割り込み処理です。 もう一つは、ポーリングという方法で、プログラムのループの中で、常にGPIOピンを監視するという方法です。ループが大きくなると、監視する間隔があくので、ピンの変化を見逃してしまうという欠点があります。 |
ここではpigpioライブラリ(インストールされている)のソフトウェア割り込みを使います。
- 衝突が起こったらevent_triggerでイベントを発生させる
- イベントが発生したらモータを逆向きに動かす関数event_callbackを記述しておく
- 上記の二つを関連づけるcb1 = pi.event_callback(イベントの数字, 呼び出す関数名)を記述
これらを動かす前に、pigpiodというデーモンを動かしておきます。
sudo pigpiod |
●モーション・センサのインスタンス
モーション・センサのプロパティは、readとnotifyなので、常にデータが更新されているので、読み出したら、それが最新データです。
Sensor_Characteristic_UUID = "10B20106-5B3B-4571-9508-CF3EFCD7BBAE"
peri = btle.Peripheral() |
SensorInformation のインスタンスを使ってセンサのvalueをリードできます。衝突の情報は3番目に書かれています。ぶつかると1で、そうでないときは0です。
●プログラム
メインのプログラムはcollision()関数にしました。モーション・センサのvalueを読み出し、衝突の項目が1だったら、イベントの「2」を発生させます。
イベントの「2」が発生すると、callback_kaihi(event,tick)関数が呼び出されます。ここでは、モータを逆向きに回転させ、ぶつかった壁から退避します。それだけなので、退避中に別の壁にぶつかると動けなくなります。
readSensor = SensorInformation.read()は、2回実行しています。これは、読み出しでエラーがおきたからです。2回目は必ず成功します。なので、最初の読み出しの前にtime.sleep(0.01)を置くと、1回目の読み出しでもエラーが出なくなりました。
##sudo pigpiod import pigpio from bluepy import btle import time Sensor_Characteristic_UUID = "10B20106-5B3B-4571-9508-CF3EFCD7BBAE" Motor_Characteristic_UUID = "10B20102-5B3B-4571-9508-CF3EFCD7BBAE" deviceAddr = "c3:f0:86:ef:c7:62" readSensor = [] massugu = [0x01,0x01,0x01,0x32,0x02,0x01,0x32] back = [0x02,0x01,0x02,0x32,0x02,0x02,0x28,0x30] kaihi = [0x03,0x00,0x03,0x00,0x64,0x00,0x02bc,0x0182,0x005a] peri = btle.Peripheral() peri.connect(deviceAddr, btle.ADDR_TYPE_RANDOM) SensorInformation = peri.getCharacteristics(uuid=Sensor_Characteristic_UUID)[0] MotorControl = peri.getCharacteristics(uuid=Motor_Characteristic_UUID)[0] pi = pigpio.pi() readSensor = [] #callback def callback_kaihi(event,tick): print(" kaihi", tick) MotorControl.write(bytearray(back)) print("back") def collision(): while 1: try: time.sleep(0.01) readSensor = SensorInformation.read() except: readSensor = SensorInformation.read() if readSensor[2]: # butukatta print("butukatta") pi.event_trigger(2) #main print("start") MotorControl.write(bytearray(massugu)) cb1 = pi.event_callback(2, callback_kaihi) print("start2") collision() print('end') cb1.event_cancel()