diff --git a/RS485Receiver.ino b/RS485Receiver.ino
index 3954b2a..8d11c48 100644
--- a/RS485Receiver.ino
+++ b/RS485Receiver.ino
@@ -12,6 +12,15 @@ Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
 #define DE_PIN 20
 #define RE_PIN 10
 
+#define THIS_ID        200
+#define PING           0x01 // QUERY THE WORKING STATUS
+#define READ_DATA      0x02 // READ DATA
+#define WRITE_DATA     0x03 // WRITE DATA
+#define REGWRITE_DATA  0x04 // QUEUES MOVES FOR ACTION COMMAND
+#define ACTION         0x05 // TRIGGERS REG WRITE WRITES
+#define SYNCWRITE_DATA 0x83 // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
+#define RESET          0x06 // RESET TO FACTORY DEFAULT
+
 void setup() {
   // Initialize Serial1 for RS485 (TX=21, RX=9)
   Serial.begin(115200);
@@ -21,9 +30,7 @@ void setup() {
   pinMode(DE_PIN, OUTPUT);
   pinMode(RE_PIN, OUTPUT);
 
-  // Enable receive mode
-  digitalWrite(DE_PIN, LOW);
-  digitalWrite(RE_PIN, LOW);
+  SetModeReceive();
 
   // Initialize OLED
   Wire.begin(3, 4);  // SDA=3, SCL=4
@@ -66,6 +73,26 @@ void loop() {
     display.println("Received:");
     display.setCursor(0, 12);
     display.setTextSize(1);
+    if (buffer[4] == PING) {
+      if (buffer[2] == THIS_ID){
+        sendPingResponse();
+        Serial.println("ME!");
+        display.print("!");
+      }
+      Serial.println("PING");
+
+      display.print("PING");
+      display.print("  ");
+      display.println(buffer[2]);
+    } else if (buffer[4] == WRITE_DATA) {
+      Serial.println("WRITE_DATA");
+      uint16_t position = (buffer[7] << 8) | buffer[6];
+      display.print("WRITE_DATA");
+      display.print("  ");
+      display.print(buffer[2]);
+      display.print("  ");
+      display.println(position);
+    }
 
     for (int i = 0; i < count; i++) {
       display.print("0x");
@@ -73,6 +100,55 @@ void loop() {
       display.print(buffer[i], HEX);
       display.print(" ");
     }
+    
+
+    
     display.display();
   }
 }
+
+
+void SetModeTransmit() {
+  digitalWrite(DE_PIN, HIGH); 
+  digitalWrite(RE_PIN, HIGH);
+  delay(10);
+}
+
+void SetModeReceive() {
+  digitalWrite(DE_PIN, LOW);   
+  digitalWrite(RE_PIN, LOW);
+  delay(10);
+}
+
+// Send a ping command to a specific servo
+void sendPingResponse() {
+    uint8_t packet[6];
+
+    packet[0] = 0xFF; // Header
+    packet[1] = 0xFF; // Header
+    packet[2] = THIS_ID;   // Servo ID
+    packet[3] = 0x03; // Length = instruction + address + checksum
+    packet[4] = 0x00; // NO ERROR
+
+    // Calculate checksum
+    uint8_t sum = 0;
+    for (int i = 2; i < 5; i++) sum += packet[i];
+    packet[5] = ~sum; // Checksum
+
+    // Send packet
+    SetModeTransmit();
+    Serial1.write(packet, sizeof(packet));
+    Serial1.flush();
+    SetModeReceive();
+
+    // if (PRINT_SENT_PACKETS) {
+    //     Serial.print("Sent ping packet: ");
+    //     for (int i = 0; i < 6; i++) {
+    //         Serial.print("0x");
+    //         if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex
+    //         Serial.print(packet[i], HEX);
+    //         Serial.print(" ");
+    //     }
+    //     Serial.println();
+    // }
+}