arduino - How to stop multiple RFID reads?

Question

I am quite new to Arduino and programming, so I beg you for patience. I am using 2 RFIDs with Arduino Mega and 2 servo motors and one OLED display. So the idea is that when I put specific RFID tag at first RC522 it opens first servo motor, and closes it after 4 seconds. Then it scans second RC522 for the same tag, and when I present it, it opens second servo, writes a message on OLED display and then moves second servo back and starts scanning first RC522 again. This all works perfectly, I did it with switch case, but the problem is after the last step it keeps lifting the servos even without presenting the tag, like it remembers (reads more than once) the tag. I just want it to do this once. Is it possible?

Here is the code :

    #include <Servo.h>
    #include <SPI.h>
    #include <Wire.h>
    #include <Adafruit_GFX.h>
    #include <Adafruit_SSD1306.h>
    #include <MFRC522.h>
    #define RST_PIN         5         
    #define SS_1_PIN        10         
    #define SS_2_PIN        8       
    #define MFRC522_SPICLOCK SPI_CLOCK_DIV16

    MFRC522 mfrc5221(SS_1_PIN, RST_PIN);
    MFRC522 mfrc5222(SS_2_PIN, RST_PIN);
    Adafruit_SSD1306 display(-1);
    Servo servo1;
    Servo servo2;                         
    unsigned long PreviousMillis = 0;         
    unsigned long CurrentMillis;             
    int atraso;                               
    int state;                               
    int pos1=95;
    int pos2=90;

    void setup ()
    {
      Serial.begin(9600);
      SPI.begin();
      servo1.attach(7);
      servo2.attach(6);
      display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(WHITE);
      display.setCursor(25,10);
      display.println("Text 1");
      display.display();
      mfrc5221.PCD_Init();
      mfrc5222.PCD_Init();
      mfrc5221.PCD_DumpVersionToSerial();
      mfrc5222.PCD_DumpVersionToSerial();
      state=1;                               
    }
    void loop ()
    {
      CurrentMillis = millis();               
      atraso=CurrentMillis-PreviousMillis;   
      next_state_FSM();                     
      output_FSM();                         
      Serial.print("State = ");
      Serial.print(state);
      Serial.print("\n");
    }

    void output_FSM(){
     switch (state){
        case 1:
        {
          servo1.write(pos1);
          servo2.write(pos2);
          display.clearDisplay();
          display.setTextSize(2);
          display.setTextColor(WHITE);
          display.setCursor(25,10);
          display.println("Text 1");
          display.display();
          if ( ! mfrc5221.PICC_IsNewCardPresent())
      {
          return;
      }
          if ( ! mfrc5221.PICC_ReadCardSerial())
      {
          return;
      }
        break;
        }

        case 2:
        {
          servo1.write(0);
        }
        break;

        case 3:
        {                               
          servo1.write(95);
          if ( ! mfrc5222.PICC_IsNewCardPresent())
      {
          return;
      }
          if ( ! mfrc5222.PICC_ReadCardSerial())
      {
          return;
      }
        break;
        }

        case 4:
        {
        servo2.write(0);
        display.clearDisplay();
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0,5);
        display.println("Text 2");
        display.setCursor(50,10);
        display.println("Text 3");
        display.display();                                                 
        }
     }
    }
    void next_state_FSM(){                   
      switch (state){
        case 1:
        {
      Serial.print("UID tag :");
      String content1= "";
      byte letter1;
      for (byte i = 0; i < mfrc5221.uid.size; i++)
      {
         Serial.print(mfrc5221.uid.uidByte[i] < 0x10 ? " 0" : " ");
         Serial.print(mfrc5221.uid.uidByte[i], HEX);
         content1.concat(String(mfrc5221.uid.uidByte[i] < 0x10 ? " 0" : " "));
         content1.concat(String(mfrc5221.uid.uidByte[i], HEX));
      }
      Serial.println();
      content1.toUpperCase();
      if (content1.substring(1) == "FB 5B 65 0A")
      {
           state=2;
           PreviousMillis=CurrentMillis;
      }
      break;
        }

        case 2:
        {
           if(atraso>4000){
            state=3;
            PreviousMillis=CurrentMillis;
          }
        break;
        }

        case 3:
        {
          Serial.print("UID tag :");
      String content2= "";
      byte letter2;
      for (byte j = 0; j < mfrc5222.uid.size; j++)
      {
         Serial.print(mfrc5222.uid.uidByte[j] < 0x10 ? " 0" : " ");
         Serial.print(mfrc5222.uid.uidByte[j], HEX);
         content2.concat(String(mfrc5222.uid.uidByte[j] < 0x10 ? " 0" : " "));
         content2.concat(String(mfrc5222.uid.uidByte[j], HEX));
      }
      Serial.println();
      content2.toUpperCase();
          if (content2.substring(1) == "FB 5B 65 0A")
      {
        state=4;
        PreviousMillis=CurrentMillis;
      }
      break;
        }

        case 4:
        {
          if(atraso>4000){
          state=1;
          PreviousMillis=CurrentMillis;   
        }
        break;
        }
      }
    }

标签： arduinorfid