68章:MPU-6050 使用 3軸ジャイロ・加速度センサーの低コスト回路(2)

    作成2016.10.07

      MPU-6050 使用 3軸ジャイロ・加速度センサーの低コスト回路の改良を検討してみました。

  1. 基板組込時に不可欠な部品と価格
      Arduino UNOでデバッグが完了した場合、基板組込に不可欠な部品は以下のとおりです。
    (1)AVRマイコンATMEGA328P-PU 1個 小計¥250
    (2)クリスタル(水晶発振子)16MHz 1個 小計¥30
    (3)コンデンサー 22pF  2個 小計¥20
     合計¥300と安価となります。


  2. AVRマイコンATMEGA328P-PUへのブートローダ書込みの必要性
     Arduino UNOでAVRマイコンATMEGA328P-PUにスケッチを書き込むには、あらかじめAVRマイコンATMEGA328P-PUへブートローダを書込む必要があります。
     あらかじめブートローダを書込み済みのAVRマイコンATMEGA328P-PUもあるそうですが、¥250のAVRマイコンATMEGA328P-PUには書き込まれていませんでした。
     書き込み方法の詳細は57章:AVRマイコンATMEGA328P-PUへのブートローダの書込み方法を参照ねがいます。


  3. MPU-6050 使用 3軸ジャイロ・加速度センサーの最小コスト回路(2)図
     押しボタンスイッチを追加しました。
      MPU-6050 使用 3軸ジャイロ・加速度センサーの最小コスト回路(2)図を以下に示します。





  4. 万能基板組込回路外観
     万能基板組込回路外観を以下に示します。





  5. 必要ファイル
     必要ファイルは以下からダウンロードできます。
    [59-1.zip]をダウンロードする。
    解凍すると以下のフォルダーがあります。
    (1)MPU6050_Aフォルダー


  6. ァイルの保存場所
    (1)MPU6050_AフォルダーをC:\Users\ユーザ名\Documents\Arduino\librariesフォルダーに保存します。


  7. 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);
    }
    


  8. Arduinoスケッチ_テキストファイル
     Arduinoスケッチ_テキストファイルは以下から参照できます。
    Arduinoスケッチ_テキストファイルにいく


    変更点
    (1)例題SerialCallResponseASCII方式のシリアル通信に変更しました。
    (2)ジャイロは、角速度を積分して、角変位に変換しています。
    (3)押しボタンスイッチを押すと角変位をゼロにリセットします。


  9. 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);
      }
    }
    


  10. Processingスケッチ_テキストファイル
     Processingスケッチ_テキストファイルは以下から参照できます。
    Processingスケッチ_テキストファイルにい

    変更点
    (1)例題SerialCallResponseASCII方式のシリアル通信に変更しました。
    (2)押しボタンスイッチを押すと角変位をゼロにリセットします。
    (3)押しボタンスイッチを押した状態では、画像のXYZ位置を変更します。
    (4)角変位Mxと角変位Myを重力加速度の方向から決定するよう変更しました。
    (5)重力加速度の方向から求めた角変位はドリフトしません。
    (6)水平面内の方向は、重力加速度から求まりません。
    (7)水平面内の方向はジャイロの計測値を用いました。
    (8)ジャイロの計測値は積分制御のため、原理的に少量のドリフトが発生します。


    (1)MPU-60503軸ジャイロスコープを動かすとProcessingのコンソールに角度Mx、角度My、角度Mzが表示されます。
    (2)角度Mx、角度Myは重力方向から求めているため、ドリフトしません。
    (3)角度Mzはジャイロから積分制御で求めているため、原理的に少量のドリフトが発生します。
    (4)また、押しボタンスイッチを押した状態で3Dサンプル(sketch_3D_MyJet)のXYZ方向位置を動かすようにしました。
    (5)MPU-60503軸ジャイロスコープの、角度Mx、角度My、角度Mzを動かすと以下のように方向が変化します。



    (6)押しボタンスイッチを押すと角度を固定したまま、XZ方向に移動します。






  11. MPU-6050 使用 3軸ジャイロ・加速度センサーの艇コスト回路まとめ
    (1) MPU-6050 使用 3軸ジャイロ・加速度センサーの電子回路を低コストで実現できました。
    (2)回路は非常にコンパクトで軽量です。
    (3)角変位Mxと角変位Myを重力加速度の方向から決定するよう変更しました。
    (4)重力加速度の方向から求めた角変位はドリフトしません。
    (5)水平面内の方向はジャイロの計測値を用いました。
    (6)ジャイロの計測値は積分制御のため、原理的に少量のドリフトが発生します。
    (7)押しボタンスイッチを押すと、ジャイロの計測値のドリフトをリセットするようにしました。
    (8)押しボタンスイッチを押した状態では、画像のXYZ位置を変更するようにしました。




69章:加速度センサADXL345とHMC5883Lコンパス使用低コスト回路に行く。

トップページに戻る。