MEGA_Sketch

#include <NewPing.h>
#include <MFRC522.h>
#include <EngineControl.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include <avr/wdt.h>            // library for default watchdog functions
#include <avr/interrupt.h>      // library for interrupts handling
#include <avr/sleep.h>          // library for sleep
#include <avr/power.h>

MPU6050 mpu;
int MPUOffsets[6] = {  -363,  -480,   4803,    171,    19,     -21};
#define LED_PIN 13
volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;
}

#define interruptPin 3
int giro = 0;
int FifoAlive = 0;
int IsAlive = -20;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[128];
Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;
float euler[3];
float ypr[3];
float Yaw, Pitch, Roll;
void MPU6050Connect() {
  static int MPUInitCntr = 0;
  mpu.initialize(); // same
  devStatus = mpu.dmpInitialize();// same
  if (devStatus != 0) {
    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
    MPUInitCntr++;
    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[devStatus]);
    Serial.println(F(")"));
    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }
  mpu.setXAccelOffset(MPUOffsets[0]);
  mpu.setYAccelOffset(MPUOffsets[1]);
  mpu.setZAccelOffset(MPUOffsets[2]);
  mpu.setXGyroOffset(MPUOffsets[3]);
  mpu.setYGyroOffset(MPUOffsets[4]);
  mpu.setZGyroOffset(MPUOffsets[5]);
  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);
  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
  attachInterrupt(digitalPinToInterrupt(2), dmpDataReady, FALLING); //pin 2 on the Uno
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize();
  delay(1000); // Let it Stabalize
  mpu.resetFIFO(); // Clear fifo buffer
  mpu.getIntStatus();
  mpuInterrupt = false; // wait for next interrupt
}
void i2cSetup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
}



const EnginePins FRONT_LEFT_ENGINE_PINS = {7, 30, 31};
const EnginePins FRONT_RIGHT_ENGINE_PINS = {6, 32, 33};
const SensorPins SENSOR_PINS = {A15, A14, A13, A12, A11};

MFRC522 mfrc522(53, 5);
EngineControl engineControl(255, 200);
NewPing sonar(22, 23);

String message = "";
String initialTag = "";
int rfidIndex = 0;
boolean leavedBase = false;
const int BUZZER = 4;
int timer = millis();

void setup() {
  Serial.begin(115200);
  Serial1.begin(9600);
  while (!Serial);
  i2cSetup();
  Serial.println(F("Alive"));
  MPU6050Connect();
  pinMode(interruptPin, INPUT_PULLUP);
  Serial.print("Nivel interrupcao: ");Serial.println(digitalRead(interruptPin));
  pinMode(BUZZER, OUTPUT);
  digitalWrite(BUZZER, HIGH);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  while(timer <=5000) {
    Serial1.read();
    timer = millis();
  }
  Going_To_Sleep();


  SPI.begin();
  mfrc522.PCD_Init();
  mfrc522.PCD_AntennaOn();
  mfrc522.PCD_SetAntennaGain(0x07 << 4);
  engineControl.setFrontLeftPins(FRONT_LEFT_ENGINE_PINS);
  engineControl.setFrontRightPins(FRONT_RIGHT_ENGINE_PINS);
  engineControl.setSensorPins(SENSOR_PINS);
  Serial.println("Setup completo");
}

void loopEngine() {
  float distance = sonar.ping_cm();
  if (distance < 15 && distance > 0) {
    engineControl.parar();
  } else {
    engineControl.loop();
  }
}

void Going_To_Sleep() {
  Serial.println("Dormindo");
  digitalWrite(LED_PIN, LOW);
  sleep_enable();
  attachInterrupt(digitalPinToInterrupt(interruptPin), wakeUp, LOW);
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);
  delay(1000);
  sleep_cpu();
  Serial.println("Acordei");
  Serial.print("Nivel interrupcao: "); Serial.println(digitalRead(interruptPin));
  digitalWrite(LED_PIN, HIGH);
}

void wakeUp() {
  Serial.println("Interrupção feita");
  sleep_disable();
  detachInterrupt(digitalPinToInterrupt(interruptPin));
}

