#include <Wire.h>
#include <SPI.h>
#include <RF24.h>
const int MPU = 0x68; // MPU6050 I2C address
float accAngleX, accAngleY;
float accAngleErrorX, accAngleErrorY;
float accX, accY, accZ;

RF24 radio(9, 10);

int angles[2];

const uint64_t pipe = 0xE8E8F0F0E1LL;

void setup() {
  Serial.begin(57600);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // resetuje sensor
  Wire.write(0x00);
  Wire.endTransmission(true);
  radio.begin();
  radio.openWritingPipe(pipe);

  delay(20);
}
void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // pobieram 6 rejestrów akcelerometru
  accX = (Wire.read() << 8 | Wire.read()) / 16384.0;  // na poczatku przeczytaj MSB potem LSB i podziel
  accY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  accZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

  accAngleX = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2 ))) / (PI / 180);
  accAngleY = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2 ))) / (PI / 180);



  Serial.print("X: ");
  Serial.print(accAngleX);
  Serial.print(" | Y: ");
  Serial.println(accAngleY);
  angles[0] = (int) accAngleX;
  angles[1] = (int) accAngleY;
  radio.write(angles, sizeof(angles));
}