diff --git a/code/cat_on_diet_feeder/cat_on_diet_feeder.ino b/code/cat_on_diet_feeder/cat_on_diet_feeder.ino
index 6eca6e4..1f33e3f 100644
--- a/code/cat_on_diet_feeder/cat_on_diet_feeder.ino
+++ b/code/cat_on_diet_feeder/cat_on_diet_feeder.ino
@@ -34,7 +34,7 @@ long door_timer = 0;    // timer to close the door
 long servo_timer = 0;   //timer to stop powering servomotor
 long red_led_timer = 0;
 
-byte tag1[14] = {2,52,54,48,48,57,49,48,57,56,48,53,69,3};
+//byte tag1[14] = {2,52,54,48,48,57,49,48,57,56,48,53,69,3};
 byte newtag[14] = EMPTY_TAG; // used for read comparisons
 const byte emptytag[14] = EMPTY_TAG;
 const byte unwrittentag[14] = UNWRITTEN_TAG;
@@ -54,7 +54,7 @@ byte * readeepromtag(short tagnb=0) {
   for (int i=0; i<14; i++)
   {
     tag[i]=EEPROM.read(i + tagnb*14);
-    delay(2);   //small delay to avoid misreading
+    //delay(2);   //small delay to avoid misreading
   }
   return tag;
 }
@@ -87,58 +87,6 @@ int findtag(byte searchtag[14]) {
   return -1;
 }
 
-void setup() {
-// start serial to PC 
-  Serial.begin(115200);
- 
-  // start serial to RFID reader
-  RFID.begin(9600);
-  // empty the data cache
-  while (RFID.available()) {
-    RFID.read();
-  }
-    
-  //attaches servo
-  door_servo.attach(SERVO_CTRL);
-  
-  // for status LEDs
-  pinMode(GREEN_LED, OUTPUT);
-  pinMode(RED_LED, OUTPUT);
-
-  //Mode for button and end-stop
-  pinMode(PROG_BUTTON, INPUT_PULLUP);
-
-  Serial.print(F("EEPROM length : "));
-  Serial.print(EEPROM.length());
-  Serial.print(F(" bytes ("));
-  Serial.print(maxeepromtags());
-  Serial.println(F(" tags)"));
-  Serial.println(findtag(newtag));
-  Serial.println(findtag(unwrittentag));
-
-  for(byte n=0; n<maxeepromtags(); n++)
-  {
-    Serial.print(F("Tag n°"));
-    Serial.print(n);
-    Serial.print(": ");
-    byte *t;
-    t=readeepromtag(n);
-    for (int z = 0; z < 14 ; z++) 
-    {
-      Serial.print(t[z]);
-      Serial.print(",");
-    }
-    Serial.println("");
-  }
-}
-
-void checkmytags() {
-  // compares each tag against the tag just read
-  // if it is 1 or more we have a match, zero is a read but no match,
-  // -1 is no read attempt made
-  if (comparetag(newtag, tag1)) tag_ok=1;
-}
-
 void tagOK() {
   if (digitalRead(PROG_BUTTON))
   {
@@ -152,7 +100,7 @@ void tagOK() {
   }
   else
   {
-    addNewTag(newtag);
+    delTag(newtag);
   }
 }
 
@@ -165,7 +113,7 @@ void tagNoOK() {
     red_led_timer = millis() + 1000;
   }
   else {
-    delTag(newtag);
+    addNewTag(newtag);
   }
 }
 
@@ -183,14 +131,17 @@ void readRFID() {
     }
     RFID.flush(); // stops multiple reads
 
-    // do the tags match up?
-    checkmytags();
+    // do the tag exist in EEPROM
+    if (findtag(newtag) >= 0) tag_ok=1;
     if (tag_ok>0) break;
   }
 
-  // now do something based on tag type
-  if (tag_ok > 0) tagOK();
-  else tagNoOK();
+  //Check if the tag is not an error before doing anything
+  if (!comparetag(newtag, unwrittentag)) {
+    // now do something based on tag type
+    if (tag_ok > 0) tagOK();
+    else tagNoOK();
+  }
   
   // empty the data cache
   while (RFID.available()) {
@@ -280,11 +231,83 @@ void updateRedLED() {
 void addNewTag(byte tag[14]) {
   Serial.print(F("Adding new tag to EEPROM : "));
   for (int z = 0; z < 14 ; z++) Serial.print(tag[z]);
+  Serial.println("");
+
+  //Find first empty or unwritten space in EEPROM
+  int tagnb = findtag(emptytag);
+  if (tagnb < 0){
+    Serial.println(F("No empty space found."));
+    tagnb = findtag(unwrittentag);
+    if (tagnb < 0) {
+      Serial.println(F("No unwritten space found either. Can't save more tag."));
+      for (int i=0; i<4; i++) {
+        digitalWrite(RED_LED, HIGH);
+        delay(500);
+        digitalWrite(RED_LED, LOW);
+        delay(500);
+      }
+      return -1;
+    }
+  }
+
+  Serial.print(F("New tag number : "));
+  Serial.println(tagnb);
+  Serial.print(F("Writing..."));
+  for (int z = 0; z < 14 ; z++) EEPROM.write(z+tagnb, tag[z]);
+  Serial.print(F("  Checking..."));
+  byte tmptag[14];
+  for (int z = 0; z < 14 ; z++) tmptag[z]=EEPROM.read(tag[z+tagnb]);
+  if (comparetag(tmptag, tag)) Serial.println("OK");
+  else Serial.println("Failed");
+  
 }
 
 void delTag(byte tag[14]) {
   Serial.print(F("Deleting tag from EEPROM : "));
   for (int z = 0; z < 14 ; z++) Serial.print(tag[z]);
+  Serial.println("");
+}
+
+void setup() {
+// start serial to PC 
+  Serial.begin(115200);
+ 
+  // start serial to RFID reader
+  RFID.begin(9600);
+  // empty the data cache
+  while (RFID.available()) {
+    RFID.read();
+  }
+  
+  // for status LEDs
+  pinMode(GREEN_LED, OUTPUT);
+  pinMode(RED_LED, OUTPUT);
+
+  //Mode for button and end-stop
+  pinMode(PROG_BUTTON, INPUT_PULLUP);
+
+  Serial.print(F("EEPROM length : "));
+  Serial.print(EEPROM.length());
+  Serial.print(F(" bytes ("));
+  Serial.print(maxeepromtags());
+  Serial.println(F(" tags)"));
+  Serial.println(findtag(newtag));
+  Serial.println(findtag(unwrittentag));
+
+//  for(byte n=0; n<maxeepromtags(); n++)
+//  {
+//    Serial.print(F("Tag n°"));
+//    Serial.print(n);
+//    Serial.print(": ");
+//    byte *t;
+//    t=readeepromtag(n);
+//    for (int z = 0; z < 14 ; z++) 
+//    {
+//      Serial.print(t[z]);
+//      Serial.print(",");
+//    }
+//    Serial.println("");
+//  }
 }
 
 void loop() {
@@ -296,5 +319,5 @@ void loop() {
   
   updateRedLED();
   
-  delay(10);
+  //delay(10);
 }
diff --git a/plans/berceau-MG996R.stl b/plans/berceau-MG996R.stl
new file mode 100644
index 0000000..49aaf2b
Binary files /dev/null and b/plans/berceau-MG996R.stl differ
diff --git a/plans/berceau-servo-MG996R.fcstd b/plans/berceau-servo-MG996R.fcstd
new file mode 100644
index 0000000..36a389f
Binary files /dev/null and b/plans/berceau-servo-MG996R.fcstd differ