#include <Wire.h>
#include <Servo.h>

short xh;
short xl;
short x;

short yh;
short yl;
short y;

Servo servo;

void readX();
void readY();

void setup(){
  Serial.begin(9600);
  Wire.begin();
  servo.attach(3);
}



void loop(){
  readX();
  readY();

  servo.write(map(x, -32768, 32767, 0, 180));

  delay(100);
}

void readX(){
  Wire.beginTransmission(0x68);
  Wire.write(0x3b);
  Wire.endTransmission(1);
  Wire.requestFrom(0x68, 1, 1);
  xh = Wire.read();
  xh = xh << 8;

  
  Wire.beginTransmission(0x68);
  Wire.write(0x3c);
  Wire.endTransmission(1);
  Wire.requestFrom(0x68, 1, 1);
  xl = Wire.read();

  x = xl | xh;
  Serial.println(x);
}

void readY(){
  Wire.beginTransmission(0x68);
  Wire.write(0x3d);
  Wire.endTransmission(1);
  Wire.requestFrom(0x68, 1, 1);
  yh = Wire.read();
  yh = yh << 8;

  
  Wire.beginTransmission(0x68);
  Wire.write(0x3e);
  Wire.endTransmission(1);
  Wire.requestFrom(0x68, 1, 1);
  yl = Wire.read();

  y = yl | yh;
  Serial.println(y);
}