arduinoでジャイロ その２

概要

arduino unoにジャイロをつないでみた。
mpu6050をi2cでつないだ。

写真

ログ

回路図

サンプルコード

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 mpu;
unsigned long time = 0;
float prev_X = 0;
float prev_Y = 0;
float prev_Z = 0;
float angle_X = 0;
float angle_Y = 0;
float angle_Z = 0;
int t = 0;
void setup()
{
    int gx, gy, gz;
    Wire.begin();
    Serial.begin(115200);
    while (!Serial);
    Serial.println("initialize device ...");
    mpu.initialize();
    delay(900);
    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    Serial.println("Updating internal sensor offsets...");
    mpu.setXGyroOffset(-464);
    mpu.setYGyroOffset(-566);
    mpu.setZGyroOffset(232);
    Serial.print(mpu.getXGyroOffset());
    Serial.print("\t");
    Serial.print(mpu.getYGyroOffset());
    Serial.print("\t");
    Serial.println(mpu.getZGyroOffset());
}
void loop()
{
    int az, gx, gy, gz;
    int sampleTime = 10;
    if (millis() - time > sampleTime)
    {
        time = millis();
        t++;
        mpu.getRotation(&gx, &gy, &gz);
        angle_X += ((prev_X + gx) * sampleTime) / 131000;
        prev_X = gx;
        if (angle_X < 0)
        {
            angle_X += 360;
        }
        else if (angle_X >= 360)
        {
            angle_X -= 360;
        }
        angle_Y += ((prev_Y + gy) * sampleTime) / 131000;
        prev_Y = gy;
        if (angle_Y < 0)
        {
            angle_Y += 360;
        }
        else if (angle_Y >= 360)
        {
            angle_Y -= 360;
        }
        angle_Z += ((prev_Z + gz) * sampleTime) / 131000;
        prev_Z = gz;
        if (angle_Z < 0)
        {
            angle_Z += 360;
        }
        else if (angle_Z >= 360)
        {
            angle_Z -= 360;
        }
        if (t > 100)
        {
            t = 0;
            Serial.print(angle_X);
            Serial.print(" ");
            Serial.print(angle_Y);
            Serial.print(" ");
            Serial.println(angle_Z);
        }
    }
}