Processing 緊急モーションセンサー(Mac)

Appleの2005年以降のPowerBook/iBook/MacBook（ノート型）には、緊急モーションセンサーが内蔵されています。緊急モーションセンサーは、ハードディスクを保護するために、あやまってコンピュータを落としたときの衝撃を感知します。
Processingには、この緊急モーションセンサーから値を得る「Sudden Motion Sensorライブラリ」があります。緊急モーションセンサーでは、以前扱った「加速度センサ」のように、コンピュータ本体を傾けたり衝撃を与えたりすることによって変化する値（XYZ軸に対する３つの値）が得られます。



Sudden Motion Sensorライブラリのサイトに従えば、３つの値は以下の方法で読み込むことが可能になります。尚、ライブラリをダウンロード＋インストールする必要があります。

//ライブラリを取り込む
import sms.*;

void setup() {
  //画面サイズをとりあえず200角
  size(200,200);
}

void draw() {
  //３つの値を読み込み、配列に代入
  int[] vals = Unimotion.getSMSArray();
  //３つの値を出力
  println(vals[0] + " " + vals[1] + " " + vals[2]);
}

上記プログラムによって出力された値は以下のようになりました（機種や状況によって多少誤差が含まれるかもしれません）。

水平時　x:  0, y: -3, z:56
左90度　x: 52, y: -3, z: 6
右90度　x:-51, y: -3, z: 5
前90度　x:  1, y: 49, z: 6
後90度　x:  1, y:-54, z: 5

xの振り幅：103（-51から52までの±51.5）
yの振り幅：103（-54から49までの±51.5）

になります。それぞれの振り幅からxとyの中点（水平時の値）を新たに求めると、

xの中点:(-51+52)/2=0.5
yの中点:(-54+49)/2=-2.5

になります。これら中点の値をオフセット値として用いることにします。つまり、計測された値からオフセット値を差し引いて角度の計算をすることになります。以前の「Arduino　加速度センサ」のときと同様に、出力値からそれぞれの角度を求める式を用意します。それぞれの角度をradX、radY、オフセット値をoffsetX、offsetY、読み取り値をx、yとすると、

//atan2()で求める場合
radX=atan2((x-offsetX),sqrt(51.5*51.5-(x-offsetX)*(x-offsetX)))
radY=atan2((y-offsetY),sqrt(51.5*51.5-(y-offsetY)*(y-offsetY)))

//またはacos()、asin()で求める場合
radX=asin((x-offsetX)/51.5)
radY=acos((y-offsetY)/51.5)

になります。式中の51.5は加速度1G（重力）の時の値です（機種によっては256くらいのときもあります）。

以上の式を使って、コンピュータ本体を傾けることでProcessing画面上の3Dモデルを動かしてみます。
3Dモデルに関しては、前回の記事「Processing　3Dモデル／OBJ Loader」のものを使うことにします。コンピュータを傾けた方向に、画面内の3Dモデルも同様に傾く内容とします。
スケッチフォルダ内にdataフォルダを作成し、3Dモデルのデータを入れておいて下さい（3Dデータは、ここからダウンロードできます／.objファイルと.mtlファイルの二つが必要です）。


