step: Controller

include/main.hpp

@@ -33,4 +33,4 @@ class ServoGrp {
     Servo _srv;
     int _pin;
     int _pos{};
-};
+};

include/rfidReader.hpp

@@ -0,0 +1,115 @@
+#pragma once
+#include <Arduino.h>
+
+class RfidReader {
+    static constexpr int BUFFER_SIZE = 14;  // RFID DATA FRAME FORMAT: 1byte head (value: 2), 10byte data (2byte version + 8byte tag), 2byte checksum, 1byte tail (value: 3)
+
+   public:
+    RfidReader(UART& serial)
+        : _serial(serial) {
+    }
+
+    void begin() {
+        _serial.begin(9600);
+    }
+
+    void flush() {
+        while (_serial.available() > 0) {
+            _serial.read();
+        }
+    }
+
+    uint32_t tick() {
+        if (_serial.available() > 0) {
+            bool call_extract_tag = false;
+            int val = _serial.read();  // read
+            if (val == -1) {           // no data was read
+                return 0;
+            }
+            if (val == 2) {  // RDM630/RDM6300 found a tag => tag incoming
+                _index = 0;
+            } else if (val == 3) {        // tag has been fully transmitted
+                call_extract_tag = true;  // extract tag at the end of the function call
+            }
+            if (_index >= BUFFER_SIZE) {  // checking for a _buffer overflow (It's very unlikely that an _buffer overflow comes up!)
+                Serial.println("Error: Buffer overflow detected!");
+                return 0;
+            }
+
+            _buffer[_index++] = val;
+            if (call_extract_tag == true) {
+                if (_index == BUFFER_SIZE) {
+                    auto tag = extractTag();
+                    return tag;
+                } else {
+                    _index = 0;
+                    return 0;
+                }
+            }
+        }
+        return 0;
+    }
+
+   private:
+    static constexpr int DATA_SIZE = 10;         // 10byte data (2byte version + 8byte tag)
+    static constexpr int DATA_VERSION_SIZE = 2;  // 2byte version (actual meaning of these two bytes may vary)
+    static constexpr int DATA_TAG_SIZE = 8;      // 8byte tag
+    static constexpr int CHECKSUM_SIZE = 2;      // 2byte checksum
+
+    UART& _serial;
+    int _index{};
+
+    char _buffer[BUFFER_SIZE];
+
+    void printVal(const char* label, const char* buff, int size) {
+        Serial.println(label);
+        for (int i = 0; i < size; ++i) {
+            Serial.print(buff);
+        }
+    }
+
+    uint32_t extractTag() {
+        char msg_head = _buffer[0];
+        char* msg_data = _buffer + 1;  // 10 byte => data contains 2byte version + 8byte tag
+        char* msg_data_version = msg_data;
+        char* msg_data_tag = msg_data + 2;
+        char* msg_checksum = _buffer + 11;  // 2 byte
+        char msg_tail = _buffer[13];
+
+        Serial.println("### Message RawData");
+        Serial.println("    HHVVVV_______ID_______CKCKTT");
+        char buff[256];
+        size_t p = sprintf(&buff[0], "    ");
+        for (size_t i = 0; i < BUFFER_SIZE; i++) {
+            p = p + sprintf(&buff[p], "%02x", _buffer[i]);
+        }
+        Serial.println(buff);
+
+        uint32_t tag = hexstr_to_value(msg_data_tag, DATA_TAG_SIZE);
+        Serial.print("Extracted Tag: ");
+        Serial.println(tag);
+        uint32_t checksum = 0;
+        for (int i = 0; i < DATA_SIZE; i += CHECKSUM_SIZE) {
+            uint32_t val = hexstr_to_value(msg_data + i, CHECKSUM_SIZE);
+            checksum ^= val;
+        }
+        Serial.print("Extracted Checksum (HEX): ");
+        Serial.print(checksum, HEX);
+        if (checksum == hexstr_to_value(msg_checksum, CHECKSUM_SIZE)) {
+            Serial.print(" (OK)");
+        } else {
+            Serial.print(" (NOT OK)");
+        }
+        Serial.println("");
+        Serial.println("--------");
+        return tag;
+    }
+
+    uint32_t hexstr_to_value(char* str, unsigned int length) {
+        char temp[16];
+        memcpy(temp, str, length);
+        temp[DATA_TAG_SIZE] = '\0';
+        uint32_t val = strtol(temp, NULL, 16);
+        return val;
+    }
+};

