[CLEAN] Arduino + UHF RFID KLM900S + Ultrasonic + Servo - [NO DELAY]

/*
  Connections:
  ------------------------------
  Arduino Uno <---> RF-KLM900S
******************************
    3.3V    <--->     EN
    5V      <--->     5V
    GND     <--->     GND
    3       <--->     TTL_R
    2       <--->     TTL_T
  ------------------------------
*/

// Libraries
#include <SoftwareSerial.h>
#include <Servo.h>

// Pin Definitions
#define trigPin   8
#define echoPin   9
#define rxPin     2
#define txPin     3
#define servoPin  11

// Objects and Communication Chanel
Servo servo1;
SoftwareSerial SSerial(rxPin, txPin); // RX, TX

// Command to Read RFID Tags Using RF-KLM900S TTL Connector
// Single Read  BB 00 22 00 00 22 7E
byte A[] = {0xBB, 0x00, 0x22, 0x00, 0x00, 0x22, 0x7E};

// Saved RFID Tags in one String Array
// [1] Card  Tag   UID : 30 08 33 B2 DD D9 01 40 00 00 00 00
// [2] Smart Tag 1 UID : E2 80 68 94 00 00 50 20 91 08 58 7E
// [3] Smart Tag 2 UID : E2 80 68 94 00 00 50 20 91 08 58 88
String TagNo[] = {"30 08 33 B2 DD D9 01 40 00 00 00 00",
                  "E2 80 68 94 00 00 50 20 91 08 58 7E",
                  "E2 80 68 94 00 00 50 20 91 08 58 88"
                 };

// Variables and Flags
String TagID_RX = "";
static String inputString;
static unsigned long currentTime = 0;
static unsigned long lastTime = 0;
static unsigned long lastTime2 = 0;
static unsigned long lastCharArrivalTime = 0;
long distance;
long duration;
int flag = 0;     // Tag was Identifyed before  [Flag]
int flag3min = 0; // Show Timer Started Message [Flag]
int count = 0;
int numOfTags = 3;

void setup()
{
  // Communication Chanel Between Arduino and RF-KLM900S RFID Reader
  SSerial.begin(115200);

  // Communication Chanel Between Arduino and PC/Laptop/Android Phone
  Serial.begin(115200);

  // Assign Inputs and Outputs
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  servo1.attach(servoPin);

  // Close Servo Gate
  servo1.write(0);


  delay(2000);
}

void loop() {

  // Messure Distance from Ultrasonic Sensor to Object
  ultra();

  lastCharArrivalTime = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  //------------------------[Send Read Command To RFID Reader]----------------------------------
  //////////////////////////////////////////////////////////////////////////////////////////////
  if (millis() - lastTime > 50 && inputString == "") {
    SSerial.write(A, sizeof(A));
    lastTime = millis();
  }
  //////////////////////////////////////////////////////////////////////////////////////////////
  //------------------------[Read RFID DATA and Convert to HEX]---------------------------------
  //////////////////////////////////////////////////////////////////////////////////////////////
  if (SSerial.available())
  {
    byte incomingByte = SSerial.read();
    inputString += "0123456789ABCDEF"[incomingByte / 16];
    inputString += "0123456789ABCDEF"[incomingByte % 16];
    inputString += ' ';
    lastCharArrivalTime = millis();
  }
  //////////////////////////////////////////////////////////////////////////////////////////////
  //--------------------------[When Read is Done Analyze Data]----------------------------------
  //////////////////////////////////////////////////////////////////////////////////////////////
  if (lastCharArrivalTime != 0 && millis() - lastCharArrivalTime >= 10) {

    // Extract Tag UID from Received RFID Data Stream
    TagID_RX = inputString.substring(24, 59);

    // Loop To Compare All Saved Tag UIDs to the Received tag UID
    for (int i = 0; i < numOfTags; i++) {

      // Compare Saved Tag UID to the Received tag UID (AND) Distance must be less or equal to 200cm
      if (!TagID_RX.compareTo(TagNo[i]) && distance <= 200) {
        Serial.print("Tag No [");
        Serial.print(i + 1);
        Serial.println("] Found.");
        Serial.println("------------------");

        // Open Servo Gate
        servo1.write(90);

        delay(1000);

        // Activate Tag was Identifyed before [Flag]
        flag = 1;

        // Rest Flags and timer
        flag3min = 0;
        count = 0;
        lastTime2 = millis();
      }

      // If tag not found (OR) Distance more than 200cm
      else {
        // If Tag not Received by the reader for 2,5 Seconds (AND) Tag was Identifyed before
        if (count > 50 && flag == 1) {

          // If Tag is removed from Reader for more than 2,5 Seconds Show message ONE time
          if (flag3min == 0) {
            Serial.println("3 min Timer Started !!");

            // Activate Show Timer Started Message [Flag]
            flag3min = 1;
          }

          // If the current time after the Timer is activated is more than 3 Minutes Do Commands
          currentTime = millis();
          if (currentTime - lastTime2 > 180000) { // 180000 = 180,000 milliSecond = 180 Seconds = 3 Minutes

            // Close Servo Gate
            servo1.write(0);

            // Rest Flags and timer
            count = 0;
            flag = 0;
            flag3min = 0;
            lastTime2 = millis();
          }
        }

        // Increase the count every 50 ms
        count++;

      }
    }

    // Rest Variable and timer
    inputString = "";
    lastCharArrivalTime = 0;

  }
  //////////////////////////////////////////////////////////////////////////////////////////////
}

void ultra() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
}