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 使用 ３軸ジャイロ・加速度センサーの最小コスト回路(2)図押しボタンスイッチを追加しました。
MPU-6050 使用 ３軸ジャイロ・加速度センサーの最小コスト回路(2)図を以下に示します。
万能基板組込回路外観万能基板組込回路外観を以下に示します。
必要ファイル必要ファイルは以下からダウンロードできます。
[59-1.zip]をダウンロードする。
解凍すると以下のフォルダーがあります。
（１）MPU6050_Aフォルダー
ァイルの保存場所（１）MPU6050_AフォルダーをC:\Users\ユーザ名\Documents\Arduino\librariesフォルダーに保存します。
ArduinoスケッチArduinoスケッチは以下となります。
//MPU6050_MyJet_V3
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define LED_PIN 13
#define SW_PIN 8
static int16_t ax, ay, az;//加速度
static int16_t gx, gy, gz;//角速度
static long I1gx=10,I1gy=10,I1gz=10;//1階積分値
static bool blinkState = false;
static long aGx,aGy,aGz;//平均値
static int gSW;
void setup()
{
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();//Initializing I2C devices
pinMode(LED_PIN, OUTPUT);
pinMode(SW_PIN, INPUT);
long sGx=10,sGy=10,sGz=10;
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;
while (Serial.available() <= 0)
{
Serial.println("0,0,0,0,0,0,1"); // 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=20;
int SW=digitalRead(SW_PIN);
if(SW==LOW)
{
if(gSW==HIGH)
{
I1gx=0;I1gy=0;I1gz=0;
}
gSW=LOW;
}
else
{gSW=HIGH;}
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;
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);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.print(",");
Serial.println(gSW);
blinkState = !blinkState;// blink LED to indicate activity
if(gSW==LOW){blinkState=HIGH;}
digitalWrite(LED_PIN, blinkState);
}
delay(100);
}
Arduinoスケッチ＿テキストファイルArduinoスケッチ＿テキストファイルは以下から参照できます。
Arduinoスケッチ＿テキストファイルにいく変更点
（１）例題SerialCallResponseASCII方式のシリアル通信に変更しました。
（２）ジャイロは、角速度を積分して、角変位に変換しています。
（３）押しボタンスイッチを押すと角変位をゼロにリセットします。
ProcessingのスケッチProcessingのスケッチを以下に示します。
//sketch_3D_MyJet_MPU6050_V3
import processing.serial.*;
Serial myPort; // The serial port
float My=0;
float Mz=0;
float Mx=0;
float Py=0;
float Pz=0;
float Px=0;
void setup ()
{
size(1000, 800, P3D);
println(Serial.list());
myPort = new Serial(this, Serial.list()[1], 38400);
myPort.bufferUntil('\n');
myPort.write("B");
}
void draw ()
{
background(0, 256, 256);
lights();
noStroke();
fill(240, 240, 230);
translate(width / 2, height / 2);
translate(Px,Pz,Py);
rotateX(Mx);
rotateY(Mz);
rotateZ(My);
OB_A2();//Z軸回転体（胴体）
OB_A3();//Z軸回転体(胴体先端）
OB2();//主翼
OB3();//水平尾翼
OB4();//垂直尾翼
OB_A4();//Z軸回転体(エンジン）
}
void serialEvent(Serial myPort)
{
float r=0.2,Mxn,Myn;
String myString = myPort.readStringUntil('\n');
myString = trim(myString);
int sensors[] = int(split(myString, ','));
for (int sensorNum = 0; sensorNum < sensors.length; sensorNum++)
{
print(sensors[sensorNum] + "\t");
}
println();
Mxn=1.0*atan2(sensors[1],sensors[2]);//重力方向
Myn=1.0*atan2(sensors[0],sensors[2]);//重力方向
if(abs(Mxn) < 3.2 && abs(Myn) < 3.2)
{
if(sensors[6]==1)
{
Mx=Mx*(1-r)+r*Mxn;//重力方向
My=My*(1-r)+r*Myn;//重力方向
Mz=sensors[5]/100000.0;
}
else
{
Px=Px*(1-r)+400*r*Mxn;//重力方向
Pz=Py*(1-r)+1000*r*Myn;//重力方向
Py=sensors[5]/400.0;
}
}
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スケッチ＿テキストファイルにい変更点
（１）例題SerialCallResponseASCII方式のシリアル通信に変更しました。
（２）押しボタンスイッチを押すと角変位をゼロにリセットします。
（３）押しボタンスイッチを押した状態では、画像のXYZ位置を変更します。
（４）角変位Mxと角変位Myを重力加速度の方向から決定するよう変更しました。
（５）重力加速度の方向から求めた角変位はドリフトしません。
（６）水平面内の方向は、重力加速度から求まりません。
（７）水平面内の方向はジャイロの計測値を用いました。
（８）ジャイロの計測値は積分制御のため、原理的に少量のドリフトが発生します。
（１）MPU-6050３軸ジャイロスコープを動かすとProcessingのコンソールに角度Mx、角度My、角度Mzが表示されます。
（２）角度Mx、角度Myは重力方向から求めているため、ドリフトしません。
（３）角度Mzはジャイロから積分制御で求めているため、原理的に少量のドリフトが発生します。
（４）また、押しボタンスイッチを押した状態で3Dサンプル（sketch_3D_MyJet）のXYZ方向位置を動かすようにしました。
（５）MPU-6050３軸ジャイロスコープの、角度Mx、角度My、角度Mzを動かすと以下のように方向が変化します。
（６）押しボタンスイッチを押すと角度を固定したまま、XZ方向に移動します。
MPU-6050 使用 ３軸ジャイロ・加速度センサーの艇コスト回路まとめ（１） MPU-6050 使用 ３軸ジャイロ・加速度センサーの電子回路を低コストで実現できました。
（２）回路は非常にコンパクトで軽量です。
（３）角変位Mxと角変位Myを重力加速度の方向から決定するよう変更しました。
（４）重力加速度の方向から求めた角変位はドリフトしません。
（５）水平面内の方向はジャイロの計測値を用いました。
（６）ジャイロの計測値は積分制御のため、原理的に少量のドリフトが発生します。
（７）押しボタンスイッチを押すと、ジャイロの計測値のドリフトをリセットするようにしました。
（８）押しボタンスイッチを押した状態では、画像のXYZ位置を変更するようにしました。