(コンピュータを傾けた時の3Dモデル／前回ブログの3Dモデルを使用）

//ライブラリのインポート
import sms.*;
import saito.objloader.*;

//モデルのオブジェクトを用意
OBJModel model;

void setup() {
  //3D画面サイズ設定
  size(400,400,P3D);
  //モデルのオブジェクトを生成
  model=new OBJModel(this);
  //3Dデータ読み込み
  model.load("macbook.obj");
  //ワイヤーフレームなし
  noStroke();
}

void draw() {
  //３つの値を読み込み、配列に代入
  int[] vals = Unimotion.getSMSArray();
  //println(vals[0] + " " + vals[1] + " " + vals[2]);
  
  //背景描画
  background(50);
  //直線光の設定
  directionalLight(200, 200, 200, -1, 1, -1);
  //環境光の設定
  ambientLight(200, 200, 200);

  //3Dモデルの位置座標設定
  translate(width/2,height*2/3,0);

  //オフセット値  
  float offsetX=-2.5;
  float offsetY=0.5;
  //角度の計算：atan2()で求める場合
  float radX=-atan2(vals[1]-offsetX,sqrt(51.5*51.5-(vals[1]-offsetX)*(vals[1]-offsetX)));
  float radY=-atan2(vals[0]-offsetY,sqrt(51.5*51.5-(vals[0]-offsetY)*(vals[0]-offsetY)));
  //またはacos()、asin()で求める場合
  //float radX=asin((vals[1]-offsetX)/51.5);
  //float radY=acos((vals[0]-offsetY)/51.5);

  //回転角度
  rotateX(radX+PI/2);
  rotateY(radY);

  //三角形分割で面を生成する
  model.drawMode(TRIANGLES);
  //スケール（200倍）
  scale(200);
  //モデル描画
  model.draw();
}

画面内の3D座標は、
左:-X
右:+X
上:-Y
下:+Y
後:-Z
前:+Z
という関係になります。
緊急モーションセンサーのX軸は、Processing上のY軸に対応しているので、vals[0]の値を3DモデルのY軸回転角度へ代入し、val[1]の値は3DモデルのX軸回転角度へ代入します。表示上90度X軸に対してずれていたので、rotateX(radX+PI/2)というようにradXにPI/2（90度）足しておきました。
directionalLight()は太陽光のような直線光であり、括弧内の数値については、最初の３つがRGBで光の色を指定、最後の３つが(0,0,0)の原点を基準に光の向きを設定することになります。
ambientLight()は環境光であり、光の向きはなく、空間全体を明るくしたり暗くしたりし（あるいは色を変える）、RGBの３つの数値で指定します。


もう一つのサンプルとして、コンピュータ自体をコントローラとして傾けて、水平面上のボールを転がすプログラムをしてみます。今回は、setup(){...}内で、一度緊急モーションセンサーから値を読み込み、それらをオフセット値として使うことにします。こうすることで、コンピュータを平らな場所においてプログラムを開始したときの状態（オフセット値）を記憶させておくことができます。ellipse()で擬似的な影を地面に落とすことで立体感がでるようにします。


（コンピュータの傾きに合わせてボールが転がる）

//ライブラリのインポート
import sms.*;

//ボールの座標用変数
float xPos,yPos;

//オフセット用変数
float xOffset;
float yOffset;

void setup() {
  //3D画面設定
  size(400,400,P3D);
  //ワイヤーフレームなし
  noStroke();
  //水平状態の読み込み
  int[] vals = Unimotion.getSMSArray();
  //読み込み値をオフセット値に設定する
  xOffset=vals[0];
  yOffset=vals[1];
}

void draw() {
  //モーションセンサーからの読み込み
  int[] vals = Unimotion.getSMSArray();
  //背景色
  background(220);
  //画面上半分の塗色
  fill(50);
  //画面上半分の矩形
  rect(0,0,width,height/2);

  //ボールの速度の計算
  float xSpeed=vals[0]-xOffset;
  float ySpeed=vals[1]-yOffset;
  //ボール移動量の計算
  xPos+=-xSpeed;
  yPos+=ySpeed;
  //ボール位置の設定
  translate(width/2+xPos,height*4/5,-200+yPos);

  //影の塗色
  fill(50);
  //影の座標と大きさ
  ellipse(-25,30,80,80/5);

  //直線光の設定
  directionalLight(255, 255, 255, -1, 1, 0);
  //ボールの塗色
  fill(255,100,50);
  //ボール描画
  sphere(30);
}

緊急モーションセンサーの読み取り値からオフセット値を差し引いた値を、そのままボールのスピードに反映させています。読み取られたY方向の値は、画面内のZ座標（前後の軸）に対応するので、yPosをtranslate()のZ軸に代入してあります。実際に動かしてみて、向きが逆であったり、座標軸がきちんと対応していない場合は、値にマイナスを掛けたり、代入先を入れ替えたりして調整してみて下さい。
ボールの位置設定のためのtranslate()は、まずX座標をwidth/2で左右中央、Y座標をheight*4/5で画面上下4/5の位置、Z座標を-200奥とした座標を基準とし、その基準の座標に変化する値となるxPosとyPosを追加して、最終的な位置を決定しています。
疑似の影として用いたellipse()は2D用の図形でありX座標とY座標しか設定できませんが、translate()以後に挿入してあるので、translate()のZ軸の値に合わせて、前後に動きつつ大きさも変化します（2D図形は、3D空間上ではZ座標値が0の位置に配置されており、translate()以後に書いた2D図形は、Z軸の値を変化させれば、見た目の大きさや位置も影響を受けて変化します）。同様に、directionalLight()に対しても、2D図形をdirectionalLight()以前に書けば、光の影響を受けませんが、directionalLight()以後に2D図形を書くと、光の影響を受けて図形自体に陰影がつきます。そのため、directionalLight()は、プログラムの冒頭の方に書かずに、ボール描画の直前（2D図形描画以降）に書いておきます。

Arduino 加速度センサ

今回は秋月電子で購入した「KXM52-1050」という３軸加速度センサモジュールを使い、重力方向に対する傾斜角を読み取ります。このセンサでは、XYZ軸の３軸ありますが、XとY軸だけでも三次元的な傾斜角を計測することができます。一応、センサのXYZの３つの出力端子をArduinoのアナログ入力端子にそれぞれ接続することにしますが、実際使うのはXとYの出力値とします。データシートをみながらセンサの端子を以下のように接続します。

１：５V（Arduino５V端子と共有）
２：５V（Arduino５V端子と共有）
３：GND（ArduinoGND端子と共有）
４：無接続
５：GND（ArduinoGND端子と共有）
６：X軸（Arduinoアナログ入力０番ピン）
７：Y軸（Arduinoアナログ入力１番ピン）
８：Z軸（Arduinoアナログ入力２番ピン）



加速度センサを水平なところにおけば、X軸とY軸は重力方向に対して直角なので０Gとなります。５V電源の場合、０Gは２.５Vとして出力されるとデータシートには書いてあります。ArduinoのanalogRead()の１０２４段階（１０ビット）であれば５１１になるはずですが、さまざまな条件で多少の誤差を含みます。実際に使用する前に、念のためArduinoの「Serial Monitor」で加速度センサの出力値をモニタリングしてみます（Arduinoのモニタリング方法については「Arduino　圧電スピーカ」を参照」。

Arduino (Serial Monitor)のプログラム：

void setup(){
  //シリアル通信開始
  Serial.begin(9600);
}

void loop(){
  //３つの値をアナログ入力で読み込む
  int x=analogRead(0);
  int y=analogRead(1);
  int z=analogRead(2);

  //Xの値を出力（十進数）
  Serial.print(x,DEC);
  //値と値の間に区切りを入れる
  Serial.print(",");
  //Yの値を出力
  Serial.print(y,DEC);
  //値と値の間に区切りを入れる
  Serial.print(",");
  //Zの値を出力し改行する
  Serial.println(z,DEC);
  delay(100);
  }
}

