MPU-6050３軸ジャイロスコープで3Dサンプルを動かすスケッチについて検討します。
購入先と価格*購入先：アマゾン
*品名：MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール
*価格：￥337（配送を含む）安いです！！（説明書の付属はありません。）
使用方法の参照アドレス（１）Arduino Fio + MPU5060 + Processing
http://twinklesmile.blog42.fc2.com/?tag=IMU
必要ファイル SNS-1007_MPU-6050_Module.rarと ToxicLibs（toxiclibs-complete-0020.zip）がダウンロードできます。
使用ファイルや要修正箇所の解説があります。
使用方法の参照アドレス（２）MPU-6050三軸加速度三軸ジャイロセンサーモジュール - Androciti Wiki
http://wiki.androciti.com/
*Arduinoとの接続図が参考になりますが、旧版(Revision 1)で説明しています。
*新版(Revision 2)はAD0を10kΩでプルダウンしたほうが良いと思います。
*インベンセンス社の開発者コーナー(http://www.invensense.com/developers/index.php?_r=default)で、 アカウント作成を推奨していますが、ログインできませんでした。
MPU-6050評価回路図MPU-6050評価回路図を以下に示します。
MPU-6050評価回路外観MPU-6050評価回路外観を以下に示します。
必要ファイル必要ファイルは以下からダウンロードできます。
[59-1.zip]をダウンロードする。
解凍すると以下のフォルダーがあります。
（１）MPU6050_Aフォルダー
ァイルの保存場所（１）MPU6050_AフォルダーをC:\Users\ユーザ名\Documents\Arduino\librariesフォルダーに保存します。
ArduinoスケッチArduinoスケッチは以下となります。
//MPU6050_MyJet
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define LED_PIN 13
static int16_t ax, ay, az;//加速度
static int16_t gx, gy, gz;//角速度
static long I1ax=0,I1ay=0,I1az=0,I1gx=0,I1gy=0,I1gz=0;//1階積分値
static long I2ax=0,I2ay=0,I2az=0;//2階積分値
static bool blinkState = false;
static long aAx,aAy,aAz,aGx,aGy,aGz;//平均値
void setup()
{
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();//Initializing I2C devices
pinMode(LED_PIN, OUTPUT);
long sAx=0,sAy=0,sAz=0,sGx=0,sGy=0,sGz=0;
int i,N=1000;
for(i=0;i<N;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sAx+=ax;sAy+=ay;sAz+=az;sGx+=gx;sGy+=gy;sGz+=gz;
}
aAx=sAx/N;aAy=sAy/N;aAz=sAz/N;aGx=sGx/N;aGy=sGy/N;aGz=sGz/N;
while (Serial.available() <= 0)
{
Serial.println("0,0,0,0,0,0"); // send an initial string
delay(300);
}
}
void loop()
{
int axp,ayp,azp,gxp,gyp,gzp;
long sAx=0,sAy=0,sAz=0,sGx=0,sGy=0,sGz=0;
int i,j,N=50;
if (Serial.available() > 0)
{
for(i=0;i<N;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sAx+=ax;sAy+=ay;sAz+=az;sGx+=gx;sGy+=gy;sGz+=gz;
}
axp=sAx/N;ayp=sAy/N;azp=sAz/N;gxp=sGx/N;gyp=sGy/N;gzp=sGz/N;
int ai=256;
aAx=(aAx*(ai-1)+axp)/ai;aAy=(aAy*(ai-1)+ayp)/ai;aAz=(aAz*(ai-1)+azp)/ai;
axp=ax-aAx;if(abs(axp)<200){axp=0;}
ayp=ay-aAy;if(abs(ayp)<200){ayp=0;}
azp=az-aAz;if(abs(azp)<200){azp=0;}
if(abs(gxp-aGx)>50){gxp=gxp-aGx;}
else{gxp=0;}
if(abs(gyp-aGy)>50){gyp=gyp-aGy;}
else{gyp=0;}
if(abs(gz-aGz)>50){gzp=gz-aGz;}
else{gzp=0;}
I1ax+=(axp-I1ax/4);I1ay+=(ayp-I1ay/4);I1az+=(azp-I1az/4);
I1gx+=(gxp-I1gx/500);I1gy+=(gyp-I1gy/500);I1gz+=(gzp-I1gz/500);
if(abs(I1ax)<200){I1ax=0;}
if(abs(I1ay)<200){I1ay=0;}
if(abs(I1az)<200){I1az=0;}
I2ax+=(I1ax/2-I2ax/20),I2ay+=(I1ay/2-I2ay/20),I2az+=(I1az/2-I2az/20);
Serial.print(I2ax); Serial.print(",");
Serial.print(I2ay); Serial.print(",");
Serial.print(I2az); Serial.print(",");
Serial.print(I1gx); Serial.print(",");
Serial.print(I1gy); Serial.print(",");
Serial.println(I1gz);
blinkState = !blinkState;// blink LED to indicate activity
digitalWrite(LED_PIN, blinkState);
//delay(20);
int incomingByte = 0; // for incoming serial data
incomingByte = Serial.read();
}
}
Arduinoスケッチ＿テキストファイルArduinoスケッチ＿テキストファイルは以下から参照できます。
Arduinoスケッチ＿テキストファイルにいくProcessingのスケッチProcessingのスケッチを以下に示します。
//sketch_3D_MyJet_MPU6050
import processing.serial.*;
Serial myPort; // The serial port
float My=0;
float Mz=0;
float Mx=0;
void setup ()
{
size(740, 660, P3D);
println(Serial.list());
myPort = new Serial(this, Serial.list()[1], 38400);
myPort.bufferUntil('\n');
}
void draw ()
{
background(0, 256, 256);
lights();
noStroke();
fill(240, 240, 230);
translate(width / 2, height / 2);
translate(mouseX-width / 2,0,200+mouseY- height / 2);
rotateX(Mx);
rotateY(Mz);
rotateZ(My);
OB_A2();//Z軸回転体（胴体）
OB_A3();//Z軸回転体(胴体先端）
OB2();//主翼
OB3();//水平尾翼
OB4();//垂直尾翼
OB_A4();//Z軸回転体(エンジン）
}
void serialEvent(Serial myPort)
{
String myString = myPort.readStringUntil('\n');
myString = trim(myString);
float sensors[] = float(split(myString, ','));
for (int sensorNum = 0; sensorNum < sensors.length; sensorNum++)
{
print(sensors[sensorNum] + "\t");
}
println();
Mx=sensors[3]/10000;
My=sensors[4]/10000;
Mz=sensors[5]/10000;
myPort.write("A");
}
void OB_A4()//Z軸回転体(エンジン）
{
int sides=16;//分割数
int Sn=6;//面数
float Pz[]={0,-1,-50,-60,-70,-70};//z座標
float R[]={15,20,20,15,15,1};//回転物半径
int i,j;
float angleIncrement = TWO_PI/sides;
pushMatrix();
translate(60, 36.3,-145);
for(j=0;j<Sn-1;j++)
{
float angle = 0;
beginShape(QUAD_STRIP);
for (i = 0; i < sides + 1; ++i)
{
vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
angle += angleIncrement;
}
endShape(CLOSE);
}
popMatrix();
pushMatrix();
translate(-60, 36.3,-145);
for(j=0;j<Sn-1;j++)
{
float angle = 0;
beginShape(QUAD_STRIP);
for (i = 0; i < sides + 1; ++i)
{
vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
angle += angleIncrement;
}
endShape(CLOSE);
}
popMatrix();
}
void OB4()//垂直尾翼
{
float Px[]={0,0,3,-3,0,0,2,-2};//ポイントx
float Py[]={0,0,0,0,-80,-80,-80,-80};//ポイントy
float Pz[]={25,-25,0,0,0,-25,-15,-15};//ポイントz
int Sn=10;//面数
int S1[]={0,0,1,1,0,0,1,1,4,5};//面ポイント1
int S2[]={2,4,2,5,3,4,3,5,6,6};//面ポイント2
int S3[]={6,6,5,6,7,7,7,7,7,7};//面ポイント3
int i;
pushMatrix();
translate(0, 0,-395);
for(i=0;i<Sn;i++)
{
beginShape();
vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
endShape(CLOSE);
}
popMatrix();
}
void OB3()//水平尾翼
{
float Px[]={0,0,0,100,100,100};//ポイントx
float Py[]={3,3,-3,2,2,-2};//ポイントy
float Pz[]={15,-15,0,-5,-15,-10};//ポイントz
int Sn=7;//面数
int S1[]={0,0,0,0,1,1,3};//面ポイント1
int S2[]={1,3,2,3,2,4,4};//面ポイント2
int S3[]={4,4,5,5,5,5,5};//面ポイント3
int i;
pushMatrix();
translate(0, 0,-415);
for(i=0;i<Sn;i++)
{
beginShape();
vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
endShape(CLOSE);
}
for(i=0;i<Sn;i++)
{
beginShape();
vertex(-Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
vertex(-Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
vertex(-Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
endShape(CLOSE);
}
popMatrix();
}
void OB2()//主翼
{
float Px[]={10,10,10,220,220,220};//ポイントx
float Py[]={20,20,10,15,15,10};//ポイントy
float Pz[]={40,-40,0,10,-10,0};//ポイントz
int Sn=7;//面数
int S1[]={0,0,0,0,1,1,3};//面ポイント1
int S2[]={1,3,2,3,2,4,4};//面ポイント2
int S3[]={4,4,5,5,5,5,5};//面ポイント3
int i;
pushMatrix();
translate(0, 0,-200);
for(i=0;i<Sn;i++)
{
beginShape();
vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
endShape(CLOSE);
}
for(i=0;i<Sn;i++)
{
beginShape();
vertex(-Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
vertex(-Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
vertex(-Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
endShape(CLOSE);
}
popMatrix();
}
void OB_A3()//Z軸回転体(胴体先端）
{
int sides=16;//分割数
int Sn=8;//面数
float Pz[]={0,-3.2,-11.7,-21.8,-31.9,-45.3,-70.2,-129.4};//z座標
float R[]={0.1,3.7,7.4,9.6,11.8,13.8,17.4,17.7};//回転物半径
int i,j;
float angleIncrement = TWO_PI/sides;
pushMatrix();
translate(0, 8.5);
for(j=0;j<Sn-1;j++)
{
float angle = 0;
beginShape(QUAD_STRIP);
for (i = 0; i < sides + 1; ++i)
{
vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
angle += angleIncrement;
}
endShape(CLOSE);
}
popMatrix();
}
void OB_A2()//Z軸回転体（胴体）
{
int sides=16;//分割数
int Sn=13;//面数
float Pz[]={-40,-46.9,-68.4,-88.2,-108.8,-129.4,-319.4,-346.2,-373,-391.2,-409.4,-433,-440};//z座標
float R[]={0.1,7.8,15.4,19.5,22.3,25,25,22.3,18.7,16,13.3,8.3,0.1};//回転物半径
int i,j;
float angleIncrement = TWO_PI/sides;
for(j=0;j<Sn-1;j++)
{
float angle = 0;
beginShape(QUAD_STRIP);
for (i = 0; i < sides + 1; ++i)
{
fill(240, 240, 230);
if(j==1){fill(20, 20, 40);}
if(j==5 && i==0){fill(20, 20, 40);}
if(j==5 && i==8){fill(20, 20, 40);}
if(j==5 && i==16){fill(20, 20, 40);}
vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
angle += angleIncrement;
}
endShape(CLOSE);
}
}
Processingスケッチ＿テキストファイルProcessingスケッチ＿テキストファイルは以下から参照できます。
Processingスケッチ＿テキストファイルにいく動作テスト（１）MPU-6050３軸ジャイロスコープを動かすとProcessingのコンソールに位置X、位置Y、位置Z、角度Mx、角度My、角度Mzが表示されます。
（２）MPU-6050３軸ジャイロスコープの位置X、位置Y、位置Zを使用して3Dサンプル（sketch_3D_MyJet）を動かすと方向翼の壊れたジェット機のような動きをします。
（３）このため、3Dサンプル（sketch_3D_MyJet）の動きにはMPU-6050３軸ジャイロスコープの、角度Mx、角度My、角度Mzのみを使用しました。
（４）また、マウスの動きで3Dサンプル（sketch_3D_MyJet）のXtoZ方向位置を動かすようにしました。
（５）MPU-6050３軸ジャイロスコープの、角度Mx、角度My、角度Mzを動かすと以下のように方向が変化します。
（６）マウスを動かすと角度を固定したまま、XZ方向に移動します。
MPU-6050３軸ジャイロスコープで3Dサンプルを動かすのまとめ（１）MPU-6050３軸ジャイロスコープは加速度Ax、Ay、Azと角速度Gx、Gy、Gzが計測できます。
（２）加速度Ax、Ay、Azを2階積分すると変位X、Y、Zに変換できます。
（３）角速度Gx、Gy、Gzを1階積分すると角変位Mx、My、Mzに変換できます。
（４）変位X、Y、Zに変換した値は不安定で、正確な制御ができませんでした。
（５）角速度Gx、Gy、Gzに変換した値は十分安定しており、3Dサンプル（sketch_3D_MyJet）の表示角度を正確に制御することができました。
（６）積分処理にあたっては、ノイズ除去のため平均化処理と閾値判定を行いました。
（７）積分処理を行うと若干のドリフトが発生します。
（８）角速度Gx、Gy、Gzの積分にあたっては、ドリフト発散防止のため、1/500の負帰還を行い、故意にゼロ方向のドリフトをあたえました。（僅かな補正です。）
（９）加速度Ax、Ay、Azの2階積分はいろいろと処理条件を変えてみましたが、安定化できませんでした