#include <Arduino.

h>
#include <ESP8266WiFi.h>
#include <ESPAsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <AsyncElegantOTA.h>
#include "DHT.h"
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Wire.h>
#include "I2Cdev.h"
// #include "MPU6050.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
#define yaw_ 0
#define pitch_ 1
#define roll_ 2
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success,
!0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
unsigned int esc_value[4];
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor
measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity
vector
int16_t gyro[3];
const int MPU = 0x68;
int16_t ax, ay, az;
int16_t gx, gy, gz;
//SPIClass* hspi = NULL;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int16_t gyro_x, gyro_y, gyro_z, temperature;
float angle_pitch, angle_roll, angle_yaw;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
long Time, timePrev, time2;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
int c = 0;
float acc_total;
#define csn 15
#define mosi 13 //hspi
#define miso 12
#define clk 14
#define ce 1
float giatri[6] = { 1, 2, 3, 0, 0, 2 }; // gia tri cua cam
bien // CE, CSN
const uint64_t address = 0xF0F0F0F0E1LL;
const char* ssid = "Redmi Note 11"; //replace with your SSID
const char* password = "12345678"; //replace with your password
AsyncWebServer server(80);
#define DHTPIN 2
#define DHTTYPE DHT22
DHT dht(DHTPIN, DHTTYPE);
RF24 radio(1, 15); // CE, CSN
void imu_cal2() {
}
MPU6050 accelgyro;

bool connect_wifi = 1;
float acc_total_first;
void get_mpu() {
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.getRotation(&gyro[roll_], &gyro[pitch_], &gyro[yaw_]);
// 131.0
float acc_total_vector = sqrt((aaReal.x * aaReal.x) + (aaReal.y * aaReal.y) +
(aaReal.z * aaReal.z)); //Calculate the total accelerometer vector
if (abs(aaReal.y) < acc_total_vector) {
//Prevent the asin function to produce a NaN
angle_pitch_acc = asin((float)aaReal.y / acc_total_vector);
//Calculate the pitch angle.
}
if (abs(aaReal.x) < acc_total_vector) { //Prevent
the asin function to produce a NaN
angle_roll_acc = asin((float)aaReal.x / acc_total_vector) * -1; //Calculate
the roll angle.
}
ypr[1] = ypr[1] * 0.97 + angle_pitch_acc * 0.03;
ypr[2] = ypr[2] * 0.97 + angle_roll_acc * 0.03;
//Serial.println(ypr[1]);
/*
static unsigned long time_mpu = millis();
 unsigned long time_bw = millis() - time_mpu;
time_mpu = millis();
Serial.println("mpu:" + String(time_bw));

*/

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

float cvt_ax = (float)ax / 16384.0;
float cvt_ay = (float)ay / 16384.0;
float cvt_az = (float)az / 16384.0;
cvt_ax = cvt_ax * 9.8;
cvt_ay = cvt_ay * 9.8;
cvt_az = cvt_az * 9.8;
acc_total = sqrt(pow(cvt_ax, 2) + pow(cvt_ay, 2) + pow(cvt_az, 2));
giatri[0] = acc_total;
giatri[3] = ypr[1];
giatri[4] = ypr[2];
}
}
void acc_setup() {
/*
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission(); //End the transmission
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission();
*/

Wire.begin();

// initialize device
mpu.initialize();
accelgyro.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(10);
mpu.setYGyroOffset(-13);
mpu.setZGyroOffset(29);
mpu.setZAccelOffset(1556);
if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
//mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection

// attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady,
RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
 // get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
}
}
void setup(void) {
Serial.begin(9600);
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
Serial.println("");
uint16_t count_delay = 0;
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
count_delay++;
if (count_delay >= 30) {
yield();
connect_wifi = 0;
dht.begin();
acc_setup();
radio.begin();
radio.openWritingPipe(address);
radio.setPALevel(RF24_PA_MIN);
radio.stopListening();
return;
}
}
Serial.println("");
Serial.print("IP address: ");
Serial.println(WiFi.localIP());

//or use this for auto generated name ESP + ChipID

//wifiManager.autoConnect();
//if you get here you have connected to the WiFi
Serial.println("connected...yeey :)");
server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
request->send(200, "text/plain", "Hello! This is a simple text message sent
from ESP8266. Microcontrollerslab");
});

AsyncElegantOTA.begin(&server); // Start ElegantOTA

server.begin();
Serial.println("HTTP server started!");
// dht.begin();
dht.begin();
acc_setup();
radio.begin();
radio.openWritingPipe(address);
radio.setPALevel(RF24_PA_MIN);
radio.stopListening();
delay(20);
}

void loop(void) {
if (connect_wifi == 1) AsyncElegantOTA.loop();
static uint8_t state = 0;
static unsigned long time_ = millis();
get_mpu();
if (millis() - time_ > 300) {
 time_ = millis();

float humi = dht.readHumidity();

float temp = dht.readTemperature();
if (isnan(humi) || isnan(temp)) {
// Serial.println(F("Failed to read from DHT sensor!"));
return;
}
// Serial.println(temp); // in ra mh nhiet do
// Serial.println(humi); // in ra mh do am
/// temp = 2 hum =1; angle_x=0;angle_y=3 angle_z=4;
giatri[1] = humi;
giatri[2] = temp;
}
static unsigned long time_2 = millis();
if (millis() - time_2 > 150) {
radio.write(giatri, sizeof(giatri));
// for(int i = 0 ;i<6;i++)
// {
// Serial.print(giatri[i]);
// Serial.print(".");
// }
// Serial.println();
time_2 = millis();
}
}