arduinoでジャイロ

概要

arduino unoにジャイロセンサーをつないで見ました。
L3GD20は、SPIで接続します。ライブラリー書きました。

ログ

写真

回路図

ライブラリー

サンプルコード

#include <SPI.h>
#include <L3GD20.h>

L3GD20 gyro(10);
int dc_offsetX = 0;
int dc_offsetY = 0;
int dc_offsetZ = 0;
unsigned long time;
int rateX;
int rateY;
int rateZ;
int prev_rateX = 0;
int prev_rateY = 0;
int prev_rateZ = 0;
double angleX = 0;
double angleY = 0;
double angleZ = 0;
void setup()
{
    int sampleNum = 500;
    short X, Y, Z;
    float x, y, z;
    Serial.begin(115200);
    Serial.print("start\t");
    gyro.begin();
    Serial.println(" ok!");
    for (int n = 0; n < sampleNum; n++)
    {
        X = gyro.read(L3GD20_X_H);
        x = (X << 8) | gyro.read(L3GD20_X_L);
        dc_offsetX += (int) x;
        Y = gyro.read(L3GD20_Y_H);
        y = (Y << 8) | gyro.read(L3GD20_Y_L);
        dc_offsetY += (int) y;
        Z = gyro.read(L3GD20_Z_H);
        z = (Z << 8) | gyro.read(L3GD20_Z_L);
        dc_offsetZ += (int) z;
    }
    dc_offsetX = dc_offsetX / sampleNum;
    dc_offsetY = dc_offsetY / sampleNum;
    dc_offsetZ = dc_offsetZ / sampleNum;
    Serial.print(" X: ");
    Serial.print(dc_offsetX);
    Serial.print(" Y: ");
    Serial.print(dc_offsetY);
    Serial.print(" Z: ");
    Serial.println(dc_offsetZ);
}
void loop()
{
    int sampleTime = 10;
    short X, Y, Z;
    float x, y, z;
    if (millis() - time > sampleTime)
    {
        time = millis();
        X = gyro.read(L3GD20_X_H);
        x = (X << 8) | gyro.read(L3GD20_X_L);
        Y = gyro.read(L3GD20_Y_H);
        y = (Y << 8) | gyro.read(L3GD20_Y_L);
        Z = gyro.read(L3GD20_Z_H);
        z = (Z << 8) | gyro.read(L3GD20_Z_L);
        rateX = ((int) x - dc_offsetX) / 100;
        angleX += ((double) (prev_rateX + rateX) * sampleTime) / 2000;
        prev_rateX = rateX;
        if (angleX < 0)
        {
            angleX += 360;
        }
        else if (angleX >= 360)
        {
            angleX -= 360;
        }
        rateY = ((int) y - dc_offsetY) / 100;
        angleY += ((double) (prev_rateY + rateY) * sampleTime) / 2000;
        prev_rateY = rateY;
        if (angleY < 0)
        {
            angleY += 360;
        }
        else if (angleY >= 360)
        {
            angleY -= 360;
        }
        rateZ = ((int) z - dc_offsetZ) / 100;
        angleZ += ((double) (prev_rateZ + rateZ) * sampleTime) / 2000;
        prev_rateZ = rateZ;
        if (angleZ < 0)
        {
            angleZ += 360;
        }
        else if (angleZ >= 360)
        {
            angleZ -= 360;
        }
        Serial.print("X: ");
        Serial.print(angleX);
        Serial.print(" rate: ");
        Serial.print(rateX);
        Serial.print(" Y: ");
        Serial.print(angleY);
        Serial.print(" rate: ");
        Serial.print(rateY);
        Serial.print(" Z: ");
        Serial.print(angleZ);
        Serial.print(" rate: ");
        Serial.println(rateZ);
    }
}