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軸の出力も利用すれば、計算は少し複雑になるかもしれませんが、±９０付近まで計測可能になります。