void loop() {

  digitalWrite(BUZZER, HIGH);
  String currentDirection;

  if (message == "") {
    if (Serial1.available()) {
      message = Serial1.readString();
      Serial.println(message);
      digitalWrite(BUZZER, LOW);
      delay(200);
      digitalWrite(BUZZER, HIGH);
    }
    return;
  }

  if (!mfrc522.PICC_IsNewCardPresent() || !mfrc522.PICC_ReadCardSerial()) {
    if (leavedBase == true) {
      loopEngine();
    }
    return;
  }

  Serial.println("rfid encontrado");

  String rfidUid = "";
  for (byte i = 0; i < mfrc522.uid.size; i++) {
    rfidUid += String(mfrc522.uid.uidByte[i] < 0x10 ? "0" : "");
    rfidUid += String(mfrc522.uid.uidByte[i], HEX);
  }

  rfidUid.toLowerCase();
  rfidUid.trim();
  Serial.println(rfidUid);

  if (initialTag == rfidUid){
    return;
  }

  initialTag = rfidUid;

  int incomingRfidIndex = message.indexOf(rfidUid);

  if (incomingRfidIndex == rfidIndex) {
    Serial1.print(rfidUid);
    currentDirection = message.substring(rfidIndex + 9, rfidIndex + 10);
    rfidIndex += 11;
  }

  Serial.print("nova direção: ");

  Serial.println(currentDirection);


  static long QTimer = millis();
  if ((long)( millis() - QTimer ) >= 100) {
    QTimer = millis();
    Serial.print(F("\t Yaw")); Serial.print(Yaw);
    Serial.println();
  }

  Serial.println(message);
  if (currentDirection == "b") {
    engineControl.impulso();
    loopEngine();
    leavedBase = true;
  } else if (currentDirection == "d") {
    engineControl.parar();
    Serial.println("Parou");
    Serial.println(Yaw);
    delay(1000);
    while (Yaw >= giro-90) {
      int count = 1;
      Serial.println("Entrou while");
      engineControl.direita();
      Serial.println(Yaw);
      if (count == 1) {
        Serial.println("Contou 1");
        if (mpuInterrupt) {
        GetDMP();
        }
      }
      static long QTimer = millis();
      if ((long)( millis() - QTimer ) >= 10) {
        QTimer = millis();
        Serial.print(F("\t Yaw")); Serial.print(Yaw);
        Serial.println();
      }
      count++;
      Serial.print("Contou: ");
      Serial.println(count);
    }
    Serial.println("Saiu while");
    giro = giro-90;

    engineControl.parar();
    delay(500);

    loopEngine();
  } else if (currentDirection == "e") {
    engineControl.parar();
    Serial.println("Parou");
    Serial.println(Yaw);
    delay(1000);
    while (Yaw <= giro+90) {
      int count = 1;
      Serial.println("Entrou while");
      engineControl.esquerda();
      Serial.println(Yaw);
      if (count == 1) {
        Serial.println("Contou 1");
        if (mpuInterrupt) {
        GetDMP();
        }
      }
      static long QTimer = millis();
      if ((long)( millis() - QTimer ) >= 10) {
        QTimer = millis();
        Serial.print(F("\t Yaw")); Serial.print(Yaw);
        Serial.println();
      }
      count++;
      Serial.print("Contou: ");
      Serial.println(count);
    }
    Serial.println("Saiu while");
    giro = giro+90;

    engineControl.parar();
    delay(500);
    loopEngine();
  } else if (currentDirection == "f") {
    loopEngine();
    digitalWrite(BUZZER, LOW);
    delay(150);
    digitalWrite(BUZZER, HIGH);
    delay(150);
    digitalWrite(BUZZER, LOW);
    delay(150);
    digitalWrite(BUZZER, HIGH);
  } else if (currentDirection == "t") {
    engineControl.tras();
  } else if (currentDirection == "p") {
    engineControl.parar();
    Serial.println("ligando buzzer");
    digitalWrite(BUZZER, LOW);
    delay(150);
    digitalWrite(BUZZER, HIGH);
    message = "";
    rfidIndex = 0;
    leavedBase = false;
    Going_To_Sleep();
  }
}

void GetDMP() {
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  uint16_t MaxPackets = 50;
  if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (fifoCount < packetSize)) {
    digitalWrite(LED_PIN, LOW);
    Serial.println(F("Reset FIFO"));
    if (fifoCount % packetSize) Serial.print(F("\t Packet corruption"));
    Serial.print(F("\tfifoCount ")); Serial.print(fifoCount);
    Serial.print(F("\tpacketSize ")); Serial.print(packetSize);
    mpuIntStatus = mpu.getIntStatus();
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpuIntStatus , BIN);
    if (mpuIntStatus & B10000) {
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpuIntStatus & B1000) {
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpuIntStatus & B1) {
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    mpu.resetFIFO();
    mpu.getIntStatus();
  } else {
    while (fifoCount  >= packetSize) {
      if (fifoCount < packetSize) break;
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;
    }
    MPUMath();
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    if (fifoCount > 0) mpu.resetFIFO();
  }
}
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180 / M_PI);
  Pitch = (ypr[1] * 180 / M_PI);
  Roll = (ypr[2] * 180 / M_PI);
}