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