diff --git a/scservo_sdk/__pycache__/__init__.cpython-313.pyc b/scservo_sdk/__pycache__/__init__.cpython-313.pyc
new file mode 100644
index 0000000..a7c6d86
Binary files /dev/null and b/scservo_sdk/__pycache__/__init__.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/group_sync_read.cpython-313.pyc b/scservo_sdk/__pycache__/group_sync_read.cpython-313.pyc
new file mode 100644
index 0000000..00e9f9c
Binary files /dev/null and b/scservo_sdk/__pycache__/group_sync_read.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/group_sync_write.cpython-313.pyc b/scservo_sdk/__pycache__/group_sync_write.cpython-313.pyc
new file mode 100644
index 0000000..72ae621
Binary files /dev/null and b/scservo_sdk/__pycache__/group_sync_write.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/hls.cpython-313.pyc b/scservo_sdk/__pycache__/hls.cpython-313.pyc
new file mode 100644
index 0000000..e844715
Binary files /dev/null and b/scservo_sdk/__pycache__/hls.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/port_handler.cpython-313.pyc b/scservo_sdk/__pycache__/port_handler.cpython-313.pyc
new file mode 100644
index 0000000..859d928
Binary files /dev/null and b/scservo_sdk/__pycache__/port_handler.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/protocol_packet_handler.cpython-313.pyc b/scservo_sdk/__pycache__/protocol_packet_handler.cpython-313.pyc
new file mode 100644
index 0000000..e83b458
Binary files /dev/null and b/scservo_sdk/__pycache__/protocol_packet_handler.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/scscl.cpython-313.pyc b/scservo_sdk/__pycache__/scscl.cpython-313.pyc
new file mode 100644
index 0000000..3baae3a
Binary files /dev/null and b/scservo_sdk/__pycache__/scscl.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/scservo_def.cpython-313.pyc b/scservo_sdk/__pycache__/scservo_def.cpython-313.pyc
new file mode 100644
index 0000000..4adce83
Binary files /dev/null and b/scservo_sdk/__pycache__/scservo_def.cpython-313.pyc differ
diff --git a/scservo_sdk/__pycache__/sms_sts.cpython-313.pyc b/scservo_sdk/__pycache__/sms_sts.cpython-313.pyc
new file mode 100644
index 0000000..ed288c9
Binary files /dev/null and b/scservo_sdk/__pycache__/sms_sts.cpython-313.pyc differ
diff --git a/scservo_sdk/port_handler.py b/scservo_sdk/port_handler.py
index 2c59122..59e7fa5 100644
--- a/scservo_sdk/port_handler.py
+++ b/scservo_sdk/port_handler.py
@@ -28,7 +28,11 @@ def closePort(self):
         self.is_open = False
 
     def clearPort(self):
+        # Clear both input AND output buffers
+        self.ser.reset_output_buffer()
+        self.ser.reset_input_buffer()
         self.ser.flush()
+        time.sleep(0.001)
 
     def setPortName(self, port_name):
         self.port_name = port_name
@@ -60,11 +64,15 @@ def readPort(self, length):
             return [ord(ch) for ch in self.ser.read(length)]
 
     def writePort(self, packet):
-        return self.ser.write(packet)
+        written_bytes = self.ser.write(packet)
+        self.ser.flush()                  # Wait for TX to finish
+        self.ser.reset_input_buffer()      # Wipe the "echo" of your own message
+        return written_bytes
+#        return self.ser.write(packet)
 
     def setPacketTimeout(self, packet_length):
         self.packet_start_time = self.getCurrentTime()
-        self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER
+        self.packet_timeout = (self.tx_time_per_byte * (packet_length + 3.0)) + LATENCY_TIMER + 20.0
 
     def setPacketTimeoutMillis(self, msec):
         self.packet_start_time = self.getCurrentTime()
@@ -102,7 +110,10 @@ def setupPort(self, cflag_baud):
 
         self.is_open = True
 
+        # Wait for port to stabilize before use
+        time.sleep(0.1)
         self.ser.reset_input_buffer()
+        self.ser.reset_output_buffer()
 
         self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
 
