/* LED Blink, Teensyduino Tutorial #1
   http://www.pjrc.com/teensy/tutorial.html
 
   This example code is in the public domain.
*/
#include <Wire.h>
// Teensy 2.0 has the LED on pin 11
// Teensy++ 2.0 has the LED on pin 6
// Teensy 3.0 has the LED on pin 13
const int ledPin = 13;

// the setup() method runs once, when the sketch starts
int reading = 0;
int device = 104;

void setup() {
  // initialize the digital pin as an output.
  pinMode(ledPin, OUTPUT);
  Wire.begin(); // orange is clock, pin 19, yellow is data pin 18
  Serial.begin(38400);
  Wire.beginTransmission(device); // transmit to device
  Wire.write(byte(0x6b));
  Wire.write(byte(0x01));
  
  //Wire.write(byte(0x0D));
  //Wire.write(byte(0x42));  // 
  Wire.endTransmission();      // stop transmitting
  
}

// the loop() methor runs over and over again,
// as long as the board has power


void loop() {
     // set the LED on
  //delay(2000);                  // wait for a second
  //digitalWrite(ledPin, LOW);    // set the LED off
  //delay(1000);                  // wait for a second
  //Wire.beginTransmission(device); // transmit to device
                               // the address specified in the datasheet is 224 (0xE0)
                               // but i2c adressing uses the high 7 bits so it's 112
  //Wire.write(byte(0x00));      // sets register pointer to the command register (0x00)  
  //Wire.write(byte(0x50));      // command sensor to measure in "inches" (0x50) 
                               // use 0x51 for centimeters
                               // use 0x52 for ping microseconds
  //Wire.endTransmission();      // stop transmitting

  // step 2: wait for readings to happen
  delay(70);                   // datasheet suggests at least 65 milliseconds

  // step 3: instruct sensor to return a particular echo reading
  Wire.beginTransmission(device); // transmit to device
  //Wire.write(byte(0x75));      // ask who am i
  Wire.write(byte(0x3B)); // X Acceleration High Bit
  //Wire.write(byte(0x42));  // 
  Wire.endTransmission();      // stop transmitting

  // step 4: request reading from sensor
  Wire.requestFrom(device, 1);    // request 2 bytes from slave device #112
  
  reading = Wire.read();  // receive high byte (overwrites previous reading)
  Serial.println(reading);   // print the reading
  if (reading > 150){
    digitalWrite(ledPin, HIGH);
    delay(100);
    digitalWrite(ledPin, LOW);
  }

  // step 5: receive reading from sensor
  /*
  if(8 <= Wire.available())    // if bytes were received
  {
    reading = Wire.read();  // receive high byte (overwrites previous reading)
    reading = reading << 8;    // shift high byte to be high 8 bits
    reading |= Wire.read(); // receive low byte as lower 8 bits
    Serial.println(reading);   // print the reading
  }
  */

  delay(50);                  // wait a bit since people have to read the output :)
}