PS3コントローラーでプログラミング・フォロを操作する

#やること
ラズパイに繋いだPS3コントローラーで、Bluetoothで接続したmicro:bit搭載のプログラミング・フォロを操作できるようにしてみた。

##プログラミング・フォロ
まずはプログラミング・フォロの組み立てについて。
組み立てはこちら

##ラズパイからmicro:bitにBluetoothで繋ぐ
次にラズパイとmicro:bitをBluetoothで接続する方法。
micro:bitのプログラムもこれを使う。
方法はこちら

##ラズパイにPS3コントローラーを繋ぐ
次にラズパイにPS3のコントローラーを繋ぐ方法。
方法はこちら

##PS3コントローラーの操作でプログラミング・フォロを操作する
今までのを合わせて、PS3コントローラーを操作してプログラミング・フォロを操作出来るようにしてみました。
プログラムはgithubにも

folo_control.py

from bluepy import btle
import binascii
import struct
import time

devadr           = "e4:1e:1f:1f:f4:49"
uuid_svc_iopin   = "e95d127b-251d-470a-a062-fa1922dfa9a8"
uuid_pin_io_conf = "e95db9fe-251d-470a-a062-fa1922dfa9a8"
uuid_pin_ad_conf = "e95d5899-251d-470a-a062-fa1922dfa9a8"
uuid_pin_data    = "e95d8d00-251d-470a-a062-fa1922dfa9a8"

device_path  = "/dev/input/js0"
EVENT_FORMAT = "LhBB"
EVENT_SIZE   = struct.calcsize(EVENT_FORMAT)

def main(chPin):

  with open(device_path, "rb") as device :
    event = device.read(EVENT_SIZE)

    while event :
      (ds3_time, ds3_val, ds3_type, ds3_num) = struct.unpack(EVENT_FORMAT, event)
      #print("{0}, {1}, {2}, {3}".format(ds3_time, ds3_val, ds3_type, ds3_num))

      # X button to end
      if ds3_num == 0 and ds3_type == 1:
        #print(" move stop")
        chPin.write(b"\x0d\x00")
        chPin.write(b"\x0e\x00")
        #print(" rotate stop")
        chPin.write(b"\x0f\x00")
        chPin.write(b"\x10\x00")
        per.disconnect()
        return

      # right stick to rotate
      if ds3_num == 3 and ds3_type == 2:

        if abs(ds3_val) < 4000:
          #print(" rotate stop")
          chPin.write(b"\x0f\x00")
          chPin.write(b"\x10\x00")
        elif ds3_val > 0 :
          val = ds3_val>>7
          #print(" rotate right %d" %val)
          chPin.write(b"\x0f\x00")
          chPin.write(b"\x10" + val.to_bytes(1, 'little'))
        else :
          ds3_val = -ds3_val
          val = ds3_val>>7
          #print(" rotate left  %d" %val)
          chPin.write(b"\x10\x00")
          chPin.write(b"\x0f" + val.to_bytes(1, 'little'))

      # left stick to move
      if ds3_num == 1 and ds3_type == 2:

        if abs(ds3_val) < 4000:
          #print(" move stop")
          chPin.write(b"\x0d\x00")
          chPin.write(b"\x0e\x00")
        elif ds3_val > 0 :
          val = ds3_val>>7
          #print(" move back    %d" %val)
          chPin.write(b"\x0d\x00")
          chPin.write(b"\x0e" + val.to_bytes(1, 'little'))
        else :
          ds3_val = -ds3_val
          val = ds3_val>>7
          #print(" move forward %d" %val)
          chPin.write(b"\x0e\x00")
          chPin.write(b"\x0d" + val.to_bytes(1, 'little'))

      time.sleep(0.01)
      event = device.read(EVENT_SIZE)

if __name__ == "__main__" :
  per = btle.Peripheral(devadr, btle.ADDR_TYPE_RANDOM)

  svcPinIO = per.getServiceByUUID(uuid_svc_iopin)

  #pin13-16 as Analog
  chPinAD = svcPinIO.getCharacteristics(uuid_pin_ad_conf)[0]
  chPinAD.write(b"\x00\xe0\x01\x00")

  #pin13-16 as Output
  chPinIO = svcPinIO.getCharacteristics(uuid_pin_io_conf)[0]
  chPinIO.write(b"\x00\x00\x00\x00")

  #pin
  chPin = svcPinIO.getCharacteristics(uuid_pin_data)[0]

  print("Remote control Folo by PS3 controller")
  print("  left  stick : move to forward or back")
  print("  right stick : turn left or right")
  print("  Button X    : End")

  main(chPin)

簡単に言うと、
micro:bitのIO端子をアナログの出力が出来るようにして、
PS3のスティックの信号(-32767〜0が前進、0〜32767が後退)を見て、動かすモーターの端子(前進ならP13)に信号を出力する(0〜255の範囲)。
入力信号から出力信号へは数値の範囲を合わせるためにそのまま1/128(右へ7bitシフト)している。

##実行

% python3 folo_control.py

操作は下記に割り振ってある

|操作 |動作 |
|-------------+---------|
|左スティック上下|前進、後退|
|右スティック左右|左右回転 |
|Xボタン　　　　 |終了 |

前進、後退、左右への回転もちゃんと動くし、スティックの倒す量にしたがってスピードも変わるように動いた。