本記事は、自分の備忘録用に残しているもののため、皆さんから見るとわかりずらいものとなっていると思います。
今後、間を埋めるために、少しずつ加筆していこうと思います。(本当かいっ!)
Spheno Miniとは、BLEで接続します
[参考情報]
ほぼほぼ以下のページを参考にさせていただきました。ありがとうございました。
https://github.com/igbopie/spherov2.js
https://github.com/raidzero/SpheroMiniDrive
Sphero Miniでは、例えば以下のようなことができます。
- LEDを付けたり消したりできます。(メインLED:カラー、バックLED:単色)
- 好きな方向に転がせます。
- センサーの情報を逐一取得できます。(加速度、ジャイロ、本体の角度、など)
BLE接続情報
今回使うBLEのサービス・キャラクタリスティックは以下の通りです。
const UUID_SPHERO_SERVICE = '00010001-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_SERVICE_COMMAND = '00010001-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_SERVICE_INITIALIZE = '00020001-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_HANDLE_1C = '00010002-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_USETHEFORCE = '00020005-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_SUBSCRIBE = '00020002-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_READ = '00020004-574f-4f20-5370-6865726f2121';
コマンドフォーマット
以下のフォーマットのコマンドをキャラクタリスティック「UUID_SPHERO_CHARACTERISTIC_HANDLE_1C」にWriteします。
[SP] [Flag] [DId] [CmdId] [SeqNo] [Data] [Chksum] [EP]
- SP:Start of Packet。0x8D固定。
- Flag:いろんなフラグ
- DId:デバイスID
- CmdId:コマンドID
- SeqNo:シーケンスカウンター
- Chksum:チェックサム
- Data:コマンドパラメータ
- EP:End of Packet。0xD8固定
※Data以外は1バイト長です。
Flagとしてこんな種類があるそうです。ORで重ねます。
コマンドとしては、通常requestsResponseとresetsInactivityTimeoutを使います。
var Flags = {
isResponse : 1,
requestsResponse : 2,
requestsOnlyErrorResponse : 4,
resetsInactivityTimeout : 8,
};
DIdとして、こんな種類があるそうです。
var DeviceId = {
apiProcessor : 0x10,
systemInfo : 0x11,
powerInfo : 0x13,
driving : 0x16,
animatronics : 0x17,
sensor : 0x18,
userIO : 0x1A,
somethingAPI : 0x1F,
};
CmdIdとして、こんな種類があるそうです。基本的に、各DeviceIdごとに定義があります。
var APIProcessCommandIds = {
echo : 0x00,
};
var SystemInfoCommandIds = {
mainApplicationVersion : 0x00,
bootloaderVersion : 0x01,
something : 0x06,
something6 : 0x12,
something7 : 0x28,
};
var PowerCommandIds = {
deepSleep : 0x00,
sleep : 0x01,
batteryVoltage : 0x03,
wake : 0x0D,
something2 : 0x10, // every x time
something3 : 0x04, // every x time
something4 : 0x1e,
};
var DrivingCommandIds = {
rawMotor : 0x01,
resetYaw : 0x06,
driveAsSphero : 0x04,
driveAsRc : 0x02,
driveWithHeading : 0x07,
stabilization : 0x0C,
};
var AnimatronicsCommandIds = {
animationBundle : 0x05,
shoulderAction : 0x0D,
domePosition : 0x0F,
shoulderActionComplete : 0x26,
enableShoulderActionCompleteAsync : 0x2A,
};
var SensorCommandIds = {
sensorMask : 0x00,
sensorResponse : 0x02,
configureCollision : 0x11,
collisionDetectedAsync : 0x12,
resetLocator : 0x13,
enableCollisionAsync : 0x14,
sensor1 : 0x0f,
sensor2 : 0x17,
configureSensorStream : 0x0c,
};
var UserIOCommandIds = {
allLEDs : 0x0E,
playAudioFile : 0x07,
audioVolume : 0x08,
stopAudio : 0xA,
testSound : 0x18,
};
var SomethingApi = {
something5 : 0x27,
};
Dataにはコマンドパラメータを指定するのですが、特定の値のバイトが含まれる場合はエスケープする必要があります。
エスケープ対象
escape : 0xAB,
startOfPacket : 0x8D,
endOfPacket : 0xD8
これが含まれる場合は、以下のようにエスケープします。結果として1バイトだったのが2バイトになります。
例えば、b=0x8Dの場合は、0xAB | (b & ~0x88) となります。
Javascriptにすると以下の感じです。
const var escapeMask = 0x88;
function command_push_byte(command, b){
if( b == startOfPacket || b == endOfPacket || b == escape ){
command.push(escape);
command.push( b & ~escapeMask );
}else{
command.push(b);
}
}
Chksumは、[Flg]から[Data]までのバイトの加算結果を反転したLSBです。
ただし、加算する対象はエスケープ前の値を使います。
var chksum = (~sum) & 0xff;
まとめると、こんな感じ。
var APIConstants = {
escape : 0xAB,
startOfPacket : 0x8D,
endOfPacket : 0xD8,
escapeMask : 0x88,
};
function command_push_byte(command, b){
if( b == APIConstants.startOfPacket || b == APIConstants.endOfPacket || b == APIConstants.escape ){
command.push(APIConstants.escape);
command.push( b & ~APIConstants.escapeMask );
}else{
command.push(b);
}
}
var g_counter = 0x00;
function command_create(deviceId, commandId, data) {
g_counter++;
var sum = 0;
var command = [];
command.push(APIConstants.startOfPacket);
var cmdflg = Flags.requestsResponse | Flags.resetsInactivityTimeout;
command.push(cmdflg);
sum += cmdflg;
command_push_byte(command, deviceId);
sum += deviceId;
command_push_byte(command, commandId);
sum += commandId;
command_push_byte(command, g_counter);
sum += g_counter;
for( var i = 0 ; i < data.length ; i++ ){
command_push_byte(command, data[i]);
sum += data[i];
}
var chk = (~sum) & 0xff;
command_push_byte(command, chk);
command.push(APIConstants.endOfPacket);
return command;
}
コマンド仕様
LEDと転がしは、単にBLEのキャラクタリスティックにコマンドを送るだけです。
[参考情報]
https://github.com/orbotix/DeveloperResources/blob/master/docs/Sphero_API_1.50.pdf
■メインLEDの点灯
DId:DeviceId.userIO
CmdId:UserIOCommandIds.allLEDs
Data:0x00 0x70 red green blue
red: 赤色LED(0x00~0xff)
green: 緑色LED(0x00~0xff)
blue: 青色LED(0x00~0xff)
■バックLEDの点灯
DId:DeviceId.userIO
CmdId:UserIOCommandIds.allLEDs
Data:0x00 0x01 intensity
intensity: 輝度(0x00~0xff)
■転がす
DId:DeviceId. driving
CmdId:DrivingCommandIds.driveWithHeading
Data:speed heading(MSB) heading(LSB) 0x01
speed:移動速度(0~255)
heading:向き(0~359)
■wake(Sphero起動処理)
Spheroと接続したとき、まずはUseTheForceとWakeを実行しないといけないそうです。
・・・・
・・・・
面倒になってきたので、ソース一式載せておきます。
var vue = new Vue({
el: "#top",
data: {
connected : false,
device_name : '',
mainled_value: '#000000',
rearled_value: '0',
speed_value: '100',
heading_value: '0'
},
computed: {
},
methods: {
sphero_scan: function(){
var parent = this;
try{
return navigator.bluetooth.requestDevice({
filters: [{services:[ UUID_SPHERO_SERVICE ]}],
optionalServices : [UUID_SPHERO_SERVICE_INITIALIZE]
})
.then(device => {
console.log("requestDevice OK");
console.log(device);
sphero_device = device;
sphero_device.addEventListener('gattserverdisconnected', onDisconnect);
parent.device_name = sphero_device.name;
});
}catch(error){
console.log('Error:' + error);
}
},
sphero_connect: function(){
if( sphero_device.connected )
return Promise.resolve();
var parent = this;
try{
return sphero_device.gatt.connect()
.then(server => {
console.log('Execute : getPrimaryService');
sphero_chars.clear();
decoder_set_callback(cb_receive_packet); /* センサー情報の取得用 */
return Promise.all([
set_service_command(server),
set_service_initialize(server)
]);
})
.then(() =>{
console.log('Execute: startNotification');
return sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).startNotifications();
})
.then(() =>{
console.log('Execute: startNotification');
return sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_SUBSCRIBE).startNotifications();
})
.then(()=>{
parent.connected = true;
console.log('Connected!!');
return parent.sphero_initialize();
});
}catch(error){
console.log('Error:' + error);
}
},
sphero_initialize: async function(){
console.log('sphero_initialize');
var command;
console.log('usetheporce');
command = Uint8Array.from(UseTheForce);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_USETHEFORCE).writeValue(command);
console.log('wake');
command = command_create(DeviceId.powerInfo, PowerCommandIds.wake, []);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
command = command_create(DeviceId.powerInfo, PowerCommandIds.wake, []);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
},
/* センサー情報の取得用 */
sphero_sensor_start: async function(){
console.log('sphero_sensor_start');
var command;
command = command_create(DeviceId.sensor, SensorCommandIds.sensor1, [0x01]);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
command = command_create(DeviceId.sensor, SensorCommandIds.sensor2, [0x00]);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
command = command_create(DeviceId.sensor, SensorCommandIds.sensorMask, [0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00]);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
command = command_create(DeviceId.sensor, SensorCommandIds.configureSensorStream, [0x03, 0x80, 0x00, 0x00]);
await sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
},
sphero_disconnect: function(){
if( sphero_device.gatt.connected ){
sphero_device.gatt.disconnect();
}
},
rearled_change: function(){
console.log("rearled_value=", this.rearled_value);
var command = command_create(DeviceId.userIO, UserIOCommandIds.allLEDs, [0x00, 0x01, parseInt(this.rearled_value)]);
return sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
},
mainled_change: function(){
console.log("mainled_value=", this.mainled_value);
var rgb = rgb_hex2bin(this.mainled_value);
var command = command_create(DeviceId.userIO, UserIOCommandIds.allLEDs, [0x00, 0x70, rgb.red, rgb.green, rgb.blue]);
return sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
},
roll_change: function(){
console.log("roll_change");
var heading = parseInt(this.heading_value);
if( heading < 0 )
heading += 360;
var command = command_create(DeviceId.driving, DrivingCommandIds.driveWithHeading, [parseInt(this.speed_value), (heading >> 8) & 0xff, heading & 0xff, 0x01]);
return sphero_chars.get(UUID_SPHERO_CHARACTERISTIC_HANDLE_1C).writeValue(Uint8Array.from(command));
}
},
mounted: function(){
console.log('mounted call');
}
});
var sphero_device = null;
var sphero_chars = new Map();
function onDisconnect(event){
vue.connected = false;
console.log('onDisconnect');
}
function onDataChanged(event){
console.log('onDataChanged');
let characteristic = event.target;
/* センサー情報の取得用 */
let packet = dataview_to_array(characteristic.value);
decoder_set_data(packet);
}
function set_service_command(server){
return server.getPrimaryService(UUID_SPHERO_SERVICE_COMMAND)
.then(service => {
console.log('Execute : getCharacteristic(command)');
return Promise.all([
set_characteristic(service, UUID_SPHERO_CHARACTERISTIC_HANDLE_1C)
]);
});
}
function set_service_initialize(server){
return server.getPrimaryService(UUID_SPHERO_SERVICE_INITIALIZE)
.then(service => {
console.log('Execute : getCharacteristic(initialize)');
return Promise.all([
set_characteristic(service, UUID_SPHERO_CHARACTERISTIC_USETHEFORCE),
set_characteristic(service, UUID_SPHERO_CHARACTERISTIC_SUBSCRIBE),
set_characteristic(service, UUID_SPHERO_CHARACTERISTIC_READ)
]);
});
}
function set_characteristic(service, characteristicUuid) {
return service.getCharacteristic(characteristicUuid)
.then(characteristic => {
console.log('setCharacteristic : ' + characteristicUuid);
sphero_chars.set(characteristicUuid, characteristic);
characteristic.addEventListener('characteristicvaluechanged', onDataChanged );
return service;
});
}
function command_push_byte(command, b){
if( b == APIConstants.startOfPacket || b == APIConstants.endOfPacket || b == APIConstants.escape ){
command.push(APIConstants.escape);
command.push( b & ~APIConstants.escapeMask );
}else{
command.push(b);
}
}
var g_counter = 0x00;
/* 0x8d 0x0a deviceid, commandid counter [data] chk 0xd8 */
function command_create(deviceId, commandId, data) {
g_counter++;
var sum = 0;
var command = [];
command.push(APIConstants.startOfPacket);
var cmdflg = Flags.requestsResponse | Flags.resetsInactivityTimeout;
command.push(cmdflg);
sum += cmdflg;
command_push_byte(command, deviceId);
sum += deviceId;
command_push_byte(command, commandId);
sum += commandId;
command_push_byte(command, g_counter);
sum += g_counter;
for( var i = 0 ; i < data.length ; i++ ){
command_push_byte(command, data[i]);
sum += data[i];
}
var chk = (~sum) & 0xff;
command_push_byte(command, chk);
command.push(APIConstants.endOfPacket);
return command;
}
/* センサー情報の取得用 */
var g_callback = null;
var g_mode = 0;
var g_chksum;
var g_response = null;
/* センサー情報の取得用 */
function decoder_set_callback(callback){
g_mode = 0;
g_callback = callback;
}
/* センサー情報の取得用 */
function decoder_set_data(data){
for( var i = 0 ; i < data.length ; i++ )
decoder_set_byte(data[i]);
}
/* センサー情報の取得用 */
function decoder_set_byte(b){
if( b == APIConstants.startOfPacket ){
g_chksum = 0;
g_mode = 1;
g_response = [b];
}else if( g_mode == 0 ){
return;
}else if( g_mode == 1 ){
if( b == APIConstants.escape ){
g_mode = 2;
}else if( b == APIConstants.endOfPacket ){
g_response.push(b);
if( g_response.length < 3 ){
console.log('decoder_set_byte: length error');
g_mode = 0;
return;
}
if( (g_chksum & 0xff) != 0xff ){
g_mode = 0;
console.log('decoder_set_byte: chksum error');
return;
}
if( g_callback != null )
g_callback(g_response);
g_mode = 0;
}else{
g_response.push(b);
g_chksum += b;
}
}else if( g_mode == 2 ){
g_response.push( b | APIConstants.escapeMask );
g_chksum += b | APIConstants.escapeMask;
mode = 1;
}
}
/* センサー情報の取得用 */
function cb_receive_packet(packet){
console.log('cb_receive_packet');
if( packet[2] == DeviceId.sensor && packet[3] == SensorCommandIds.sensorResponse){
// todo : parse sensor data
}
}
/* センサー情報の取得用 */
function dataview_to_array(array){
var result = new Array(array.byteLength);
for( var i = 0 ; i < array.byteLength ; i++ )
result[i] = array.getUint8(i);
return result;
}
function rgb_hex2bin(rgb) {
var offset = 0;
if(rgb.substring(0,1) == '#')
offset++;
var rgbbin = {};
rgbbin.red = parseInt(rgb.substring(offset, offset + 2), 16);
rgbbin.green = parseInt(rgb.substring(offset + 2, offset + 4), 16);
rgbbin.blue = parseInt(rgb.substring(offset + 4, offset + 6), 16);
return rgbbin;
}
const UUID_SPHERO_SERVICE = '00010001-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_SERVICE_COMMAND = '00010001-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_SERVICE_INITIALIZE = '00020001-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_HANDLE_1C = '00010002-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_USETHEFORCE = '00020005-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_SUBSCRIBE = '00020002-574f-4f20-5370-6865726f2121';
const UUID_SPHERO_CHARACTERISTIC_READ = '00020004-574f-4f20-5370-6865726f2121';
/* usetheforce...band */
const UseTheForce = [0x75, 0x73, 0x65, 0x74, 0x68, 0x65, 0x66, 0x6f, 0x72, 0x63, 0x65, 0x2e, 0x2e, 0x2e, 0x62, 0x61, 0x6e, 0x64];
var DeviceId = {
apiProcessor : 0x10,
systemInfo : 0x11,
powerInfo : 0x13,
driving : 0x16,
animatronics : 0x17,
sensor : 0x18,
userIO : 0x1A,
somethingAPI : 0x1F,
};
var SomethingApi = {
something5 : 0x27,
};
var APIProcessCommandIds = {
echo : 0x00,
};
var SystemInfoCommandIds = {
mainApplicationVersion : 0x00,
bootloaderVersion : 0x01,
something : 0x06,
something6 : 0x12,
something7 : 0x28,
};
var PowerCommandIds = {
deepSleep : 0x00,
sleep : 0x01,
batteryVoltage : 0x03,
wake : 0x0D,
something2 : 0x10, // every x time
something3 : 0x04, // every x time
something4 : 0x1e,
};
var DrivingCommandIds = {
rawMotor : 0x01,
resetYaw : 0x06,
driveAsSphero : 0x04,
driveAsRc : 0x02,
driveWithHeading : 0x07,
stabilization : 0x0C,
};
var AnimatronicsCommandIds = {
animationBundle : 0x05,
shoulderAction : 0x0D,
domePosition : 0x0F,
shoulderActionComplete : 0x26,
enableShoulderActionCompleteAsync : 0x2A,
};
var SensorCommandIds = {
sensorMask : 0x00,
sensorResponse : 0x02,
configureCollision : 0x11,
collisionDetectedAsync : 0x12,
resetLocator : 0x13,
enableCollisionAsync : 0x14,
sensor1 : 0x0f,
sensor2 : 0x17,
configureSensorStream : 0x0c,
};
var UserIOCommandIds = {
allLEDs : 0x0E,
playAudioFile : 0x07,
audioVolume : 0x08,
stopAudio : 0xA,
testSound : 0x18,
};
var Flags = {
isResponse : 1,
requestsResponse : 2,
requestsOnlyErrorResponse : 4,
resetsInactivityTimeout : 8,
};
var APIConstants = {
escape : 0xAB,
startOfPacket : 0x8D,
endOfPacket : 0xD8,
escapeMask : 0x88,
};
<!DOCTYPE html>
<html lang="ja">
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8" />
<meta http-equiv="Content-Security-Policy" content="default-src * data: gap: https://ssl.gstatic.com 'unsafe-eval'; style-src * 'unsafe-inline'; media-src *; img-src * data: content:;">
<meta name="format-detection" content="telephone=no">
<meta name="msapplication-tap-highlight" content="no">
<meta name="viewport" content="user-scalable=no, initial-scale=1, maximum-scale=1, minimum-scale=1, width=device-width">
<!-- jQuery (necessary for Bootstrap's JavaScript plugins) -->
<script src="https://ajax.googleapis.com/ajax/libs/jquery/1.12.4/jquery.min.js"></script>
<!-- Latest compiled and minified CSS -->
<link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/3.3.7/css/bootstrap.min.css" integrity="sha384-BVYiiSIFeK1dGmJRAkycuHAHRg32OmUcww7on3RYdg4Va+PmSTsz/K68vbdEjh4u" crossorigin="anonymous">
<!-- Optional theme -->
<link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/3.3.7/css/bootstrap-theme.min.css" integrity="sha384-rHyoN1iRsVXV4nD0JutlnGaslCJuC7uwjduW9SVrLvRYooPp2bWYgmgJQIXwl/Sp" crossorigin="anonymous">
<!-- Latest compiled and minified JavaScript -->
<script src="https://maxcdn.bootstrapcdn.com/bootstrap/3.3.7/js/bootstrap.min.js" integrity="sha384-Tc5IQib027qvyjSMfHjOMaLkfuWVxZxUPnCJA7l2mCWNIpG9mGCD8wGNIcPD7Txa" crossorigin="anonymous"></script>
<title>Sphero Mini Test</title>
</head>
<body>
<div id="top" class="container">
<h1>Sphero Mini Test</h1>
DeviceName: {{device_name}}<br>
Connected: {{connected}}<br>
<button class="btn" v-on:click="sphero_scan()">スキャン</button>
<button class="btn" v-on:click="sphero_connect()">接続</button>
<button class="btn" v-on:click="sphero_disconnect()">切断</button>
<hr>
MainLED:<input type="color" name="mainled" v-on:change="mainled_change" v-model="mainled_value">{{mainled_value}}<br>
RearLED:<input type="range" name="rearled" min="0" max="255" v-on:change="rearled_change" v-model="rearled_value">{{rearled_value}}<br>
<br>
Speed:<input type="range" name="speed" min="0" max="255" v-model="speed_value">{{speed_value}}<br>
Heading:<input type="range" name="heading" min="-179" max="180" v-model="heading_value">{{heading_value}}<br>
<button class="btn" v-on:click="roll_change">Roll</button>
</div>
<script src="https://unpkg.com/vue"></script>
<script src="js/sphero_const.js"></script>
<script src="js/start.js"></script>
</body>
</html>
上記を見ての通り、HTML5のWeb Bluetooth APIを使っています。
BLE付きのAndroidデバイスのChromeから上記のページをHTTPSで参照すれば、動作します。
接続対象のデバイス(Sphero Mini)は、「SM-XXXX」として見えるようです。
センサー情報の取得(未完成)
センサ情報を取得します。
有効にすると、Sphero MiniからNotificationでデータが垂れ流されてきます。
データは、細切れに送られてくるので、それらを結合して、Start of PacketからEnd of Packetまでを抜きだし、エスケープを解く必要があります。
そこらへんは、start.jsのところの、decode_XXXX()という関数にまとめています。
ソースコード中に「/* センサー情報の取得用 */」と目印を付けておきました。センサー情報を取得しない場合は不要です。
ただ、受信したデータの内容が理解できないです。
それから、センタ情報の受信を開始するときのパラメータの意味もわかりません。ということで、★未完成★です。
(どなたかが解析されると超うれしい。。。)
とりあえず、以上です。