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_Engin_MPU6050
import processing.serial.*;
Serial myPort; // The serial port
Part P;//使用クラスの定義
int gCT=0;
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');
P=new Part();//My_OBクラスのオブジェクトを生成
}
void draw ()
{
colorMode(HSB,100,100,100,100);
background(0,0,100);
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);
int N=64;//角度分割数
float Ai=4*PI*gCT/N;
float R=20;
float L=85;
float Aj=asin(R*sin(Ai)/L);
float Yg=R*sin(Ai)/2;
float Xg=R*cos(Ai)+L*cos(Aj)/2;
float Xs=R*cos(Ai)+L*cos(Aj);
gCT++;if(gCT>N){gCT=0;}
scale(1.5,1.5,1.5);
rotateX(-90*PI/180);
pushMatrix();
translate(0,0,-Xs);
fill(60,100, 100,100);
P.Part2();//ピストン
popMatrix();
pushMatrix();
//translate(0,0,-62.5);
fill(40,50, 100,100);
translate(-Yg,0,-Xg);
rotateY(-Aj);
P.Part3();//リンク
popMatrix();
pushMatrix();
fill(70,20, 70,100);
rotateY(Ai);
P.Part4();//クランク
popMatrix();
pushMatrix();
fill(gCT/2,100,100,50);
translate(0,0,-(145+30+Xs)/2);
P.Z_Half_Circle(30,145-30-Xs);//燃焼ガス
popMatrix();
fill(50,80,40,70);
P.Part1();//シリンダ
}
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");
}
//部品
class Part
{
void Part4()//クランク
{
pushMatrix();
rotateX(-90*PI/180);
pushMatrix();
translate(0,0,15);
Z_Bar(30,10);
translate(0,0,37.5);
Z_Bar(5,65);
popMatrix();
translate(0,20,0);
Z_Bar(5,20);
popMatrix();
}
void Part3()//リンク
{
int Pn=9;//ポイント数
float[] X={5,10,10,5,-5,-10,-10,-5,5,5.5};
float[] Y={52.5,47.5,-47.5,-52.5,-52.5,-47.5,47.5,52.5,52.5};
float L=10;
pushMatrix();
rotateX(90*PI/180);
Z_Side(Pn,X,Y,L);//側面
translate(0,0,-L/2);
Z_Polyon(Pn,X,Y);//多角形
translate(0,0,L);
Z_Polyon(Pn,X,Y);//多角形
popMatrix();
}
void Part2()//ピストン
{
pushMatrix();
rotateX(90*PI/180);
Z_Bar(5,60);
popMatrix();
pushMatrix();
translate(0,0,5);
Z_Half_Pipe(30,20,50);//Half_Pipe 半パイプ
translate(0,0,-30);
Z_Half_Circle(30,10);//Half_Circle 半円板
popMatrix();
}
void Part1()//シリンダ
{
pushMatrix();
translate(0,0,-2.5);
Z_Half_Pipe(50,40,75);//Half_Pipe 半パイプ
translate(0,0,-90);
Z_Half_Pipe(40,30,125);//Half_Pipe 半パイプ
translate(0,0,-57.5);
Z_Half_Circle(40,10);//Half_Circle 半円板
popMatrix();
pushMatrix();
//void Z_Pipe(float Ro,float Ri,float L)//パイプ
rotateX(90*PI/180);
translate(0,0,-40);
Z_Pipe(15,5,40);
popMatrix();
}
void Z_Half_Pipe(float R1,float R2,float L)//Half_Pipe 半パイプ
{
int Pn1=8;//R1,R2分割数
int Pn=2*Pn1+3;//全ポイント数
float[] X=new float[Pn];
float[] Y=new float[Pn];
int i;
float Ai;
for(i=0;i<=Pn1;i++)
{
Ai=i*PI/Pn1;
X[i]=R1*cos(Ai);
Y[i]=R1*sin(Ai);
}
for(i=0;i<=Pn1;i++)
{
Ai=PI-i*PI/Pn1;
X[i+Pn1+1]=R2*cos(Ai);
Y[i+Pn1+1]=R2*sin(Ai);
}
X[2*Pn1+2]=R1;Y[2*Pn1+2]=0;
pushMatrix();
Z_Side(Pn,X,Y,L);//側面
translate(0,0,-L/2);
Z_Polyon(Pn,X,Y);//多角形
translate(0,0,L);
Z_Polyon(Pn,X,Y);//多角形
popMatrix();
}
void Z_Half_Circle(float R1,float L)//Half_Circle 半円板
{
int Pn1=8;//R1分割数
int Pn=Pn1+2;//全ポイント数
float[] X=new float[Pn];
float[] Y=new float[Pn];
int i;
float Ai;
for(i=0;i<=Pn1;i++)
{
Ai=i*PI/Pn1;
X[i]=R1*cos(Ai);
Y[i]=R1*sin(Ai);
}
X[Pn1+1]=R1;Y[Pn1+1]=0;
pushMatrix();
Z_Side(Pn,X,Y,L);//側面
translate(0,0,-L/2);
Z_Polyon(Pn,X,Y);//多角形
translate(0,0,L);
Z_Polyon(Pn,X,Y);//多角形
popMatrix();
}
void Z_Bar(float R1,float L)//Bar 丸棒
{
int Pn1=16;//R1分割数
int Pn=Pn1+2;//全ポイント数
float[] X=new float[Pn];
float[] Y=new float[Pn];
int i;
float Ai;
for(i=0;i<=Pn1;i++)
{
Ai=i*2*PI/Pn1;
X[i]=R1*cos(Ai);
Y[i]=R1*sin(Ai);
}
X[Pn1+1]=R1;Y[Pn1+1]=0;
pushMatrix();
Z_Side(Pn,X,Y,L);//側面
translate(0,0,-L/2);
Z_Polyon(Pn,X,Y);//多角形
translate(0,0,L);
Z_Polyon(Pn,X,Y);//多角形
popMatrix();
}
void Z_Side(int Pn,float X[],float Y[],float L)//側面
{
int i;
beginShape(QUAD_STRIP);
for (i = 0; i < Pn; ++i)
{
vertex(X[i],Y[i], -L/2);
vertex(X[i],Y[i], L/2);
}
endShape(CLOSE);
}
void Z_Polyon(int Pn,float X[],float Y[])//多角形
{
int i;
beginShape();
for (i = 0; i < Pn; ++i)
{
vertex(X[i],Y[i]);
}
endShape(CLOSE);
}
void Z_Pipe(float Ro,float Ri,float L)//パイプ
{
pushMatrix();
Z_Cylinder(Ro,L);
Z_Cylinder(Ri,L);
translate(0,0,-L/2);
Z_Ring_2D(Ro,Ri);
translate(0,0,L);
Z_Ring_2D(Ro,Ri);
popMatrix();
}
void Z_Cylinder(float r,float L)//円筒
{
int sides=16;//分割数
int i;
beginShape(QUAD_STRIP);
for (i = 0; i < sides + 1; ++i)
{
float angle = i*TWO_PI/sides;
vertex(r*cos(angle), r*sin(angle), -L/2);
vertex(r*cos(angle), r*sin(angle), L/2);
}
endShape(CLOSE);
}
void Z_Ring_2D(float Ro,float Ri)
{
int sides=16;//分割数
int i;
beginShape();
for (i = 0; i < sides + 1; ++i)
{
float angle = i*TWO_PI/sides;
vertex(Ro*cos(angle), Ro*sin(angle),0);
}
beginContour();
for (i = 0; i < sides + 1; ++i)
{
float angle = -i*TWO_PI/sides;
vertex(Ri*cos(angle), Ri*sin(angle),0);
}
endContour();
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サンプルを動かす（２）のまとめ（１）画像の変更のみですので修正は簡単でした。
（２）sketch_3D_Enginは部品の動きがあるので、よりリアルな感じがします。