diff --git a/sms_sts/ping.py b/sms_sts/ping.py
index a147ba9..adc257c 100644
--- a/sms_sts/ping.py
+++ b/sms_sts/ping.py
@@ -10,6 +10,20 @@
 import sys
 import os
 
+
+import subprocess
+
+# ... after portHandler.openPort() ...
+
+# Use the setserial command to force low latency mode
+try:
+    subprocess.run(['sudo', 'setserial', '/dev/serial0', 'low_latency'], check=True)
+    print("Low latency mode enabled")
+except:
+    print("Could not set low latency (ensure 'setserial' is installed)")
+
+
+
 sys.path.append("..")
 from scservo_sdk import *                   # Uses FTServo SDK library
 
@@ -17,7 +31,7 @@
 # Initialize PortHandler instance
 # Set the port path
 # Get methods and members of PortHandlerLinux or PortHandlerWindows
-portHandler = PortHandler('/dev/ttyUSB0') #ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
+portHandler = PortHandler('/dev/ttyAMA0') #ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
 
 # Initialize PacketHandler instance
 # Get methods and members of Protocol
diff --git a/sms_sts/read.py b/sms_sts/read.py
index 551f0f3..8b20266 100644
--- a/sms_sts/read.py
+++ b/sms_sts/read.py
@@ -18,7 +18,7 @@
 # Initialize PortHandler instance
 # Set the port path
 # Get methods and members of PortHandlerLinux or PortHandlerWindows
-portHandler = PortHandler('/dev/ttyUSB0')# ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
+portHandler = PortHandler('/dev/ttyAMA0')# ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
 
 # Initialize PacketHandler instance
 # Get methods and members of Protocol
diff --git a/sms_sts/test_servo.py b/sms_sts/test_servo.py
new file mode 100644
index 0000000..c05fc8c
--- /dev/null
+++ b/sms_sts/test_servo.py
@@ -0,0 +1,65 @@
+import serial
+import time
+import subprocess
+
+# 1. Force Low Latency at the OS level for the Pi 5's ttyAMA10
+try:
+    # On Pi 5, serial0 usually points to ttyAMA10
+    subprocess.run(['sudo', 'setserial', '/dev/serial0', 'low_latency'], check=True)
+    print("SUCCESS: Low latency mode enabled")
+except Exception as e:
+    print(f"WARNING: Could not set low latency (check if setserial is installed): {e}")
+
+# 2. Configure the Serial Port
+try:
+    ser = serial.Serial(
+        port='/dev/serial0',
+        baudrate=1000000,
+        timeout=0.1, 
+        write_timeout=0.5
+    )
+except Exception as e:
+    print(f"ERROR: Could not open port: {e}")
+    exit()
+
+def send_ping(servo_id):
+    # STS/SCS Ping Packet: FF FF [ID] [Len] [Instr] [Checksum]
+    # Checksum = ~(ID + Length + Instruction) & 0xFF
+    checksum = (~(servo_id + 0x02 + 0x01)) & 0xFF
+    packet = bytearray([0xFF, 0xFF, servo_id, 0x02, 0x01, checksum])
+    
+    print(f"\n--- Testing ID: {servo_id} ---")
+    print(f"Sending: {packet.hex().upper()}")
+    
+    ser.reset_input_buffer() # Clear anything sitting in the buffer
+    ser.write(packet)
+    ser.flush()              # Ensure data physically leaves the TX pin
+    
+    # Tiny pause for the automatic directional circuit on your board
+    time.sleep(0.05) 
+    
+    if ser.in_waiting > 0:
+        response = ser.read(ser.in_waiting)
+        print(f"RECEIVED: {response.hex().upper()}")
+        if len(response) >= 6:
+            # A valid response usually starts with FF FF
+            print("RESULT: Servo responded! Hardware communication is working.")
+        else:
+            print("RESULT: Received data, but it's too short (potentially an echo).")
+    else:
+        print("RESULT: Total silence. No response.")
+
+try:
+    if ser.is_open:
+        print("Serial port opened at 1,000,000 baud")
+        
+        # Test ID 1 (Standard default)
+        for servo_id in range(1, 6):
+          send_ping(servo_id)
+
+        # Test ID 254 (Broadcast - every servo should answer)
+        send_ping(254)
+
+finally:
+    ser.close()
+    print("\nPort closed.")
\ No newline at end of file