#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;
float t;
float delta_time;
void setup() {
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
  Serial.begin(9600);
  delay(100);
  mpu.begin();
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  delta_time = 0;
  t = (millis() / 1000.f);
}

float rot_x = 0; // radians
void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  delta_time = (millis() / 1000.f) - t;
  t = (millis() / 1000.f);
  rot_x += g.gyro.x * delta_time;
  Serial.println(rot_x / PI * 180);

  float val0 = max(cos(rot_x), 0);
  float val1 = max(sin(rot_x), 0);
  float val2 = max(-cos(rot_x), 0);
  float val3 = max(-sin(rot_x), 0);

  analogWrite(3, (int)(val0 * val0 * 255));
  analogWrite(5, (int)(val1 * val1 * 255));
  analogWrite(6, (int)(val2 * val2 * 255));
  analogWrite(9, (int)(val3 * val3 * 255));
}