// Changes by Koepel for a minimalistic working sketch.
// The Acceleration X-axis is used for the servo motor.
// ------------------------------------------
//Created by Barbu Vulc!
//GACSM = Gyro-accelerometer-controlled Servo Motor!

//Libraries:
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>

//Sensor object:
Adafruit_MPU6050 mpu;
//Servo motor object & pin connection:
Servo servo;

//Project initialization!
void setup() 
{
  Serial.begin(115200);
  Serial.println("Adafruit MPU6050 test!");

  servo.attach(13);

  //Initialization!
  if(!mpu.begin()) 
  {
    Serial.println("Failed to find MPU6050 chip");
    while (1) 
    {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  //Setup for accelerometer range:
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  //Setup for gyroscope range:
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  //Set up filter bandwidth:
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
}

void loop() 
{
  //Get new sensor events with the readings.
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  //Print out the values of accelerometer!
  Serial.print("Accel: (");
  Serial.print(a.acceleration.x);
  Serial.print(", ");  
  Serial.print(a.acceleration.y);
  Serial.print(", ");  
  Serial.print(a.acceleration.z);
  Serial.print("), ");
  //Print out the values of gyroscope!
  Serial.print("Gyro: (");  
  Serial.print(g.gyro.x);
  Serial.print(", ");  
  Serial.print(g.gyro.y);
  Serial.print(", ");  
  Serial.print(g.gyro.z);
  Serial.print("), ");
  //Print out the temperature values!
  Serial.print("T: ");  
  Serial.print(temp.temperature);
  Serial.println();
  
  // Use the Acceleration X-axis for the servo motor

  int angle = (int) (a.acceleration.x * 5.0 + 90.0);
  constrain(angle, 0, 180);      // be sure to clip between these values
  servo.write(angle);

  delay(250);       // slow down the sketch
}