#include <Wire.h>

void setup() {
  Serial.begin(115200);
  Serial.println("BMI160 Test");
  Wire.begin();
  delay(100);
  
  Wire.beginTransmission(0x68);
  Wire.write(0x7E);
  Wire.write(0x11);
  Wire.endTransmission(1);
  delay(50);
  Wire.beginTransmission(0x68);
  Wire.write(0x7E);
  Wire.write(0x15);
  Wire.endTransmission(1);
  delay(100);
}

void loop() {
  int16_t d[6];
  Wire.beginTransmission(0x68);
  Wire.write(0x12);
  Wire.endTransmission(1);
  Wire.requestFrom(0x68,6);
  d[0]=Wire.read()|(Wire.read()<<8);
  d[1]=Wire.read()|(Wire.read()<<8);
  d[2]=Wire.read()|(Wire.read()<<8);
  Wire.beginTransmission(0x68);
  Wire.write(0x0C);
  Wire.endTransmission(1);
  Wire.requestFrom(0x68,6);
  d[3]=Wire.read()|(Wire.read()<<8);
  d[4]=Wire.read()|(Wire.read()<<8);
  d[5]=Wire.read()|(Wire.read()<<8);

  Serial.print("X:");Serial.print(d[0]/16384.0,2);Serial.print(" Y:");
  Serial.print(d[1]/16384.0,2);Serial.print(" Z:");Serial.print(d[2]/16384.0,2);
  Serial.print(" RX:");Serial.print(d[3]/131.0,1);Serial.print(" RY:");
  Serial.print(d[4]/131.0,1);Serial.print(" RZ:");Serial.println(d[5]/131.0,1);
  delay(200);
}