■mbedプログラミング
・サーボ2はp22に接続します。
　PwmOut servo2(p22);・右、左の感度が違うために、同じように左右に動かしているのに違和感が感じられるときは、次の部分で補正します。ジャイロごとに左右の感度を補正します。

・・ジャイロ1補正
  if(gy1_wk1 > 0){
    gy1_wk1=gy1_wk1*1.07;// 右回転補正
  }
  if(gy1_wk1 < 0){
    gy1_wk1=gy1_wk1*1.0;//　左回転補正
  }

・・ジャイロ2補正
　if(gy2_wk1 > 0){
　　gy2_wk1=gy2_wk1*1.0;// 右回転補正
　}
　if(gy2_wk1 < 0){
　　gy2_wk1=gy2_wk1*1.1;//　左回転補正
　}

■■次のURLでPublishしています。
　　http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test07/lxsreg

■■mbedソース・リスト

// Gyro test07
// Akidukidensi AE-GYRO-SMD
// with 2Gyro+2Servo
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);
PwmOut servo1(p21);
PwmOut servo2(p22);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

int main() {
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  float gy1_avg=0,gy1_sum;
  int i,gy1_lc=9,gy1_lc_old=0;
  float pwidth1;
  float gy2_center;
  float gy2_sgm=0;
  float gy2_wk0=0,gy2_wk1=0;
  float gy2_avg=0,gy2_sum;
  int gy2_lc=9,gy2_lc_old=0;
  float pwidth2;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test07 ****\n");
  servo1.period_ms(20);
  servo2.period_ms(20);

  gy1_center=0;
  gy2_center=0;
  for(i=0;i<300;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    gy2_center=gy2_center+round3(gy2_adc.read()*3.3);
    wait(0.001);
  }
  gy1_center=round3(gy1_center/300);
  gy2_center=round3(gy2_center/300);
  servo1.pulsewidth(0.0015);
  servo2.pulsewidth(0.0015);
  
  while(1){
    gy1_sum=0;
    gy2_sum=0;
    for(i=0;i<20;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      gy2_sum=gy2_sum+round3(gy2_adc.read()*3.3);
      wait(0.005);
    }
    gy1_avg=round3(gy1_sum/20);
    gy2_avg=round3(gy2_sum/20);
    
    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
    gy2_wk1=round3((gy2_avg-gy2_center)*1000/5);
    if(gy1_wk1 > 0){
      gy1_wk1=gy1_wk1*1.07;// migi hosei
    }
    if(gy1_wk1 < 0){
      gy1_wk1=gy1_wk1*1.0;//hidari hosei
    }
    
    if(gy2_wk1 > 0){
      gy2_wk1=gy2_wk1*1.0;// migi hosei
    }
    if(gy2_wk1 < 0){
      gy2_wk1=gy2_wk1*1.1;//hidari hosei
    }
    
    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
    gy2_sgm=round3(gy2_sgm+(gy2_wk0+gy2_wk1)*0.1/2);
    gy1_wk0=gy1_wk1;
    gy2_wk0=gy2_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    else if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    if(gy2_sgm > 90){
      gy2_sgm=90;  
    }
    else if(gy2_sgm < -90){
      gy2_sgm=-90;
    }

    pwidth1=-gy1_sgm;
    pwidth1=(pwidth1+90)/180/1000+0.001;
    gy1_lc=(gy1_sgm-5)/10+9;
    if(gy1_lc >20){
      gy1_lc=19;
    }
    if(gy1_lc <0){
      gy1_lc=0;
    }
    
    pwidth2=gy2_sgm;
    pwidth2=(pwidth2+90)/180/1000+0.001;
    gy2_lc=(gy2_sgm-5)/10+9;
    if(gy2_lc >20){
      gy2_lc=19;
    }
    if(gy2_lc <0){
      gy2_lc=0;
    }
    
    lcd.locate(0,1);
    //lcd.printf("%3.2f %3.2f %2.0f %2.1f",gy1_avg,gy1_center,gy1_sgm,pwidth*1000);
    //lcd.locate(0,2);
    lcd.printf("--------+--------");
    lcd.locate(gy1_lc_old,2);
    lcd.printf(" ");
    lcd.locate(gy1_lc,2);
    lcd.printf("*");
    gy1_lc_old=gy1_lc;
    servo1.pulsewidth(pwidth1);
    lcd.locate(gy2_lc_old,3);
    lcd.printf(" ");
    lcd.locate(gy2_lc,3);
    lcd.printf("*");
    gy2_lc_old=gy2_lc;
    servo2.pulsewidth(pwidth2);
  }//while    
}//main      

■完成！
　二つのサーボを写真のように組み合わせますることで、上下左右の回転を実現します。下がサーボ1でジャイロ1の横回転に対応します。上がサーボ2でジャイロ2の立て回転に対応します。


　ウルトラマンのフィギュアをサーボ・アームに取り付けて、回転の目安としました。ウルトラマンでなくてもかまいません。

　ジャイロ・モジュールにフラットケーブルを取り付けて、延長し、自由に動かすことができるようにしています。

　ジャイロを回転するとウルトラマンもこのように回転します。

　Youtubeに動画を登録しました。モーショントラッキングシステムの動きをご覧ください。
http://www.youtube.com/watch?v=eMc3hsISmpQ




　ラジコン飛行機などに搭載し、操縦者が首を動かすと取り付けているカメラが動くようにするといいですね。

========<コラム>=====================

モーション・トラッキング・システム・・・ジャイロ・モジュールを頭に取り付け、サーボにはカメラを取り付けます。頭を動かす通りにカメラが動くので、ラジコン飛行機やロボットに搭載すれば頭を動かすとカメラをそちらに向けることができます。操縦者の意思をダイレクトに反映することが可能となり、操縦しやすくなります。

補正・・・ジャイロやサーボには個体差があります。少しでも違和感を取り除くために補正が必要です。