３つの値を一行で出力する際に、Arduinoの出力画面上で読みやすいようにそれぞれの値の間に「","」の区切りの記号（コンマ）をいれます。この区切り記号は何でもいいのですが、これがないとそれぞれの数値同士が隣り合わせになって読みにくくなります（また、３つの数値をそれぞれ改行して出力すると、どれがX軸の値でどれがY軸の値なのか分かりにくくなるので、３つ出力してから改行しています）。
固定した角度で計測しても数値が安定しないので、１００個の値をサンプリングして平均値を求めたいと思います。平均値のプログラムを付け加えます。１００個分の値の合計となると、数字も大きくなるので「int」型の整数ではなく、より大きい値が扱える「long」型の整数を変数として使います。

//加算用の変数
long x_sum, ysum, z_sum;
//回数の変数
int count=0;

void setup(){
  Serial.begin(9600);
}

void loop(){
  int x=analogRead(0);
  int y=analogRead(1);
  int z=analogRead(2);

  //それぞれに値を足していく（合計数）
  x_sum+=x;
  y_sum+=y;
  z_sum+=z;

  //回数を＋１する（カウントアップ）
  count++;

  //１００回カウントしたら
  if(count>99){
    //合計数を100で割って平均値を出す
    Serial.print(x_sum/100,DEC);
    Serial.print(",");
    Serial.print(y_sum/100,DEC);
    Serial.print(",");
    Serial.println(z_sum/100,DEC);
    //カウントを０に戻す
    count=0;
    //合計数を０に戻す
    x_sum=0;
    y_sum=0;
    z_sum=0;
  }
}

まずは、X軸について計測することにします。水平状態（０G）に対して定規などを用いて−９０度傾けて−１Gの値、９０度傾けて＋１Gの値を上記プログラムを用いて計測することにします。
プログラム上では、xの値をx_sumに足していき、変数countで何回足したかを数えておきます（１ループで一回足されます）。countが１００になったら、１００回分の合計数であるx_sumを１００で割り、その値を出力します（yについても同様に計測します）。
０Gの値については、水平に置いて計測してもいいのですが、今回は−１Gの時の値と＋１Gの時の値の中点を用いることにします。よって、以下のような計測結果になります。