platformio.ini

@@ -18,7 +18,6 @@ lib_extra_dirs = core/lib
 ; build_flags = -std=c++20
 
 [env:Lair]
-# Board: https://docs.platformio.org/en/latest/boards/atmelsam/seeed_xiao.html
 platform = renesas-ra
 board = nano_r4
 framework = arduino
@@ -32,6 +31,20 @@ platform_packages =
 lib_deps =
   arduino-libraries/Servo @ ^1.3.0
 
+[env:Controller]
+platform = renesas-ra
+board = nano_r4
+framework = arduino
+build_unflags = -std=gnu++11
+build_flags = -std=c++20
+build_src_filter = 
+	-<*>
+	+<mainControl.cpp>
+platform_packages =
+  toolchain-gccarmnoneeabi@~1.120301.0
+lib_deps =
+  arduino-libraries/Servo @ ^1.3.0
+
 ; mainControl.cpp
 
 # Native environment (Host PC)

src/mainControl.cpp

@@ -1,472 +1,182 @@
 #include "main.hpp"
+#include "rfidReader.hpp"
 
 using namespace abh;
 
-Input extResetGame(0);
+constexpr TimeStamp MotorsafetyTimeDelay = 5000;
+constexpr int ServoStartPos = 0;
+constexpr int ServoEndPos = 200;
 
-Output Magnet1(1);
+Output SLedPin(LED_BUILTIN);
-Output Magnet2(3);
-Servo Servo1;
-Servo Servo2;
 
-Input endSwitch1(2);
+Output DoorLock(14);
-Input endSwitch2(3);
+// Input endSwitchOpen(19, abh::Port::LogicType::Inverted);   // N.C. Switch
+// Input endSwitchClose(21, abh::Port::LogicType::Inverted);  // N.C. Switch
+Input endSwitchOpen(19);
+Input endSwitchClose(21);
 
-#define Rfid1
+Output openDoorCmd(9);
+Input doorOpendRes(8);
+ServoGrp DoorServo(3);
+Input resetSwitch(7);
 
-Output SLedPin(LED_BUILTIN, abh::Port::LogicType::Inverted);
 static StatusLed _statusLed(SLedPin);
+RfidReader reader(Serial1);
+
+std::array<int, 2> Ids{8733826, 8767021};  // ignore 1821054
+
+// std::array<int, 2> Ids {9715, 22907};
 
 void setup() {
     Serial.begin(115200);
+    reader.begin();
+
     _statusLed.set(StatusLed::Mode::DoubleFlash);
 
-    Servo1.attach(2);
+    openDoorCmd.write(false);
-    Servo2.attach(3);
-
-    Servo1.write(45);
-    Servo2.write(90);
 }
 
+enum State {
+    WaitForRfid,
+    Open,
+    WaitForRatOpend,
+    WeitForReset,
+    WaitForRatClosed,
+    WaitForCloseCmd,
+    Close,
+};
+
 State state{};
 
