Genuino101はimuを積んでいて、加速度センサ、ジャイロ情報が得られます。
processingを使って直方体をグルグルさせるデモスケッチが有名ですが、
今回はこれをpythonista3を使ってグルグルさせてみました。
uiのレイアウトは以下のとおり。
ソースは以下の通り。
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が正確になる。