　　角度：　　　重力：Ｘ軸平均値：Ｙ軸平均値
－９０度：　　－１Ｇ：　　３１６：　　２７１
中点０度：　　　０Ｇ：　　５３６：　　４９１
＋９０度：　　＋１Ｇ：　　７５６：　　７１１　

これらの値は、今回使用した加速度センサと計測状況において求められた値なので、各自で似たような方法で計測してください。

それでは、この計測結果をもとに、Processingにセンサからの出力値をシリアル通信し、Processing上の３D立体を動かしてみたいと思います。センサを傾ければ、同様に３D立体も同じ角度で傾くようにします。シリアル通信は、１０２４段階の値を文字列で送ることにします（「Arduino-Processing　シリアル通信５」を参照）。このプログラムでは、X軸とY軸だけを読み取ることにします。

Arduinoのプログラム：

void setup(){
  //シリアル通信開始
  Serial.begin(9600);
}

void loop(){
  //２つの値をアナログ入力で読み込む
  int x=analogRead(0);
  int y=analogRead(1);

  if(Serial.available()>0){
    //Xの値を出力
    Serial.print(x,DEC);
    //値と値の間に区切りを入れる
    Serial.print(",");
    //Yの値を改行して出力
    Serial.println(y,DEC);
    //合図用データを読み込みバッファを空にする
    Serial.read();
  }
}

センサから読み取ったXとY軸の値をそのままArduinoから送信します。
Processingでは、受け取った値を角度に変換する計算が必要になります。まず０Gを基準にして、水平時の値が０になるようにオフセット値（X軸の場合：５３６、Y軸の場合：４９１）を設けて差し引いておきます。そうすれば、

　　角度：　　　重力：　　　Ｘ軸：　　　Ｙ軸
－９０度：　　－１Ｇ：　－２２０：　－２２０
　　中点：　　　０Ｇ：　　　　０：　　　　０
＋９０度：　　＋１Ｇ：　＋２２０：　＋２２０

となります。振り幅は０Gを基準にプラスマイナス２２０となります。
次に角度の計算ですが単位はラジアンを用います。−９０度から＋９０度までの範囲なので、ラジアンでいうと−PI/2から＋PI/2になります（PIは円周率のπです）。X軸の値が１１０であれば、振り幅である２２０（１G）の半分なので０.５Gになります。角度については９０度の半分なので４５度になりそうですが、実際は３０度になります。−４５度の場合は、以下の図のように約−１５６になります。



この計算方法は以下のようにして求められます。

acos()、asin()を用いる場合：
まず、Arduinoから送られて来たX軸の値をx、オフセット値をx_offset（今回のオフセット値は５３６）、オフセット調整した値をx0とすると、

x0=x-x_offset;

になり、角度をradX（ラジアン）とすると

sin(radX)=x0/220;

という関係になります。例えば、x0=110を代入すればsin(radX)=1/2なので、radXは30度となります。
Processingにはasin()やacos()の関数があるので、それを利用すると

radX=asin(sin(radX));

という関係になり、sin(radX)にx0/220を代入し

radX=asin(x0/220);

となることで角度radXが求まります。
Y軸についてはacos()で求めると、

radY=acos(y0/220);

になります。

atan2()を用いる場合：
また、この関係をタンジェントで表せば、

tan(radX)=x0/sqrt(220*220-x0*x0)

となります。sqrt()は平方根（ルート）を求める関数です。
角度を求めるには、atan2()という関数を用いて、

radX=atan2(x0,sqrt(220*220-x0*x0));

とします。そうすると角度radXが求められます。

加速度センサのX軸プラス方向をProcessingの３D空間のX軸マイナス方向に対応させるために-radXに変換します。加速度センサのY軸方向を３D空間のZ軸方向に対応させて、

rotateX(-radX)
rotateZ(radY)

となります。
もし、加速度センサの回転方向と、３D立体の回転方向が逆になってしまうときは、値にマイナスを掛けます。また、９０度ずれているときはPI/2を足します。実際にセンサを動かして、同じように３D立体が動くか確かめて下さい。

Processingのプログラム：

import processing.serial.*;
Serial port;

//読み取り値の変数
int x,y;

//X軸-1G時316、+1G時756であることから
//X軸のオフセット値
int x_offset=536;
//X軸の振り幅（-1G〜0G又は0G〜+1G）
int x_range=220;