+void openDoor();
+void closeDoor();
+
 void loop() {
     static Tick tick(5);
     static Tick diagTick(250);
-    static Tick servoTick(2000);
-
     if (!tick) {
         return;
     }
-    static int servo{};
 
-    if (servoTick) {
+    doorOpendRes.sample();
-        servo++;
+    endSwitchOpen.sample();
-        if (servo % 2) {
+    endSwitchClose.sample();
-            Servo1.write(45);
+    resetSwitch.sample();
-            Servo2.write(0);
+
-        } else {
+    if (state != WaitForRfid) {
-            Servo1.write(90);
+        reader.flush();
-            Servo2.write(180);
-        }
     }
 
     switch (state) {
-        default:
+        case WaitForRfid: {
-        case Idle: {
+            auto tagId = reader.tick();
-            state = Start;
+            if (tagId == 0) {
+                break;
+            }
+
+            bool found{};
+            for (auto& id : Ids) {
+                if (id == tagId) {
+                    found = true;
+                }
+            }
+
+            if (!found) {
+                break;
+            }
+            char buff[256];
+            scanf(buff, "Tag-ID 0x%x accepted", tagId);
+
+            state = State::Open;
         } break;
 
-        case Start: {
+        case Open: {
-            state = Running;
+            DoorLock.write(true);
+            openDoor();
+            DoorLock.write(false);
+
+            openDoorCmd.write(true);
+            // wait for RatLairOpened
+            Serial.println("--openSlave");
+            state = State::WaitForRatOpend;
         } break;
 
-        case Running: {
+        case WaitForRatOpend: {
-            state = Idle;
+            if (doorOpendRes.get() == Input::State::Active) {
+                Serial.println("--slaveOpend");
+                state = State::WeitForReset;
+            }
+        } break;
+
+        case WeitForReset: {
+            if (resetSwitch.get() == Input::State::Active) {
+                Serial.println("--reset");
+
+                openDoorCmd.write(false);
+                state = WaitForRatClosed;
+            }
+        } break;
+
+        case WaitForRatClosed: {
+            if (doorOpendRes.get() !=  Input::State::Active) {
+                Serial.println("--slaveClosed");
+                state = State::Close;
+            }
+        } break;
+
+        case Close: {
+            closeDoor();
+            state = State::WaitForRfid;
         } break;
     }
 
     if (diagTick) {
         _statusLed.tick();
-        Serial.printf("S=%d 0=%d | s=%d \r\n", (bool)true, (bool)false, state);
+        char buff[256];
+        sprintf(buff, "S=%d | res=%d cI=%d sO=%d sC%d \r\n",
+                state, resetSwitch.get(), doorOpendRes.get(), endSwitchOpen.get(), endSwitchClose.get());
+        Serial.print(buff);
     }
 }
 
-
+void openDoor() {
-
+    Serial.println("--open");
-
+    auto motorsafetyTime = millis();
-
+    DoorServo.attach();
-
+    int pos = ServoEndPos;
-
+    while (endSwitchOpen.get() == Input::State::Passive) {
-
+        if ((millis() - motorsafetyTime) > MotorsafetyTimeDelay) {
-
+            Serial.println("Abort due to timeout");
-
+            DoorServo.detach();
-
+            return;
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-#include <Servo.h>
-
-const int BUFFER_SIZE = 14; // RFID DATA FRAME FORMAT: 1byte head (value: 2), 10byte data (2byte version + 8byte tag), 2byte checksum, 1byte tail (value: 3)
-const int DATA_SIZE = 10; // 10byte data (2byte version + 8byte tag)
-const int DATA_VERSION_SIZE = 2; // 2byte version (actual meaning of these two bytes may vary)
-const int DATA_TAG_SIZE = 8; // 8byte tag
-const int CHECKSUM_SIZE = 2; // 2byte checksum
-uint8_t buffer[BUFFER_SIZE]; // used to store an incoming data frame
-int buffer_index = 0;
-String data;  
-int datainteger;
-
-Servo tor_servo;    // Tor mit Ratte
-Servo tor2_servo;   // Großes Tor in der Ecke
-
-#define Reset 52
-#define LED 7
-#define Schloss1 46
-#define Schloss2 47
-#define Tortasteroffen 32
-#define Tortastergeschlossen 28
-
-int servo_position, success, tortasterzustand, tortasterzaehler = 0;
-int sollID = 9715;
-int sollID2 = 22907;
-int torzustand = 0; //0=zu | 1=offen
-int torFalleTasterOffenZustand, torFalleTasterZuZustand = 0;
-int buttonState;             // the current reading from the input pin
-int lastButtonState = LOW;   // the previous reading from the input pin
-unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
-unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers
-unsigned long motorsafetyTime = 0;
-unsigned long motorsafetyTimeDelay = 5000;
-
-void setup() {
-  Serial.begin(9600);
-  Serial1.begin(115200);
-  Serial3.begin(9600);
-  pinMode(Reset, INPUT);
-  pinMode(Tortasteroffen, INPUT);
-  pinMode(Tortastergeschlossen, INPUT);
-  pinMode(LED, OUTPUT);
-  pinMode(Schloss1, OUTPUT);
-  digitalWrite(Schloss1, HIGH);
-  pinMode(Schloss2, OUTPUT);
-  digitalWrite(Schloss2, HIGH);
-  digitalWrite(LED, HIGH);
-
-  //Torzustand erfragen
-  torFalleTasterOffenZustand = digitalRead(Tortasteroffen);
-  torFalleTasterZuZustand = digitalRead(Tortastergeschlossen);
-    Serial.print("Fallentortasteroffen: ");
-    Serial.println(torFalleTasterOffenZustand);
-    Serial.print("Fallentortastergeschlossen: ");
-    Serial.println(torFalleTasterZuZustand);
-    Serial.println("-------------------------------");
-  
-  if(torFalleTasterOffenZustand == 0) {
-     Serial.println("FallenTor ist offen");
-     torzustand=1;
-  } else {
-      Serial.println("FallenTor ist zu");
-      digitalWrite(LED, LOW);
-      tor_offnen();
-  }
-
-  Serial.println("INIT DONE");
-}
-
-void loop() {  
-  int reading = digitalRead(Reset);
-  if (reading != lastButtonState) {
-    lastDebounceTime = millis();
-  }
- 
-  if ((millis() - lastDebounceTime) > debounceDelay) {
-    if (reading != buttonState) {
-      buttonState = reading;
-      if (buttonState == HIGH && torzustand==1) {
-        Serial.println("Resettaster wurde gedrückt");
-        tor_schliessen();
-      }
-    }
-  }
-  lastButtonState = reading;
-  
-  if (Serial3.available() > 0 && torzustand==0) {
-      bool call_extract_tag = false;
-      int ssvalue = Serial3.read(); // read
-      Serial.println(ssvalue);
-      if (ssvalue == -1) { // no data was read
-        return;
-      }
-      if (ssvalue == 2) { // RDM630/RDM6300 found a tag => tag incoming
-        buffer_index = 0;
-      } else if (ssvalue == 3) { // tag has been fully transmitted
-        call_extract_tag = true; // extract tag at the end of the function call
-      }
-      if (buffer_index >= BUFFER_SIZE) { // checking for a buffer overflow (It's very unlikely that an buffer overflow comes up!)
-        Serial.println("Error: Buffer overflow detected!");
-        return;
-      }
-
-      buffer[buffer_index++] = ssvalue; // everything is alright => copy current value to buffer
-      if (call_extract_tag == true && success == 0) {
-        if (buffer_index == BUFFER_SIZE) {
-          unsigned tag = extract_tag();
-          Serial.println(tag);
-          if (tag == sollID || tag == sollID2) {
-            Serial.println("Herzlichen Glückwunsch Sie haben erfolgreich eine Ratte gefangen!");
-            tor_offnen();
-          }
-        } else { // something is wrong... start again looking for preamble (value: 2)
-          buffer_index = 0;
-          return;
         }
-      }
+        DoorServo.write(--pos);
-   } else if (Serial3.available() > 0 && torzustand==1) {
+        if (pos <= 0) {
-      while (Serial3.available() > 0) {
+            break;
-       Serial3.read();
-    }
-   }
-}
-
-unsigned extract_tag() {
-  uint8_t msg_head = buffer[0];
-  uint8_t *msg_data = buffer + 1; // 10 byte => data contains 2byte version + 8byte tag
-  uint8_t *msg_data_version = msg_data;
-  uint8_t *msg_data_tag = msg_data + 2;
-  uint8_t *msg_checksum = buffer + 11; // 2 byte
-  uint8_t msg_tail = buffer[13];
-  // print message that was sent from RDM630/RDM6300
-  Serial.println("--------");
-  Serial.print("Message-Head: ");
-  Serial.println(msg_head);
-  Serial.println("Message-Data (HEX): ");
-  for (int i = 0; i < DATA_VERSION_SIZE; ++i) {
-    Serial.print(char(msg_data_version[i]));
-  }
-  Serial.println(" (version)");
-  for (int i = 0; i < DATA_TAG_SIZE; ++i) {
-    Serial.print(char(msg_data_tag[i]));
-  }
-  Serial.println(" (tag)");
-  Serial.print("Message-Checksum (HEX): ");
-  for (int i = 0; i < CHECKSUM_SIZE; ++i) {
-    Serial.print(char(msg_checksum[i]));
-  }
-  Serial.println("");
-  Serial.print("Message-Tail: ");
-  Serial.println(msg_tail);
-  Serial.println("--");
-  long tag = hexstr_to_value(msg_data_tag, DATA_TAG_SIZE);
-  Serial.print("Extracted Tag: ");
-  Serial.println(tag);
-  long checksum = 0;
-  for (int i = 0; i < DATA_SIZE; i += CHECKSUM_SIZE) {
-    long val = hexstr_to_value(msg_data + i, CHECKSUM_SIZE);
-    checksum ^= val;
-  }
-  Serial.print("Extracted Checksum (HEX): ");
-  Serial.print(checksum, HEX);
-  if (checksum == hexstr_to_value(msg_checksum, CHECKSUM_SIZE)) { // compare calculated checksum to retrieved checksum
-    Serial.print(" (OK)"); // calculated checksum corresponds to transmitted checksum!
-  } else {
-    Serial.print(" (NOT OK)"); // checksums do not match
-  }
-  Serial.println("");
-  Serial.println("--------");
-  return tag;
-}
-long hexstr_to_value(char *str, unsigned int length) { // converts a hexadecimal value (encoded as ASCII string) to a numeric value
-  char* copy = malloc((sizeof(char) * length) + 1);
-  memcpy(copy, str, sizeof(char) * length);
-  copy[length] = '\0';
-  // the variable "copy" is a copy of the parameter "str". "copy" has an additional '\0' element to make sure that "str" is null-terminated.
-  long value = strtol(copy, NULL, 16);  // strtol converts a null-terminated string to a long value
-  free(copy); // clean up
-  return value;
-}
-//Hakentor ist bei 76 Grad zu | Rattentor bei 85 Grad
-void tor_offnen() {
-  if(torzustand==0) {
-    Serial.println("öffnen");
-    digitalWrite(LED, HIGH);
-    //tor_servo.write(0);
-
-    motorsafetyTime = millis();
-    digitalWrite(Schloss1, LOW);
-
-  
-    int servo_position = 200;
-    //FallenTor
-    tor2_servo.attach (9);
-      while (digitalRead(Tortasteroffen) == 1){
-        if ((millis() - motorsafetyTime) > motorsafetyTimeDelay) {
-          //kill Tor öffnen es dauert zu lang
-          digitalWrite(Schloss1, HIGH);
-          Serial.println("Es hat zu lange gedauert. Etwas stimmt nicht. Das Tor öffnen wurde beendet #1");
-          break;
         }
-        servo_position--;
-        tor2_servo.write(servo_position);
         delay(10);
-      }
+        endSwitchOpen.sample();
-      int test = servo_position+10;
-      tor2_servo.write(test);
-      delay(20);
-      tor2_servo.detach();
-      Serial.println("FallenTor offen");
-
-    digitalWrite(Schloss1, HIGH);
-    digitalWrite(Schloss2, LOW);
-
-     //RattenTor
-     tor_servo.attach (4);
-   for(servo_position = 83; servo_position >=0; servo_position -=1){
-      if ((millis() - motorsafetyTime) > motorsafetyTimeDelay) {
-        //kill Tor öffnen es dauert zu lang
-        digitalWrite(Schloss2, HIGH);
-        Serial.println("Es hat zu lange gedauert. Etwas stimmt nicht. Das Tor öffnen wurde beendet #2");
-        break;
-      }
-      tor_servo.write(servo_position);
-      delay(10);
     }
-    tor_servo.detach();
+    DoorServo.write(pos + 10);
-    Serial.println("RattennTor offen");
+    delay(20);
-
+    DoorServo.detach();
-    
+    Serial.println("--isOpen");
-    digitalWrite(Schloss2, HIGH);
-    Serial.println("Alle Elektroschlösser entspannt");
-
-    //tor2_servo.write(0);
-    if ((millis() - motorsafetyTime) > motorsafetyTimeDelay) {
-        //kill Tor öffnen es dauert zu lang
-        Serial.println("Es hat zu lange gedauert. Etwas stimmt nicht. Das Tor öffnen wurde beendet #3");
-      } else {
-         torzustand = 1;
-          Serial.println("1Torzustand = 1 - Tore wurden geöffnet");
-      }
-  }
-  while (Serial3.available() > 0) {
-     Serial3.read();
-  }
-}
-void tor_schliessen() {
-  if(torzustand==1) {
-    Serial.println("schliessen");
-    digitalWrite(LED, LOW);
-    //RattenTor
-    tor_servo.attach (4);
-      for(servo_position = 0; servo_position <=83; servo_position +=1){
-        tor_servo.write(servo_position);
-        delay(10);
-      }
-      tor_servo.detach();
-      int servo_position = 0;
-      //FallenTor
-      tor2_servo.attach (9);
-      while (digitalRead(Tortastergeschlossen) == 1){
-        servo_position++;
-        tor2_servo.write(servo_position);
-        delay(10);
-      }
-      
-      int test = servo_position-3;
-      tor2_servo.write(test);
-      delay(20);
-      tor2_servo.detach();
-
-    torzustand = 0;
-    Serial.println("2Torzustand = 0 - Tore wurden geschlossen");
-  }
-  while (Serial3.available() > 0) {
-     Serial3.read();
-  }
 }
 
-
-
-
-
-
-
-
-
-
-
-// Hakentor ist bei 76 Grad zu | Rattentor bei 85 Grad
-void openDor() {
-    if (!DoorIsOpen) {
-        Serial.println("--open");
-
-        auto motorsafetyTime = millis();
-        DoorLock.write(false); // digitalWrite(Schloss1, LOW);
-
-        int servo_position = 200;
-        // FallenTor
-        while (digitalRead(Tortasteroffen) == 1) {
-            if ((millis() - motorsafetyTime) > motorsafetyTimeDelay) {
-                // kill Tor öffnen es dauert zu lang
-                digitalWrite(Schloss1, HIGH);
-                Serial.println("Es hat zu lange gedauert. Etwas stimmt nicht. Das Tor öffnen wurde beendet #1");
-                break;
-            }
-            servo_position--;
-            tor2_servo.write(servo_position);
-            delay(10);
-        }
-        int test = servo_position + 10;
-        tor2_servo.write(test);
-        delay(20);
-        tor2_servo.detach();
-        Serial.println("FallenTor offen");
-
-        digitalWrite(Schloss1, HIGH);
-        digitalWrite(Schloss2, LOW);
-
-        // RattenTor
-        for (servo_position = 83; servo_position >= 0; servo_position -= 1) {
-            if ((millis() - motorsafetyTime) > motorsafetyTimeDelay) {
-                // kill Tor öffnen es dauert zu lang
-                digitalWrite(Schloss2, HIGH);
-                Serial.println("Es hat zu lange gedauert. Etwas stimmt nicht. Das Tor öffnen wurde beendet #2");
-                break;
-            }
-            ratDoor.write(servo_position);
-            delay(10);
-        }
-        ratDoor.detach();
-        Serial.println("RattennTor offen");
-
-        digitalWrite(Schloss2, HIGH);
-        Serial.println("Alle Elektroschlösser entspannt");
-
-        // tor2_servo.write(0);
-        if ((millis() - motorsafetyTime) > motorsafetyTimeDelay) {
-            // kill Tor öffnen es dauert zu lang
-            Serial.println("Es hat zu lange gedauert. Etwas stimmt nicht. Das Tor öffnen wurde beendet #3");
-        } else {
-            DoorIsOpen = true;
-            Serial.println("1DoorIsOpen = 1 - Tore wurden geöffnet");
-        }
-    }
-}
 void closeDoor() {
-    if (DoorIsOpen) {
+    Serial.println("--close");
-        Serial.println("--close");
+    DoorServo.attach();
-        // RattenTor
+    int pos = ServoStartPos;
-        ratDoor.attach(4);
+    while (endSwitchClose.get() == Input::State::Passive) {  // N.C. - Switch
-        for (servo_position = 0; servo_position <= 83; servo_position += 1) {
+        DoorServo.write(++pos);
-            ratDoor.write(servo_position);
+        delay(10);
-            delay(10);
+        endSwitchClose.sample();
+        if (pos > ServoEndPos) {
+            break;
         }
-        ratDoor.detach();
-        int servo_position = 0;
-        // FallenTor
-        tor2_servo.attach(9);
-        while (digitalRead(DoorClodedPin) == 1) {
-            servo_position++;
-            tor2_servo.write(servo_position);
-            delay(10);
-        }
-
-        int test = servo_position - 3;
-        tor2_servo.write(test);
-        delay(20);
-        tor2_servo.detach();
-
-        DoorIsOpen = false;
-        Serial.println("  -- close door");
     }
+    DoorServo.write(pos - 3);
+    delay(20);
+    DoorServo.detach();
+    Serial.println("--isClosed");
 }