LoginSignup
7
4

More than 5 years have passed since last update.

@runomee(天上の宴)

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

Posted at

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

IMG_0350.JPG

uiのレイアウトは以下のとおり。
IMG_0349.JPG

ソースは以下の通り。

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を参考に作っています。peripheral.read_characteristic_valueで読み出しています。
genuino側のスケッチは以下のとおり。

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が正確になる。

7
4
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
7
4