MicroPythonで「ＢＭＸ０５５使用９軸センサーモジュール」のジャイロセンサで「ＧＷＳサーボ　Ｓ０３Ｔ／２ＢＢＭＧ／Ｆ」を駆動するコード

main.py

# Control S03T 2BBMG F by 9-axis BMX055 sensor
#
# http://akizukidenshi.com/catalog/g/gM-01968/
# http://akizukidenshi.com/catalog/g/gK-13010/
# https://docs.micropython.org/en/latest/library/pyb.I2C.html
# https://qiita.com/hurusu1006/items/f493ee4eb9998d5bd740

#           V+  -------- VCC
# [pyboard] X9  -------- SCL [AE-BMX055] J7 ✓
#           X10 -------- SDA
#           GND -------- GND
#
#           GND -------- GND
# [pyboard] V+  -------- +V  [S03T 2BBMG F]
#           X4  -------- SIG

import pyb
from pyb import Pin, Timer, I2C
import time

class AE_BMX055:
  def __init__(self, i2c, *, addr_accel=0x19, addr_gyro=0x69, addr_mag=0x13, debug=False):
    """
    BMX055を初期化する。

    :param i2c: I2Cクラスのインスタンスオブジェクトを指定する
    :param addr_accel: 加速度センサのI²Cアドレスを指定する
    :param addr_gyro: ジャイロセンサのI²Cアドレスを指定する
    :param addr_mag: 磁気センサのI²Cアドレスを指定する
    :param debug: Trueを指定すると、内部データをコンソールに出力する
    """
    self.__i2c = i2c
    self.__addr_accel = addr_accel
    self.__addr_gyro = addr_gyro
    self.__addr_mag = addr_mag
    self.__debug = debug

    self.__init_accel()
    self.__init_gyro()
    self.__init_mag()
    self.__delay(300)

  @property
  def accel(self):
    data = [None] * 6
    for i in range(6):
      # Read 6 bytes of data
      # xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
      data[i] = self.__read_1byte(addr=self.__addr_accel, register=(2 + i))

    if self.__debug:
      print('AE_BMX055.accel raw i2c data:', [hex(x) for x in data])

    # Convert the data to 12-bits
    xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16
    if xAccl > 2047:
      xAccl -= 4096
    yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16
    if yAccl > 2047:
      yAccl -= 4096
    zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16
    if zAccl > 2047:
      zAccl -= 4096

    xAccl = xAccl * 0.0098  # renge +-2g
    yAccl = yAccl * 0.0098  # renge +-2g
    zAccl = zAccl * 0.0098  # renge +-2g

    return xAccl, yAccl, zAccl

  @property
  def gyro(self):
    data = [None] * 6
    for i in range(6):
      # Read 6 bytes of data
      # xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
      data[i] = self.__read_1byte(addr=self.__addr_gyro, register=(2 + i))

    if self.__debug:
      print('AE_BMX055.gyro raw i2c data:', [hex(x) for x in data])

    # Convert the data
    xGyro = (data[1] * 256) + data[0]
    if xGyro > 32767:
      xGyro -= 65536
    yGyro = (data[3] * 256) + data[2]
    if yGyro > 32767:
      yGyro -= 65536
    zGyro = (data[5] * 256) + data[4]
    if zGyro > 32767:
      zGyro -= 65536

    xGyro = xGyro * 0.0038  # Full scale = +/- 125 degree/s
    yGyro = yGyro * 0.0038  # Full scale = +/- 125 degree/s
    zGyro = zGyro * 0.0038  # Full scale = +/- 125 degree/s

    return xGyro, yGyro, zGyro

  @property
  def mag(self):
    data = [None] * 8
    for i in range(8):
      # Read 6 bytes of data
      # xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
      data[i] = self.__read_1byte(addr=self.__addr_mag, register=(0x42 + i))

    if self.__debug:
      print('AE_BMX055.mag raw i2c data:', [hex(x) for x in data])

    # Convert the data
    xMag = ((data[1]<<8) | (data[0]>>3))
    if xMag > 4095:
      xMag -= 8192
    yMag = ((data[3]<<8) | (data[2]>>3))
    if yMag > 4095:
      yMag -= 8192
    zMag = ((data[5]<<8) | (data[4]>>3))
    if zMag > 16383:
      zMag -= 32768

    return xMag, yMag, zMag

  def __init_accel(self):
    """
    BMX055の加速度センサを初期化する。
    """
    self.__i2c.mem_write(
      0x03,  # Range = +/- 2g
      self.__addr_accel,
      0x0F   # Select PMU_Range register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x08,  # Bandwidth = 7.81 Hz
      self.__addr_accel,
      0x10   # Select PMU_BW register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x00,  # Normal mode, Sleep duration = 0.5ms
      self.__addr_accel,
      0x11   # Select PMU_LPW register
    )
    self.__delay(100)

  def __init_gyro(self):
    """
    BMX055のジャイロセンサを初期化する。
    """
    self.__i2c.mem_write(
      0x04,  # Full scale = +/- 125 degree/s
      self.__addr_gyro,
      0x0F   # Select Range register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x07,  # ODR = 100 Hz
      self.__addr_gyro,
      0x10   # Select Bandwidth register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x00,  # Normal mode, Sleep duration = 2ms
      self.__addr_gyro,
      0x11   # Select LPM1 register
    )
    self.__delay(100)

  def __init_mag(self):
    """
    BMX055の磁気センサを初期化する。
    """
    self.__i2c.mem_write(
      0x83,  # Soft reset
      self.__addr_mag,
      0x4B   # Select Mag register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x01,  # Soft reset
      self.__addr_mag,
      0x4B   # Select Mag register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x00,  # Normal Mode, ODR = 10 Hz
      self.__addr_mag,
      0x4C   # Select Mag register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x84,  # X, Y, Z-Axis enabled
      self.__addr_mag,
      0x4E   # Select Mag register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x04,  # No. of Repetitions for X-Y Axis = 9
      self.__addr_mag,
      0x51   # Select Mag register
    )
    self.__delay(100)

    self.__i2c.mem_write(
      0x16,  # No. of Repetitions for Z-Axis = 15
      self.__addr_mag,
      0x52   # Select Mag register
    )
    self.__delay(100)

  def __read_1byte(self, *, addr, register):
    return int.from_bytes(self.__i2c.mem_read(1, addr, register), 'big')

  def __delay(self, ms):
    pyb.delay(ms)

# ----- ----- ----- ----- ----- #

p = Pin('X4')
tim = Timer(2, freq=1000)
ch = tim.channel(4, Timer.PWM, pin=p)
i2c = I2C(1, I2C.MASTER)
bmx055 = AE_BMX055(i2c)

while True:
  # Note: この値は加速度センサのはずだが、ジャイロの値を示しているようなのでこちらを使用
  gyro, *_ = bmx055.accel
  pct = min(max(0, (gyro + 10) * 5), 100)

  print(pct)
  ch.pulse_width_percent(pct)
  time.sleep_ms(100)