MPU-6050 使用 ３軸ジャイロ・加速度センサーの低コスト回路について検討してみました。
基板組込時に不可欠な部品と価格Arduino UNOでデバッグが完了した場合、基板組込に不可欠な部品は以下のとおりです。
（１）AVRマイコンATMEGA328P-PU 1個 小計￥250
（２）クリスタル（水晶発振子）１６ＭＨｚ 1個 小計￥30
（３）コンデンサー 22pF 2個 小計￥20
合計￥300と安価となります。
AVRマイコンATMEGA328P-PUへのブートローダ書込みの必要性Arduino UNOでAVRマイコンATMEGA328P-PUにスケッチを書き込むには、あらかじめAVRマイコンATMEGA328P-PUへブートローダを書込む必要があります。
あらかじめブートローダを書込み済みのAVRマイコンATMEGA328P-PUもあるそうですが、￥250のAVRマイコンATMEGA328P-PUには書き込まれていませんでした。
書き込み方法の詳細は５７章：AVRマイコンATMEGA328P-PUへのブートローダの書込み方法を参照ねがいます。
MPU-6050 使用 ３軸ジャイロ・加速度センサーの最小コスト回路図観MPU-6050 使用 ３軸ジャイロ・加速度センサーの最小コスト回路図を以下に示します。
動作確認用の仮配線回路外観動作確認用の仮配線回路外観を以下に示します。
万能基板組込回路外観万能基板組込回路外観を以下に示します。
ArduinoスケッチArduinoスケッチは以下となります。
//MPU6050_MyJet_V2
#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 I1gx=0,I1gy=0,I1gz=0;//1階積分値
static bool blinkState = false;
static long aGx,aGy,aGz;//平均値
void setup()
{
Wire.begin();
Serial.begin(19200);
accelgyro.initialize();//Initializing I2C devices
pinMode(LED_PIN, OUTPUT);
long sGx=0,sGy=0,sGz=0;
int i,N=500;
delay(300);
for(i=0;i< N;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sGx+=gx;sGy+=gy;sGz+=gz;
}
aGx=sGx/N;aGy=sGy/N;aGz=sGz/N;
//delay(100);
}
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=20;
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;
axp=ax;
ayp=ay;
azp=az;
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;}
//I1gx+=(gxp-I1gx/500);I1gy+=(gyp-I1gy/500);I1gz+=(gzp-I1gz/500);
I1gx+=(gxp);I1gy+=(gyp);I1gz+=(gzp);
Serial.print(axp); Serial.print(" ");
Serial.print(ayp); Serial.print(" ");
Serial.print(azp); Serial.print(" ");
Serial.print(I1gx); Serial.print(" ");
Serial.print(I1gy); Serial.print(" ");
Serial.print(I1gz);Serial.println(" ");
blinkState = !blinkState;// blink LED to indicate activity
digitalWrite(LED_PIN, blinkState);
//delay(50);
}
Arduinoスケッチ＿テキストファイルArduinoスケッチ＿テキストファイルは以下から参照できます。
Arduinoスケッチ＿テキストファイルにいく変更点
（１）計測値は20回の平均としました。
（２）加速度は単純平均値としました。
（３）ジャイロはvoid setup()で角速度計測値のゼロ補正用平均値を求めます。
（４）ジャイロは、角速度を積分して、角変位に変換しています。
ProcessingのスケッチProcessingのスケッチを以下に示します。
//3D_MyJet_MPU6050_V2
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], 19200);
}
void draw ()
{
float r=0.2,Mxn,Myn;
if(myPort.available()>16 )//受信データが16バイト以上の場合
{
String myString = myPort.readString();//受信バッファを全部読む
float sensors[] = float(split(myString, ' '));
if(sensors.length==7)
{
Mxn=2*atan2(sensors[1],sensors[2]);//重力方向
Myn=2*atan2(sensors[0],sensors[2]);//重力方向
if(abs(Mxn) < 3.2 && abs(Myn) < 3.2)
{
Mx=Mx*(1-r)+r*Mxn;//重力方向
My=My*(1-r)+r*Myn;//重力方向
}
//Mx=sensors[3]/200000;//ジャイロ
//My=sensors[4]/200000;//ジャイロ
Mz=sensors[5]/200000;//ジャイロ
println(Mx + "\t"+My+ "\t"+Mz );
}
}
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 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スケッチ＿テキストファイルにいく変更点
（１）角変位Mxと角変位Myを重力加速度の方向から決定するよう変更しました。
（２）重力加速度の方向から求めた角変位はドリフトしません。
（３）水平面内の方向は、重力加速度から求まりません。
（４）水平面内の方向はジャイロの計測値を用いました。
（５）ジャイロの計測値は積分制御のため、原理的に少量のドリフトが発生します。
動作テスト（１）MPU-6050３軸ジャイロスコープを動かすとProcessingのコンソールに角度Mx、角度My、角度Mzが表示されます。
（２）角度Mx、角度Myは重力方向から求めているため、ドリフトしません。
（３）角度Mzはジャイロから積分制御で求めているため、原理的に少量のドリフトが発生します。
（４）また、マウスの動きで3Dサンプル（sketch_3D_MyJet）のXtoZ方向位置を動かすようにしました。
（５）MPU-6050３軸ジャイロスコープの、角度Mx、角度My、角度Mzを動かすと以下のように方向が変化します。
（６）マウスを動かすと角度を固定したまま、XZ方向に移動します。
MPU-6050 使用 ３軸ジャイロ・加速度センサーの艇コスト回路まとめ（１） MPU-6050 使用 ３軸ジャイロ・加速度センサーの電子回路を低コストで実現できました。
（２）回路は非常にコンパクトで軽量です。
（３）角変位Mxと角変位Myを重力加速度の方向から決定するよう変更しました。
（４）重力加速度の方向から求めた角変位はドリフトしません。
（５）水平面内の方向はジャイロの計測値を用いました。
（６）ジャイロの計測値は積分制御のため、原理的に少量のドリフトが発生します。