Raspberry Pi と Arduino で楽器作り(14) - ジャイロで角度検出

ジャイロセンサで角度の状態を出力しようともがきます。

1. 積分して角度を求める

前回の投稿のあと、積算すれば角度が求められるとの指摘をいただきました。そういった発想のない自分にはおどろきでしたが、わりと当然の知識のようでした。言われてみれば角速度(deg/s)に時間(s)を掛ければそりゃ角度になりますね。というわけで Arduino で積分をしてみます。

static deg1, deg2;
long g1_init = 289;
long g2_init = 286;
...

void loop(){
  int g1 = analogRead(G1PIN) - g1_init;
  int g2 = analogRead(G2PIN) - g2_init;

  deg1 += g1 * 0.005;
  deg2 += g2 * 0.005;
  
  Serial.print(deg1);
  Serial.print(",");
  Serial.print(deg2);
}

何度も足していくために、 loop() 関数のスコープ外に static な変数を用意します。また、静止時の値を0にするために初期値を引いています。これで積分したことになるかなと思ってやってみると……。

2. が、うまくいかない

ちゃんと積分したじゃんか

なぜか上のグラフのようになってくれず、0に戻ってしまいます。センサの値を出力してみると、止まる前に負の値が出ているのもわかります。どうしたもんだと思って調べていると、同じ問題を扱ったサイトがありました。
参考：角速度（ジャイロ）センサ値を正しい角速度に変換する①
センサ特性を見るにおそらく使っているセンサは同じなのですが、上のページを③まで読むと、この村田製作所のENC-03Rというのは角加速度を出力するみたいです。角速度は角加速度を積分したものなので、2回積分すれば角度が出るということですね。めんどくさい……。上記のページを参考に、まずは傾けたら絶対値の大きな値が出て、元に戻したら絶対値の小さな値が出るようにしたいと思います。

static int deg1 = 0, deg2 = 0;
static float gv1 = 0, gv2 = 0;
int zcnt1 = 0, zcnt2 = 0;
long ga1_init = 289;
long ga2_init = 286;

void setup() {
  Serial.begin(9600);
}
void loop() {
  float garead1, garead2;
  float ga1 = 0, ga2 = 0;
  int i;

  garead1 = analogRead(G1PIN);
  garead2 = analogRead(G2PIN);
  
  if(abs(garead1 - ga1_init) <= 3) {
    garead1 = ga1_init;
    zcnt1 += 1;
  }else{
    zcnt1 = 0;
  }
  if(abs(garead2 - ga2_init) <= 3) {
    garead2 = ga2_init;
    zcnt2 += 1;
  }else{
    zcnt2 = 0;
  }
  for(i = 0; i < 10; i++) {
    ga1 += (garead1 - ga1_init) * 5 / (1023 * 0.00067);
    ga2 += (garead2 - ga2_init) * 5 / (1023 * 0.00067);
  }
  ga1 /= 10;
  ga2 /= 10;

  if(zcnt1 > 3) {
    gv1 = 0;
  }else{
    gv1 += ga1;
  }
  if(zcnt2 > 3) {
    gv2 = 0;
  }else{
    gv2 += ga2;
  }
  
  deg1 += gv1 * 0.005;
  deg2 += gv2 * 0.005;

  Serial.print(deg1);
  Serial.print(",");
  Serial.println(deg2);
}

基本は2回積分を行っているだけですが、それ以外の先ほどのものからの変更は橙色の部分です。角加速度の絶対値が小さい場合は0にすることと、0が続いたとき角速度を0にリセットするということをしています。このコードだと結果は以下のようになりました。

そう簡単にはいかない

水平に戻しても0まで戻ってくれず、何回もやっているとこの誤差がつみあがっていってしまいます。これでは正しく傾いていることを検出できないので、このへんを修正します。

3. 戻そうとしたことを検知

どうやって修正しようかと考えると、つまり上がった分だけ下がってくれないのが問題なので、下げようとする入力があったら強制的に0にしてしまえばいいのではないかと考えました。

  if(gv1 < 0){
    mcnt1 += 1;
  }else if(gv1 > 0){
    pcnt1 += 1;
  }else{
    pcnt1 = 0;
    mcnt1 = 0;
  }
  if(gv2 < 0){
    mcnt2 += 1;
  }else if(gv2 > 0){
    pcnt2 += 1;
  }else{
    pcnt2 = 0;
    mcnt2 = 0;
  }
  if(deg1 > 4000 && mcnt1 > 80) {
    deg1 = 0;
  }
  if(deg2 > 4000 && mcnt2 > 80) {
    deg2 = 0;
  }
  if(deg1 < -4000 && pcnt1 > 80) {
    deg1 = 0;
  }
  if(deg2 < -4000 && pcnt2 > 80) {
    deg2 = 0;
  }  

これをさっきの赤マーカーのところに挿入しました。pcntで正方向の入力、mcntで負方向の入力をカウントし、角度が正のとき負方向の入力が、角度が負のとき正方向の入力がそれぞれ80より多く連続した場合、強制的に0に戻しています。80は適当に決めた値ですが、大きすぎるとリセットされず、小さすぎるとすぐリセットされてしまいそこからさらに値が動いてしまうので使い物になりません。しかし、この状態でも80回を超える正/負の入力があった場合、0から動いてしまいます。ただこれが4000より小さい値に収まるように各種値を調整すれば、傾いているかいないかの検出は楽にできると思います。これに加えて、ある値以下で安定していた場合強制的に0にリセットする機能もつけました。

  if(abs(deg1) < 4000 && deg1 != 0) {
    stablecnt1 += 1;
  }else{
    stablecnt1 = 0;
  }
  if(abs(deg2) < 4000 && deg2 != 0) {
    stablecnt2 += 1;
  }else{
    stablecnt2 = 0;
  }
  if(stablecnt1 > 80) {
    deg1 = 0;
  }
  if(stablecnt2 > 80) {
    deg2 = 0;
  }

上に傾けたあと戻し、左にひねってみると、実際のグラフは下のようになりました。

やりたいことができた！

 絶対値が4000より大きくなる角度で維持すれば値が維持され、それ以外のときには0になることがわかります。一応はこれで思った通りの動作をしたと思います。まだたまに0に戻らないことがありますが、左ひねりと上傾けしか使わない予定なので、それ以外の数値を切り捨てるようにすればもっと感度の高い検出ができるのかなと思うので、ちゃんと動くようにしたいです。