/*
     * Created by Cuautli Garcia and using example codes
     * written by Limor Fried/Ladyada from Adafruit Industries
     * 
     * FabAcademy
     *
     * Version 1.0
     * Last Autor: Cuautli Garcia - cuautli.garciaa@gmail.com
     *
     */
      
#include <Adafruit_MPU6050.h> //Library for MPU6050
#include <Adafruit_Sensor.h>  //Library for sensors
#include <Adafruit_PWMServoDriver.h>//Library for PCA9685
#include <Wire.h>             //Library for I2C communication

byte data[1];                 //Variable to save the recieved value
String readwrite;             //String to save what the keyboard sends
String value;                 //String to save what the keyboard sends
Adafruit_MPU6050 mpu;         //Create and instance for the MPU
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);//Initialize PCA9685
#define SERVOMIN  100         //Minimum pulse length count (out of 4096)
#define SERVOMAX  500         //Maximum pulse length count (out of 4096)
#define SERVO_FREQ 50         //Servomotor frequency for analog servos at 50Hz

void setup(void) {
  Serial.begin(115200);       //Start serial communication at 115200
  while (!Serial)
    delay(10);                //Pauses the board until serial console is openned
  Serial.println("I2C & Serial test!");//I2C & serial practice
  if (!mpu.begin()) {         //Initialize the MPU6050
    Serial.println("Failed to find MPU6050 chip");//If the MPU cannot be initialized
  }
  Serial.println("MPU6050 Found!");//MPU was found
  //Set the ranges and bands of the MPU6050
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");         //Print a space
  pwm.begin();                //Initialize the PCA
  pwm.setOscillatorFrequency(27000000);//Set the clock oscillator at 27 MHz
  pwm.setPWMFreq(SERVO_FREQ); //Frecuecia PWM de 50Hz o T=20ms
  delay(100);                 //Delay of 100 ms
}

void writeMPU(){              //Function to talk with the MPU
  sensors_event_t a, g, temp; //Get the MPU events
  mpu.getEvent(&a, &g, &temp);//Assign the values to a,g and temp
  //Print acceleration values
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");
  //Print rotation values
  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");
  //Print temperature values
  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");
  Serial.println("");
  }

  void writeDIG(){          //Function to read the ATtiny45
  Wire.requestFrom(60,1);   //Request to the created board the value of the sensor
  data[1] = Wire.read();    //Assign the read value to data
  //Print digital sensor value
  Serial.print("Digital sensor: ");
  Serial.println(data[1]);
  Serial.println("");
    }

void loop() {
  //Print initial message
  Serial.println("Do you want to read a sensor or write a value for an actuator?");
  Serial.println("Type read or write depending on the required action");
  //Wait for an answer
  while (Serial.available()==0) {  }
  if(Serial.available()){
    //Assign the written value to the readwrite variable
    readwrite = Serial.readStringUntil('\n');
  }
  if(readwrite == "read"){//If I choose to read
        //Message to know what reading I want to do
        Serial.println("Write mpu to read the MPU6050 or dig to read the digital input");
        while (Serial.available()==0) {  }  //Wait for an answer
        if(Serial.available()){
          //Assign the written text to the value variable
          value = Serial.readStringUntil('\n');
        }
        if(value != "0"){//If the value is different than 0
          if(value == "mpu"){ //If the user chooses the mpu sensor
            Serial.println("The MPU readings are:");//Print message
            writeMPU();       //Call the MPU function
          }else if(value == "dig"){//If the user chooses the ATtiny45
            Serial.println("The digital reading is:");//Print message
            writeDIG();       //Call the sensor of the ATtiny45 
          }
        }
  }
  if(readwrite == "write"){//If I choose to write
        //Message to know what do the user wants to activate
        Serial.println("Write the servomotor position followed by ':' and the angle");
        Serial.println("For example: to move the servo 8 to 90°, write 0890");
        while (Serial.available()==0) {  }  //Wait for an answer
        if(Serial.available()){
          //Assign the written text to the value variable
          value = Serial.readStringUntil('\n');
        }
        if(value != "0"){//If the value is different than 0
          String serv = value.substring(0,2);//Get the first two characters
          String posi = value.substring(2,5);//Get the next three characters
          int servo = serv.toInt();         //Convert the text to an integer
          int pos = posi.toInt();           //Convert the text to an integer
          //Calculate the pulse length depending on the given value
          int real = (pos*(SERVOMAX-SERVOMIN)/180)+SERVOMIN;
          //Print the servo number followed by the pulse length
          Serial.print(servo);
          Serial.print(" -> ");
          Serial.println(real);
          //Assign the pulse lenght to the given servomotor
          pwm.setPWM(servo, 0, real);
          //Wait for 0.5s
          delay(500);      
        }
  }
  //Wait a second
  delay(1000);
}