// LoI2C example.

#include "LoI2C.h"

LoWire loWire;

void setup() 
{
  Serial.begin(115200);

  loWire.init(8, 9); // sda, scl pin


  // I2C Scanner
  Serial.print( "I2C Scan:");
  for( int address = 1; address <= 127; address++)
  {
    if( loWire.write( address, nullptr, 0))
    {
      Serial.print( " 0x");
      if( address < 0x10)
        Serial.print( '0');
      Serial.print( address, HEX);
    }
  }
  Serial.println();

  // Initialize the MPU-6050
  byte data1[2] = { 0x6B, 0x00};   // register 0x6B, value 0x00
  loWire.write( 0x68, data1, sizeof(data1));
}

void loop() 
{
  byte data2[1] = { 0x3B};         // register 0x3B
  loWire.write( 0x68, data2, sizeof(data2), false);   // false for a repeated start

  byte data3[14];
  loWire.read( 0x68, data3, sizeof(data3));
  // dump the data
  for( int i=0; i<14; i++)
  {
    Serial.print( "0x");
    if(data3[i] < 0x10)
      Serial.print( "0");
    Serial.print( data3[i]);
    Serial.print( ", ");
  }
  Serial.println();

  delay(500);
}