pythonista3で、Genuino101をbleを使って連動させてみる

Genuino101はimuを積んでいて、加速度センサ、ジャイロ情報が得られます。
processingを使って直方体をグルグルさせるデモスケッチが有名ですが、
今回はこれをpythonista3を使ってグルグルさせてみました。

![IMG_0350.JPG](https://qiita-image-store.s3.amazonaws.com/0/176818/6e614286-73a0-93e1-7b54-5680ca57ba4c.jpeg)

uiのレイアウトは以下のとおり。
![IMG_0349.JPG](https://qiita-image-store.s3.amazonaws.com/0/176818/8b07de50-e6e6-ad85-6288-3fdda706c01e.jpeg)



ソースは以下の通り。



```python:genuino101.py
# coding: utf-8
# For use in pythonista on iOS
from objc_util import *
import ui
import motion
from scene import *
import math
import numpy as np
import struct
import cb
#boxの大きさ
scale = 40
W=3
L=1.5
H=0.25
#switchの初期値 1 motion 起動中
STOP_SWITCH=False
#motionセンサを使う時はTrue genuinoを使う時はFalse
MOTION_SW=False
IMU_SERVICE_UUID="19B10010-E8F2-537E-4F6C-D104768A121"
AX_CHAR_UUID ="19B10011-E8F2-537E-4F6C-D104768A1214"
AY_CHAR_UUID="19B10012-E8F2-537E-4F6C-D104768A1214"
AZ_CHAR_UUID="19B10013-E8F2-537E-4F6C-D104768A1214"
GX_CHAR_UUID="19B10014-E8F2-537E-4F6C-D104768A1214"
GY_CHAR_UUID="19B10015-E8F2-537E-4F6C-D104768A1214"
GZ_CHAR_UUID="19B10016-E8F2-537E-4F6C-D104768A1214"
ROLL_CHAR_UUID="19B10017-E8F2-537E-4F6C-D104768A1214"
PITCH_CHAR_UUID="19B10018-E8F2-537E-4F6C-D104768A1214"
YAW_CHAR_UUID="19B10019-E8F2-537E-4F6C-D104768A1214"
pitch,roll,yaw=0.0,0.0,0.0
ax,ay,az=0.0,0.0,0.0
gx,gy,gz=0.0,0.0,0.0
load_framework('SceneKit')
SCNView, SCNScene, SCNBox, SCNText, SCNNode, SCNLight, SCNCamera, SCNAction, SCNTransaction,UIFont= map(ObjCClass, ['SCNView', 'SCNScene', 'SCNBox', 'SCNText', 'SCNNode', 'SCNLight',  'SCNCamera', 'SCNAction','SCNTransaction','UIFont'])
class SCNVector3 (Structure):
    _fields_ = [('x', c_float), ('y', c_float), ('z', c_float)]

class MyCentralManagerDelegate (object):
    def __init__(self):
        self.peripheral = None
        self.toggle = False
        self.did_service=False
        self.pitch,self.roll,self.yaw=0.0,0.0,0.0
        self.ax,self.ay,self.az=0.0,0.0,0.0
        self.gx,self.gy,self.gz=0.0,0.0,0.0

    def did_discover_peripheral(self, p):
        print('+++ Discovered peripheral: %s (%s)' % (p.name, p.uuid))
        if p.name and 'GENUINO 101-DA05' in p.name and not self.peripheral:
            # Keep a reference to the peripheral, so it doesn't get garbage-collected:
            self.peripheral =p
            cb.connect_peripheral(self.peripheral)

    def did_connect_peripheral(self, p):
        print('*** Connected: %s' % p.name)
        print('Discovering services...')
        p.discover_services()

    def did_fail_to_connect_peripheral(self, p, error):
        print('Failed to connect')
    def did_disconnect_peripheral(self, p, error):
        print('Disconnected, error: %s' % (error,))
        self.peripheral = None

    def did_discover_services(self, p, error):
        for s in p.services:
            if IMU_SERVICE_UUID in s.uuid:
                print('+++ IMU Service')
                p.discover_characteristics(s)
    def did_discover_characteristics(self, s, error):
        print('Did discover characteristics...')
        if IMU_SERVICE_UUID in s.uuid:
            self.schar=s.characteristics
            self.did_service=True
    def send_cmd(self, cmd):
        for c in self.schar:
            self.data_char=c
            self.peripheral.read_characteristic_value(c)
            #self.peripheral.set_notify_value(c, True)
        return self.pitch,self.roll,self.yaw,self.ax,self.ay,self.az,self.gx,self.gy,self.gz
    def did_update_value(self, c,error):
        imudata = struct.unpack('f',c.value)[0]
        self.get_data(c.uuid,imudata)
    def did_write_value(self, c, error):
        print('did_write_value')
        if self.toggle :
            self.peripheral.write_characteristic_value(c, chr(0x01), True)
            print('LED On...')
        else:
            self.peripheral.write_characteristic_value(c, chr(0x00), True)
            print('LED Off...')
        self.toggle = not self.toggle
    def get_data(self,cid,msg):
        if cid==AX_CHAR_UUID:
            self.ax=msg
        elif cid==AY_CHAR_UUID:
            self.ay=msg
        elif cid==AZ_CHAR_UUID:
            self.az=msg
        elif cid==GX_CHAR_UUID:
            self.gx=msg
        elif cid==GY_CHAR_UUID:
            self.gy=msg
        elif cid==GZ_CHAR_UUID:
            self.gz=msg
        elif cid==PITCH_CHAR_UUID:
            self.pitch=msg
        elif cid==ROLL_CHAR_UUID:
            self.roll=msg
        elif cid==YAW_CHAR_UUID:
            self.yaw=msg
@on_main_thread
class MyScene (Scene):
    def setup(self):
        pass
    def draw(self):
        global pitch,roll,yaw,ax,ay,az,gx,gy,gz
        global main_view,mngr
        #加速度、ジャイロの値を更新
        if not STOP_SWITCH : 
            if MOTION_SW==False :
                pitch,roll,yaw,ax,ay,az,gx,gy,gz=mngr.send_cmd('')        
            else:
               ax,ay,az = motion.get_user_acceleration()
               gx,gy,gz= motion.get_gravity()
               pitch, roll, yaw =motion.get_attitude()
               #mx,my,mz,ma=motion.get_magnetic_field()
               # ラジアン→度へ変換
               pitch=pitch*180/3.1415926
               roll=roll*180/3.1415926
               yaw=yaw*180/3.1415926
       
            #加速度、ジャイロ、地磁気センサーの値を表示
            main_view['ax'].text=str(round(ax,2))
            main_view['ay'].text=str(round(ay,2))
            main_view['az'].text=str(round(az,2))
        
            main_view['gx'].text=str(round(gx,2))
            main_view['gy'].text=str(round(gy,2))
            main_view['gz'].text=str(round(gz,2))
            
            main_view['pitch'].text=str(round(pitch,2))
            main_view['roll'].text=str(round(roll,2))
            main_view['yaw'].text=str(round(yaw,2))
            self.box_node.runAction_(SCNAction.rotateToX_y_z_duration_(-math.pi*pitch/180, math.pi*roll/180,math.pi*yaw/180,0))
        
    def make_view(self,mc):
        pitch,roll,yaw=0.0,0.0,0.0
        main_view_objc = mc
        
        scene_view = SCNView.alloc().initWithFrame_options_(((0, 0),(400, 400)), None).autorelease()
        scene_view.setAutoresizingMask_(18)
        scene_view.setAllowsCameraControl_(False)
        scene = SCNScene.scene()
        root_node = scene.rootNode()  
    
        self.box= SCNBox.boxWithWidth_height_length_chamferRadius_(W, L, H, 0)
        self.box_node = SCNNode.nodeWithGeometry_(self.box)
        self.box_node.setPosition_((0, 0, 0))
    
        light_node = SCNNode.node()
        light_node.setPosition_((1.5, 1.5, 1.5))
        light = SCNLight.light()
        light.setType_('omni')
        light.setCastsShadow_(True)
        light.setColor_(UIColor.redColor().CGColor())
        light_node.setLight_(light)
  
        camera = SCNCamera.camera()
        camera_node = SCNNode.node()
        camera_node.setCamera(camera)
        camera_node.setPosition((0, 0, 5))
    
        root_node.addChildNode_(camera_node)
        root_node.addChildNode_(self.box_node)
        root_node.addChildNode_(light_node)
    
        scene_view.setScene_(scene)
        main_view_objc.addSubview_(scene_view) 
        
def switch1(self):
    global STOP_SWITCH
    if not STOP_SWITCH:
        #動きを止める
        STOP_SWITCH=True  
    else :
        #動きを再開する
        STOP_SWITCH=False      
def switch2(self):
    global MOTION_SW,mngr
    if not main_view['switch2'].value :
       MOTION_SW=False
       mngr = MyCentralManagerDelegate()
       cb.set_central_delegate(mngr)
       cb.scan_for_peripherals()
       print('Scanning for peripherals...')
       main_view['state'].text='Scanning'
       while not mngr.peripheral:pass
       main_view['state'].text='Detected'
       while not mngr.did_service:pass
    else :
        MOTION_SW=True
        motion.start_updates()
        main_view['state'].text='motion mode'
if __name__ == "__main__":  
    main_view = ui.load_view()
    main_view.name = 'Genuino101/motion Demo'
    main_view_objc = ObjCInstance(main_view['view1']) 
    if MOTION_SW==False:
        main_view['switch2'].value=False
    else:
        main_view['switch2'].value=True  
    switch2('')
    #run MyScene
    my_scene = MyScene()
    scene_view = SceneView()
    scene_view.scene = my_scene
    main_view_objc.addSubview_(scene_view)
    #make scenekit
    my_scene.make_view(main_view_objc)
    #present view
    main_view.present('sheet')
```

bleの部分は[cb — Connecting to Bluetooth LE Peripherals](http://omz-software.com/pythonista/docs/ios/cb.html)を参考に作っています。peripheral.read_characteristic_valueで読み出しています。
genuino側のスケッチは以下のとおり。


```c:imutest5.ino
#include <CurieBLE.h>
#include <CurieIMU.h>
#include <MadgwickAHRS.h>

float accelScale, gyroScale;

// Create my own UUIDs; used https://www.uuidgenerator.net/
#define IMU_SERVICE_UUID "19B10010-E8F2-537E-4F6C-D104768A1214"
#define AX_CHAR_UUID "19B10011-E8F2-537E-4F6C-D104768A1214"
#define AY_CHAR_UUID "19B10012-E8F2-537E-4F6C-D104768A1214"
#define AZ_CHAR_UUID "19B10013-E8F2-537E-4F6C-D104768A1214"
#define GX_CHAR_UUID "19B10014-E8F2-537E-4F6C-D104768A1214"
#define GY_CHAR_UUID "19B10015-E8F2-537E-4F6C-D104768A1214"
#define GZ_CHAR_UUID "19B10016-E8F2-537E-4F6C-D104768A1214"
#define ROLL_CHAR_UUID "19B10017-E8F2-537E-4F6C-D104768A1214"
#define PITCH_CHAR_UUID "19B10018-E8F2-537E-4F6C-D104768A1214"
#define HEADING_CHAR_UUID "19B10019-E8F2-537E-4F6C-D104768A1214"
// Arduino 101 acts as a BLE peripheral
BLEPeripheral blePeripheral;

// IMU data is registered as a BLE service
BLEService imuService(IMU_SERVICE_UUID);

// Each IMU data point is its own characteristic
BLEFloatCharacteristic axChar(AX_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic ayChar(AY_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic azChar(AZ_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic gxChar(GX_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic gyChar(GY_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic gzChar(GZ_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic rollChar(ROLL_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic pitchChar(PITCH_CHAR_UUID, BLERead | BLENotify);
BLEFloatCharacteristic headingChar(HEADING_CHAR_UUID, BLERead | BLENotify);

// Assign pin to indicate BLE connection 
const int INDICATOR_PIN = 13;
Madgwick filter;

float ax,ay,az;
float gx,gy,gz;
float roll, pitch, heading;

long previousMillis = 0;

void setup() {
  
  //Initializing IMU...
  CurieIMU.begin();
  CurieIMU.autoCalibrateGyroOffset();
  CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  // Initialize BLE peripheral
  blePeripheral.setLocalName("IMU");
  blePeripheral.setAdvertisedServiceUuid(imuService.uuid());
  blePeripheral.addAttribute(imuService);
  blePeripheral.addAttribute(axChar);
  blePeripheral.addAttribute(ayChar);
  blePeripheral.addAttribute(azChar);
  blePeripheral.addAttribute(gxChar);
  blePeripheral.addAttribute(gyChar);
  blePeripheral.addAttribute(gzChar);
  blePeripheral.addAttribute(rollChar);
  blePeripheral.addAttribute(pitchChar);
  blePeripheral.addAttribute(headingChar);

  // Set initial values
  axChar.setValue(ax);
  ayChar.setValue(ay);
  azChar.setValue(az);
  gxChar.setValue(gx);
  gyChar.setValue(gy);
  gzChar.setValue(gz);
  rollChar.setValue(roll);
  pitchChar.setValue(pitch);
  headingChar.setValue(heading);
  
  // Now, activate the BLE peripheral
  blePeripheral.begin();
  //Bluetooth device active, waiting for connections...
}

void loop() {
  // Check if the connection to the central is active or not
  BLECentral central = blePeripheral.central();

  if(central) {
    //Connected to central
    digitalWrite(INDICATOR_PIN, HIGH);
    
    while(central.connected()) {
      updateImuData();
    }
    //Disconnected from central
    digitalWrite(INDICATOR_PIN, LOW);
  }
}

void updateImuData() {
  int aix, aiy, aiz;
  int gix, giy, giz;
 
  CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
  // convert from raw data to gravity and degrees/second unit
  ax = convertRawAcceleration(aix);
  ay = convertRawAcceleration(aiy);
  az = convertRawAcceleration(aiz);
  gx = convertRawGyro(gix);
  gy = convertRawGyro(giy);
  gz = convertRawGyro(giz);

  // update the filter, which computes orientation
  filter.updateIMU(gx, gy, gz, ax, ay, az);
  // print the heading, pitch and roll
  roll = filter.getRoll();
  pitch = filter.getPitch();
  heading = filter.getYaw();
  
  axChar.setValue(ax);
  ayChar.setValue(ay);
  azChar.setValue(az);
  gxChar.setValue(gx);
  gyChar.setValue(gy);
  gzChar.setValue(gz);
  rollChar.setValue(roll);
  pitchChar.setValue(pitch);
  headingChar.setValue(heading);

}
float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
 
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
 
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}
```

シリアル通信をBLE通信にしただけのもの。
MadgwickAHRS.hがすごい。このfilterをかますことで、pitch,roll,yawが正確になる。