/*
 MPU6050 conectat: SDA la GPIO21 SCK la GPIO22
 trimite unghiuri reala pe Terminal V12
 afiseaza pe oled ax, ay, az si temp
 Oled conectat: SDA la GPIO21 SCK la GPIO22
 amble sunt I2C dar identificate prin adresele lor din cod
*/

#define BLYNK_PRINT Serial
#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>

#include <SimpleTimer.h>
SimpleTimer timer;

char auth[] = "e5beac0d31f4433da82dc64b99a03b39"; 
char ssid[] = "UPCF4821BC";
char pass[] = "Gherla1956";


#include<Wire.h>
#include <SPI.h>


#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#define OLED_ADDR   0x3C
Adafruit_SSD1306 display(-1);


 
const int MPU_addr=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
 
int minVal=265;
int maxVal=402;
 
double x;
double y;
double z;
double t;


void setup(){

Serial.begin(115200);
   SPI.begin();
    Blynk.begin(auth, ssid, pass);
    timer.setInterval(1000L, sendUptime);
  
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
//Serial.begin(115200);

display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);

display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print(" Blynk MPU 6050");
  display.display();
  delay(1000);
 
}

void sendUptime(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
Tmp=Wire.read()<<8|Wire.read();

int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
 
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
t=Tmp/340.00+36.53;

   Blynk.virtualWrite(12, "  AngleX= ");
   Blynk.virtualWrite(12, x); 
   Blynk.virtualWrite(12, "  AngleY= ");
   Blynk.virtualWrite(12, y); 
   Blynk.virtualWrite(12, "  AngleZ= ");
   Blynk.virtualWrite(12, z); 
   Blynk.virtualWrite(12, "  Temp= ");
   Blynk.virtualWrite(12, t); 

Serial.print("AngleX= ");
Serial.println(x);

Serial.print("AngleY= ");
Serial.println(y);
 
Serial.print("AngleZ= ");
Serial.println(z);

Serial.print("T= ");
Serial.println(t);

Serial.println("-----------------------------------------");
 
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0);
display.print("X: ");
display.println(x);

display.setTextSize(2);
display.setCursor(0,20);
display.print("Y: ");
display.println(y);
 
display.setTextSize(2);
display.setCursor(0,40);
display.print("Z: ");
display.println(z);

display.setCursor(0,60);
display.print("T: ");
display.println(t);
display.display();
     } 

void loop()
{
  Blynk.run(); 
  timer.run();
}
