imu.cpp

#include <Servo.h>

#include <SPI.h>
#include <Wire.h>
#include <SparkFunLSM9DS1.h>


Servo servo;

LSM9DS1 imu;

const int TRIG_PIN = 16;
const int ECHO_PIN = 2;

const int IMU_M = 0x1e;
const int IMU_AG = 0x6b;

void setup() {
    // put your setup code here, to run once:

    servo.attach(12);

    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    digitalWrite(TRIG_PIN, LOW);

    imu.settings.device.commInterface = IMU_MODE_I2C; // Set mode to I2C
    imu.settings.device.mAddress = IMU_M; // Set mag address to 0x1E
    imu.settings.device.agAddress = IMU_AG; // Set ag address to 0x6B

    Serial.begin(9600);

    while(!imu.begin()) {
        Serial.println("Failed to start IMU");
        delay(1000);
    }
}

void loop() {
    // servo.write(45);

    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    auto duration = pulseIn(ECHO_PIN, HIGH);

    auto distance = duration / 29.1 / 2;

    // Serial.print(distance);
    // Serial.println(" cm");

    // servo.write(distance);

    imu.readAccel();
    Serial.println(imu.az);
}