//Y軸-1G時271、+1G時711であることから
//Y軸のオフセット値
int y_offset=496;
//Y軸の振り幅（-1G〜0G又は0G〜+1G）
int y_range=220;

//角度（ラジアン）の変数
float radX,radY;

void setup(){
  //3D画面サイズ400×400
  size(400,400,P3D);
  //シリアルポート設定
  port = new Serial(this,"/dev/tty.usbserial-A50019vD",9600);
  //念のためバッファを空にする
  port.clear();
  //「10」(ラインフィード)が来る度にserialEvent()作動
  port.bufferUntil(10);
 //図形塗り面なし（ワイヤフレーム描画）
  noFill();
}

void draw(){
  //背景色を白
  background(255);

  //３D立体の座標を画面中央、-100奥に配置
  translate(width/2,height/2,-100);

  //オフセット調整（最小値-220、最大値220）
  int x0=constrain(x-x_offset,-220,220);
  int y0=constrain(y-y_offset,-220,220);

  //角度の計算（ラジアン）
  radX=asin(x0/x_range);//asin()で求める
  radY=acos(y0/y_range);//acos()で求める
  //radX=atan2( x0,sqrt(x_range*x_range-x0*x0) );//atan2()で求める場合
  //radY=atan2( y0,sqrt(y_range*y_range-y0*y0) );
  
  //センサX軸の角度は3D立体のX軸の角度に対応
  //センサY軸の角度は3D立体のZ軸の角度に対応
  //角度をそれぞれ代入
  rotateX(-radX);
  rotateZ(radY);

  //直方体を描画
  box(200,30,100);
}

//シリアル通信
void serialEvent(Serial p){
  //文字列用変数を用意し、
  //「10」(ラインフィード）が来るまで読み込む
  String stringData=port.readStringUntil(10);

  //データが空でないとき
  if(stringData!=null){
    //改行記号を取り除く
    stringData=trim(stringData);
   //コンマで区切ってデータを分解、整数化
    int data[]=int(split(stringData,','));

   //データ数が２個のとき
    if(data.length==2){
      //データの値を代入
      x=data[0];
      y=data[1];
      //合図用データ送信
      port.write(65);
    }
  }
}

//マウスボタンを押して通信開始
void mousePressed(){
  //合図用データ送信
  port.write(65);
}

void draw(){...}内の「オフセット調整」箇所の

int x0=constrain(x-x_offset,-220,220)

は、constrain()を用いて、読み取った値xからオフセット値であるx_offsetを差引き、最小値−２２０から最大値２２０までの値になるように制限しています。

ノイズのせいか、動きがぎこちない場合はフィルターのプログラムを挿入し滑らかにします。そのためには、radX、radYと同様にプログラムの冒頭でフィルター用の変数：

float filterX,filterY;

を用意しておき、void draw(){...}内の最後の角度を求める箇所を以下のように変更してください。

radX=asin(x0/x_range);//変更なし
radY=acos(y0/y_range);//変更なし

//フィルターの式 
filterX+=(radX-filterX)*0.3;//新たに挿入
filterY+=(radY-filterY)*0.3;//新たに挿入

rotateX(-filterX);//変更
rotateZ(filterY); //変更

フィルターの式の「0.3」は係数であり、1.0に近づくほどフィルターの効果はなくなります。逆に0.1のように係数の値を小さくすれば、滑らかになりつつ反応が鈍く動くようになります。適度に調整してみてください。


尚、もっと簡単に加速度センサを扱いたい場合は（あまり正確な角度にこだわらないのであれば）、

//オフセット調整（最小値-220、最大値220）
int x0=constrain(x-x_offset,-220,220);
int y0=constrain(y-y_offset,-220,220);
//角度の計算（ラジアン）
radX=asin(x0/x_range);
radY=acos(y0/y_range);

の部分を、

radX=2.0*x*PI/1023;
radY=2.0*y*PI/1023;

に置き換えてもセンサを傾けた方向に３D立体が傾きます。この計算では、読み取った直接の値に比例して角度も変わります（比率が多少ずれてしまいます）。この場合は、モニタリングで調べた最小値／最大値／オフセット値などの設定もする必要はありません。式の中の「2.0」というのは係数であり、大きくすれば傾きも大きくなるので画面で確認しながら調整してください。

−９０度や＋９０度付近では、出力値の変化が微妙になるので、きちんとした角度が出ない場合があります。出力値補正のためにZ軸の出力も利用すれば、計算は少し複雑になるかもしれませんが、±９０付近まで計測可能になります。