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

MPU6050 mpu(Wire);

// servo motorji
Servo servox;
Servo servoy;
Servo servoz;

// meje gibanja servo motorjev
const int smax = 180;
const int smin = 0;



void setup() {
  Wire.begin();
  mpu.begin();
  mpu.calcGyroOffsets(true);
  
  // priklop servo motorjev
  servox.attach(11);
  servoy.attach(10);
  servoz.attach(9);

  // postavi servo motorje na sredino
  servox.write(90);
  servoy.write(90);
  servoz.write(90);

}

void loop() {
  mpu.update();

  // preberi pospešek
  float pospesekx = mpu.getAccX(); // pospešek x od -2 do 2
  float pospeseky = mpu.getAccY();
  float pospesekz = mpu.getAccZ();

  // pretvori nagib v kot za servo motor(iz -1 do 1 v 0 do 180)
  int kotx = map(pospesekx * 100, -200, 200, smin, smax);
  int koty = map(pospeseky * 100, -200, 200, smin, smax);
  int kotz = map(pospesekz * 100, -200, 200, smin, smax);
  
  // premaknemo servo motorja
  servox.write(kotx);
  servoy.write(koty);
  servoz.write(kotz);
  delay(50);

}