diff --git a/Controller.py b/Controller.py
index 8f44dad..e4af5bc 100644
--- a/Controller.py
+++ b/Controller.py
@@ -1,93 +1,200 @@
-import pygame
+import pygame
 import math
-import time
+import GlobalVariables as gv
 
-import DataTypes as dt
-
-def normalize(x, y, deadzone=0.15):
-    if abs(x) < deadzone:
-        x = 0.0
-    if abs(y) < deadzone:
-        y = 0.0
-
-    mag = math.hypot(x, y)
-    if mag > 1.0:
-        x /= mag
-        y /= mag
-
-    return x, y
-
-
-def controller_loop(shared_intent: dt.ControlIntent, lock):
+def controller():
     pygame.init()
-    pygame.joystick.init()
+    
+    # This is a simple class that will help us print to the screen.
+    # It has nothing to do with the joysticks, just outputting the
+    # information.
+    class TextPrint:
+        def __init__(self):
+            self.reset()
+            self.font = pygame.font.Font(None, 25)
 
-    joysticks = {}
-    prev_buttons = {}
+        def tprint(self, screen, text):
+            text_bitmap = self.font.render(text, True, (0, 0, 0))
+            screen.blit(text_bitmap, (self.x, self.y))
+            self.y += self.line_height
 
-    clock = pygame.time.Clock()
+        def reset(self):
+            self.x = 10
+            self.y = 10
+            self.line_height = 15
 
-    while True:
-        # 🔑 REQUIRED: keeps joystick state updating
-        pygame.event.pump()
+        def indent(self):
+            self.x += 10
 
-        for event in pygame.event.get():
-            if event.type == pygame.JOYDEVICEADDED:
-                joy = pygame.joystick.Joystick(event.device_index)
-                joysticks[joy.get_instance_id()] = joy
-                prev_buttons[joy.get_instance_id()] = [0] * joy.get_numbuttons()
-                print(f"[Controller] Joystick connected: {joy.get_name()}")
+        def unindent(self):
+            self.x -= 10
 
-            elif event.type == pygame.JOYDEVICEREMOVED:
-                joysticks.pop(event.instance_id, None)
-                prev_buttons.pop(event.instance_id, None)
-                print("[Controller] Joystick disconnected")
 
-        if not joysticks:
-            time.sleep(0.1)
-            continue
+    def main():
+        
+        def normalize_controller_input(x, y):
+            # Berechne die Laenge des Vektors
+            magnitude = math.sqrt(x**2 + y**2)
+    
+            # Wenn die Laenge groesser als 1 ist, normalisiere sie
+            if magnitude > 1.0:
+                x /= magnitude
+                y /= magnitude
+    
+            return x, y
+    
+        # Set the width and height of the screen (width, height), and name the window.
+        screen = pygame.display.set_mode((500, 700))
+        pygame.display.set_caption("Controller Inputs")
 
-        # Use first joystick
-        joy = next(iter(joysticks.values()))
-        jid = joy.get_instance_id()
+        # Used to manage how fast the screen updates.
+        clock = pygame.time.Clock()
 
-        # ---- AXES ----
-        forward = -joy.get_axis(1)   # usually inverted
-        sideways = joy.get_axis(0)
+        # Get ready to print.
+        text_print = TextPrint()
 
-        forward, sideways = normalize(forward, sideways)
+        # This dict can be left as-is, since pygame will generate a
+        # pygame.JOYDEVICEADDED event for every joystick connected
+        # at the start of the program.
+        joysticks = {}
 
-        walk = abs(forward) > 0 or abs(sideways) > 0
+        done = False
+        while not done:
+            # Event processing step.
+            # Possible joystick events: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN,
+            # JOYBUTTONUP, JOYHATMOTION, JOYDEVICEADDED, JOYDEVICEREMOVED
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT:
+                    done = True  # Flag that we are done so we exit this loop.
 
-        # ---- BUTTONS ----
-        buttons = joy.get_numbuttons()
-        current_buttons = [joy.get_button(i) for i in range(buttons)]
+                if event.type == pygame.JOYBUTTONDOWN:
+                    print("Joystick button pressed.")
+                    if event.button == 3 and gv.emote is None:
+                        gv.emote = "wave"
+                    if event.button == 2 and gv.emote is None:
+                        gv.robotCommunication.send_text("Huhrensohn")
+                    if event.button == 0:
+                        joystick = joysticks[event.instance_id]
+                        gv
+                        if joystick.rumble(0, 0.7, 500):
+                            print(f"Rumble effect played on joystick {event.instance_id}")
 
-        emote = None
-        quit_flag = False
+                if event.type == pygame.JOYBUTTONUP:
+                    print("Joystick button released.")
 
-        for i in range(buttons):
-            if current_buttons[i] and not prev_buttons[jid][i]:
-                # Button DOWN (edge)
-                if i == 0:
-                    emote = "HELLO"
-                elif i == 1:
-                    emote = "SAD"
-                elif i == 9:
-                    quit_flag = True
+                # Handle hotplugging
+                if event.type == pygame.JOYDEVICEADDED:
+                    # This event will be generated when the program starts for every
+                    # joystick, filling up the list without needing to create them manually.
+                    joy = pygame.joystick.Joystick(event.device_index)
+                    joysticks[joy.get_instance_id()] = joy
+                    print(f"Joystick {joy.get_instance_id()} connencted")
 
-        prev_buttons[jid] = current_buttons
+                if event.type == pygame.JOYDEVICEREMOVED:
+                    del joysticks[event.instance_id]
+                    print(f"Joystick {event.instance_id} disconnected")
 
-        # ---- UPDATE INTENT (atomic) ----
-        with lock:
-            shared_intent.move_x = forward
-            shared_intent.move_y = sideways
-            shared_intent.walk = walk
+            # Drawing step
+            # First, clear the screen to white. Don't put other drawing commands
+            # above this, or they will be erased with this command.
+            screen.fill((255, 255, 255))
+            text_print.reset()
 
-            if emote:
-                shared_intent.emote = emote
+            # Get count of joysticks.
+            joystick_count = pygame.joystick.get_count()
 
-            if quit_flag:
-                shared_intent.quit = True
+            text_print.tprint(screen, f"Number of joysticks: {joystick_count}")
+            text_print.indent()
 
-        clock.tick(60)
+            # For each joystick:
+            for joystick in joysticks.values():
+                jid = joystick.get_instance_id()
+
+                text_print.tprint(screen, f"Joystick {jid}")
+                text_print.indent()
+
+                # Get the name from the OS for the controller/joystick.
+                name = joystick.get_name()
+                text_print.tprint(screen, f"Joystick name: {name}")
+
+                guid = joystick.get_guid()
+                text_print.tprint(screen, f"GUID: {guid}")
+
+                power_level = joystick.get_power_level()
+                text_print.tprint(screen, f"Joystick's power level: {power_level}")
+
+                # Usually axis run in pairs, up/down for one, and left/right for
+                # the other. Triggers count as axes.
+                axes = joystick.get_numaxes()
+                text_print.tprint(screen, f"Number of axes: {axes}")
+                text_print.indent()
+
+                for i in range(axes):
+                    axis = joystick.get_axis(i)
+                    gv.vector_dirmov = [axis]
+                    text_print.tprint(screen, f"Axis {i} value: {axis:>6.3f}")
+                text_print.unindent()
+                
+                # Get Movement Direction Vector
+                # Left stick (translation)
+                vx = joystick.get_axis(1)   # forward/back
+                vy = joystick.get_axis(0)   # strafe
+
+                vx, vy = normalize_controller_input(vx, vy)
+
+                # Right stick X (rotation) — adjust axis index if needed
+                omega = joystick.get_axis(2)
+
+                # Deadzone for rotation
+                if abs(omega) < 0.2:
+                    omega = 0.0
+
+                # Apply axis snapping (only for translation)
+                if 0.8 < vx and -0.2 < vy < 0.2:
+                    vx, vy = 1.0, 0.0
+                elif vx < -0.8 and -0.2 < vy < 0.2:
+                    vx, vy = -1.0, 0.0
+                elif -0.2 < vx < 0.2 and 0.8 < vy:
+                    vx, vy = 0.0, 1.0
+                elif -0.2 < vx < 0.2 and vy < -0.8:
+                    vx, vy = 0.0, -1.0
+
+                # Final movement vector
+                gv.vector_dirmov = [vx, vy, omega]
+
+                # Robot state
+                if abs(vx) < 0.2 and abs(vy) < 0.2 and abs(omega) < 0.2:
+                    gv.robot_state = "idle"
+                else:
+                    gv.robot_state = "walking"
+
+                buttons = joystick.get_numbuttons()
+                text_print.tprint(screen, f"Number of buttons: {buttons}")
+                text_print.indent()
+
+                for i in range(buttons):
+                    button = joystick.get_button(i)
+                    text_print.tprint(screen, f"Button {i:>2} value: {button}")
+                text_print.unindent()
+
+                hats = joystick.get_numhats()
+                text_print.tprint(screen, f"Number of hats: {hats}")
+                text_print.indent()
+
+                # Hat position. All or nothing for direction, not a float like
+                # get_axis(). Position is a tuple of int values (x, y).
+                for i in range(hats):
+                    hat = joystick.get_hat(i)
+                    text_print.tprint(screen, f"Hat {i} value: {str(hat)}")
+                text_print.unindent()
+
+                text_print.unindent()
+
+            # Go ahead and update the screen with what we've drawn.
+            pygame.display.flip()
+
+            # Limit to 30 frames per second.
+            clock.tick(30)
+            
+    main()
+    pygame.quit()
\ No newline at end of file
diff --git a/DataTypes.py b/DataTypes.py
index 061cafa..7496c2b 100644
--- a/DataTypes.py
+++ b/DataTypes.py
@@ -19,8 +19,6 @@ class PosArray:
     def __getitem__(self, key): return self.data[key]
     def __iter__(self): return iter(self.data)
     def __repr__(self): return f"PosArray(\n{self.data}\n)"
-    def copy(self):
-        return PosArray(self.data.copy())
 
 
 @dataclass
@@ -39,8 +37,6 @@ class DegArray:
     def __getitem__(self, key): return self.data[key]
     def __iter__(self): return iter(self.data)
     def __repr__(self): return f"PosArray(\n{self.data}\n)"
-    def copy(self):
-        return DegArray(self.data.copy())
 
 @dataclass
 class RadArray:
@@ -57,8 +53,7 @@ class RadArray:
     def __getitem__(self, key): return self.data[key]
     def __iter__(self): return iter(self.data)
     def __repr__(self): return f"PosArray(\n{self.data}\n)"
-    def copy(self):
-        return RadArray(self.data.copy())
+
 
 @dataclass
 class RobotCommand:
@@ -68,10 +63,6 @@ class RobotCommand:
 
 @dataclass
 class ControlIntent:
-    move_x: float = 0.0
-    move_y: float = 0.0
-    turn: float = 0.0
-    emote: str | None = None
-    walk: bool = False
-    quit: bool = False
-
+    move_vector: Vector2
+    turn: float
+    emote: str | None
\ No newline at end of file
diff --git a/EspCommunication.py b/EspCommunication.py
index fddaca7..e4f4770 100644
--- a/EspCommunication.py
+++ b/EspCommunication.py
@@ -8,139 +8,113 @@ from queue import Queue
 import config as cfg
 import DataTypes as dt
 
-
 # ============================================================
-# Protocol definitions (SHARED with ESP32)
+# Protocol definitions (MATCHES THE ULTIMATE .INO)
 # ============================================================
-
 PACKET_VERSION = 1
-
-# Packet IDs
 PKT_COMMAND = 1
-PKT_STATUS  = 2
-PKT_DEBUG   = 3
+PKT_CONFIG  = 2
+PKT_DISPLAY = 3
 
-# Field Types
-FT_ARRAY_F32 = 1     # float32 array
-FT_STRING    = 2     # utf-8 string
+FT_ARRAY_F32 = 1
+FT_STRING    = 2
 FT_UINT8     = 3
-FT_FLOAT32   = 4
+FT_CONFIG_VAL = 4
 
-# Header: packet_id | version | payload_len | timestamp_ms
-HEADER_FMT  = "<B B H I"
-HEADER_SIZE = struct.calcsize(HEADER_FMT)
-
-# TLV field: type | length | value
+HEADER_FMT  = "<B B H I" # ID, Ver, Len, Timestamp
 TLV_FMT = "<B H"
-TLV_SIZE = struct.calcsize(TLV_FMT)
-
-
-# ============================================================
-# ESP32 Communication Thread
-# ============================================================
 
 class ESP32Communication(Thread):
-    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
+    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port):
         super().__init__(daemon=True)
-
-        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        self.sock.settimeout(timeout)
-
-        print(f"Connecting to ESP32 at {host}:{port} ...")
-        self.sock.connect((host, port))
-        print("Connected to ESP32!")
-
+        self.addr = (host, port)
+        
+        # UDP Socket - Zero Lag
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        
         self.tx_queue = Queue()
-        self.rx_queue = Queue()
         self.running = Event()
         self.running.set()
+        self.last_motion_data = None
+        print(f"Communication Initialized: Target {host}:{port}")
 
-    # --------------------------------------------------------
-    # Thread loop
-    # --------------------------------------------------------
     def run(self):
-        self.sock.setblocking(False)
-
+        # --- THE FIX: Send Configs immediately upon starting the thread ---
+        self.sync_robot_configs()
+        
         while self.running.is_set():
-            self._handle_tx()
-            self._handle_rx()
-            time.sleep(0.002)
+            if not self.tx_queue.empty():
+                data = self.tx_queue.get()
+                try:
+                    self.sock.sendto(data, self.addr)
+                except Exception as e:
+                    print(f"UDP Send Error: {e}")
+            
+            # Tiny sleep to prevent 100% CPU usage
+            time.sleep(0.001)
 
-    # --------------------------------------------------------
-    # Public API
-    # --------------------------------------------------------
-    def send_command(self, radial_array :dt.RadArray, state=0, name=""):
+    def sync_robot_configs(self):
         """
-        Example command packet:
-        - 3x6 float array
-        - uint8 state
-        - optional string
+        Sends the parameters from your config.py to the ESP32.
+        This tells the ESP32 how to map degrees to pulses and 
+        how fast the Python loop is running for smoothing.
         """
-        data = np.degrees(radial_array).astype(np.uint8)
-        print(data)
-        fields = [
-            self._tlv_array(data),
-            self._tlv_uint8(state)
+        print("Sending Config Sync to Robot...")
+        
+        # Calculate interval in ms based on your FPS (tickpersec)
+        interval_ms = int(1000 / cfg.standard_tickpersec)
+        
+        # Config IDs: 1=Min, 2=Max, 3=Interval
+        configs = [
+            (1, 125), # servoMin (Default for 50Hz)
+            (2, 625), # servoMax (Default for 50Hz)
+            (3, interval_ms)
         ]
+        
+        fields = []
+        for c_id, val in configs:
+            # Payload: 1 byte for ID, 2 bytes for Value (Little Endian)
+            payload = struct.pack("<B h", c_id, val)
+            fields.append(struct.pack(TLV_FMT, FT_CONFIG_VAL, len(payload)) + payload)
+            
+        packet = self._build_packet(PKT_CONFIG, fields)
+        self.tx_queue.put(packet)
 
-        if name:
-            fields.append(self._tlv_string(name))
-
+    def send_motion(self, radial_array: dt.RadArray):
+        """Sends motion only if the data has changed."""
+        # 1. Convert to degrees/uint8 as you did before
+        new_data = np.degrees(radial_array.data).astype(np.uint8)
+        
+        # 2. Check if it's different from the last sent packet
+        if self.last_motion_data is not None:
+            if np.array_equal(new_data, self.last_motion_data):
+                return  # Skip sending if data is identical
+        
+        # 3. Update the tracker and send
+        self.last_motion_data = new_data.copy()
+        
+        fields = [self._tlv(FT_ARRAY_F32, new_data.tobytes())]
         packet = self._build_packet(PKT_COMMAND, fields)
         self.tx_queue.put(packet)
 
-    def receive(self):
-        """
-        Non-blocking receive.
-        Returns (packet_id, timestamp, fields) or None
-        """
-        try:
-            return self.rx_queue.get_nowait()
-        except Exception:
-            return None
+    def send_text(self, text=""):
+        """Sends a dedicated display packet independent of motion."""
+        # Convert string to bytes
+        text_bytes = text.encode("utf-8")
+    
+        # Wrap in TLV format (Type 2 = FT_STRING)
+        field = self._tlv(FT_STRING, text_bytes)
+    
+        # Build the packet using the PKT_DISPLAY (ID 3)
+        packet = self._build_packet(PKT_DISPLAY, [field])
+    
+        self.tx_queue.put(packet)
 
-    def stop(self):
-        self.running.clear()
-        self.join()
-        self.sock.close()
+    def _tlv(self, ftype, data):
+        return struct.pack(TLV_FMT, ftype, len(data)) + data
 
-    # --------------------------------------------------------
-    # TX / RX handlers
-    # --------------------------------------------------------
-    def _handle_tx(self):
-        if not self.tx_queue.empty():
-            data = self.tx_queue.get()
-            self.sock.sendall(data)
-
-    def _handle_rx(self):
-        try:
-            header = self._recv_exact(HEADER_SIZE)
-            if not header:
-                return
-
-            packet_id, version, payload_len, timestamp = struct.unpack(HEADER_FMT, header)
-
-            if payload_len == 0:
-                # Nothing to parse
-                self.rx_queue.put((packet_id, timestamp, []))
-                return
-
-            payload = self._recv_exact(payload_len)
-            if payload is None:
-                return  # incomplete, skip
-
-            fields = self._parse_tlv(payload)
-            self.rx_queue.put((packet_id, timestamp, fields))
-
-        except BlockingIOError:
-            pass
-
-    # --------------------------------------------------------
-    # Packet construction
-    # --------------------------------------------------------
     def _build_packet(self, packet_id, fields):
         payload = b"".join(fields)
-
         header = struct.pack(
             HEADER_FMT,
             packet_id,
@@ -148,57 +122,8 @@ class ESP32Communication(Thread):
             len(payload),
             int(time.time() * 1000) & 0xFFFFFFFF
         )
-
         return header + payload
 
-    # --------------------------------------------------------
-    # TLV helpers
-    # --------------------------------------------------------
-    def _tlv(self, ftype, data):
-        return struct.pack(TLV_FMT, ftype, len(data)) + data
-
-    def _tlv_array(self, array):
-        # Convert to uint8 degrees before sending
-        arr_uint8 = array.astype(np.uint8)
-        return self._tlv(FT_ARRAY_F32, arr_uint8.tobytes())
-
-    def _tlv_string(self, text):
-        return self._tlv(FT_STRING, text.encode("utf-8"))
-
-    def _tlv_uint8(self, value):
-        return self._tlv(FT_UINT8, struct.pack("<B", value))
-
-    # --------------------------------------------------------
-    # TLV parsing
-    # --------------------------------------------------------
-    def _parse_tlv(self, payload):
-        pos = 0
-        fields = []
-
-        while pos < len(payload):
-            ftype, flen = struct.unpack(
-                TLV_FMT, payload[pos:pos + TLV_SIZE]
-            )
-            pos += TLV_SIZE
-
-            data = payload[pos:pos + flen]
-            pos += flen
-
-            fields.append((ftype, data))
-
-        return fields
-
-    # --------------------------------------------------------
-    # Socket helper
-    # --------------------------------------------------------
-    def _recv_exact(self, size):
-        data = b""
-        while len(data) < size:
-            try:
-                chunk = self.sock.recv(size - len(data))
-                if not chunk:
-                    return None
-                data += chunk
-            except BlockingIOError:
-                return None
-        return data
\ No newline at end of file
+    def stop(self):
+        self.running.clear()
+        self.sock.close()
\ No newline at end of file
diff --git a/GlobalVariables.py b/GlobalVariables.py
index 3d80ce4..553c3ef 100644
--- a/GlobalVariables.py
+++ b/GlobalVariables.py
@@ -9,6 +9,9 @@ import config as cfg
 # Globale Variablen
 robotCommunication = None
 shared_sim = None
+emote = None
+display_text = ""
+
 if cfg.sim:
     shared_sim = Simulation()
 else:
@@ -17,7 +20,7 @@ else:
     else:
         robotCommunication = ESP32Communication()
 
-vector_dirmov = [0, 0]  # Direction Movement
+vector_dirmov = [0, 0, 0]  # Direction Movement [vx, vy, omega]
 current_rad: dt.RadArray  # Current Rad
 current_pos: dt.PosArray  # Current Position
 
@@ -43,26 +46,26 @@ control_pause: bool = False
 #        [90, 150, 65],
 #    ]
 #)
-init_deg: dt.DegArray = dt.DegArray(
-    [
-        [90, 30, 95],
-        [90, 30, 95],
-        [90, 30, 95],
-        [90, 150, 85],
-        [90, 150, 85],
-        [90, 150, 85],
-    ]
-)
 #init_deg: dt.DegArray = dt.DegArray(
 #    [
-#        [90, 45, 140],
-#        [90, 45, 140],
-#        [90, 45, 140],
-#        [90, 135, 40],
-#        [90, 135, 40],
-#        [90, 135, 40],
+#        [90, 30, 95],
+#        [90, 30, 95],
+#        [90, 30, 95],
+#        [90, 150, 85],
+#        [90, 150, 85],
+#        [90, 150, 85],
 #    ]
 #)
+init_deg: dt.DegArray = dt.DegArray(
+    [
+        [90, 45, 140],
+        [90, 45, 140],
+        [90, 45, 140],
+        [90, 135, 40],
+        [90, 135, 40],
+        [90, 135, 40],
+    ]
+)
 
 init90_deg: dt.DegArray = dt.DegArray(
     [
diff --git a/README.md b/README.md
index 5562300..9d65d46 100644
--- a/README.md
+++ b/README.md
@@ -1,67 +1,103 @@
 
 # JackBot — Hexapod Control & Simulation
 
-**Purpose**
-- **Overview**: JackBot is a Python project for controlling and simulating a six-legged (hexapod) robot. It computes foot trajectories, runs inverse kinematics against a URDF model, and sends joint commands to either a hardware controller (ESP32 / Arduino) or a PyBullet simulation.
+JackBot is a Python project for controlling and simulating a six-legged hexapod robot.
+It uses IKPy for inverse kinematics, PyBullet for optional simulation, and can send joint commands to ESP32 or Arduino hardware.
 
-**Setup Dependencies and Virtual Enviroment**
-- **Python packages**: At minimum the project uses `numpy`, `pygame`, `ikpy`, `pybullet`, `pyserial`, and `matplotlib`.
-- **Install** (recommended inside a venv):
+## Requirements
 
-Linux / macOS (Bash):
+- Python 3.12 recommended on Windows
+- Python 3.11 / 3.12 is safest for `pygame` compatibility
+- Required Python packages:
+	- `numpy`
+	- `pygame`
+	- `ikpy`
+	- `pybullet`
+	- `pyserial`
+	- `matplotlib`
+
+## Setup
+
+### Linux / macOS (Bash)
 
 ```bash
 python3 -m venv .venv
 source .venv/bin/activate
+python -m pip install --upgrade pip setuptools wheel
 pip install numpy pygame ikpy pybullet pyserial matplotlib
 ```
 
-Windows (PowerShell):
+### Windows (PowerShell)
 
 ```powershell
 python -m venv .venv
 .\.venv\Scripts\Activate.ps1
+python -m pip install --upgrade pip setuptools wheel
 pip install numpy pygame ikpy pybullet pyserial matplotlib
 ```
 
-If PowerShell blocks activation, run:
+If PowerShell blocks activation:
 
 ```powershell
 Set-ExecutionPolicy -Scope Process -ExecutionPolicy RemoteSigned
 .\.venv\Scripts\Activate.ps1
 ```
 
-**Quick Start**
-- **Simulation**: enable the simulator in `config.py` by setting `sim = True`, then run:
+## Run
+
+From the repository root:
+
+```powershell
+python main.py
+```
+
+or on Linux/macOS:
 
 ```bash
 python3 main.py
 ```
-- **Hardware**: set `sim = False` in `config.py` and choose `arduinoConnection = True` or provide your ESP32 settings. Verify `port`, `baudrate`, `esp32_ip`, and `esp32_port` in `config.py`.
 
-**Project structure (key files)**
-- **Entry point**: [main.py](main.py) — starts the controller and state loop.
-- **Controller**: [Controller.py](Controller.py) — joystick input and `ControlIntent` updates.
-- **Kinematics**: [kinematics.py](kinematics.py) — URDF-based forward/inverse kinematics using IKPy.
-- **Simulation**: [simulation.py](simulation.py) — PyBullet visualization and joint control.
-- **Hardware comms**: [EspCommunication.py](EspCommunication.py) and [ArduinoCommunication.py](ArduinoCommunication.py) — send commands to ESP32 or Arduino.
-- **Runtime globals**: [GlobalVariables.py](GlobalVariables.py) — central runtime objects and flags.
-- **Types**: [DataTypes.py](DataTypes.py) — `PosArray`, `DegArray`, `RadArray`, `ControlIntent`.
-- **States**: [RobotState/idle.py](RobotState/idle.py) and [RobotState/walking.py](RobotState/walking.py) — state machine implementations.
-- **Config**: [config.py](config.py) — ports, flags (`sim`, `arduinoConnection`), URDF path, timing values.
+## Configuration
 
-**How it works**
-- `main.py` creates a `ControlIntent` and `RobotContext`, starts the controller thread, and runs a state loop. The `WalkingState` computes per-leg foot paths (Bezier/linear), converts positions to joint angles with `kinematics.ikpyInverse()`, then sends commands to hardware or updates the simulator.
+Edit `config.py` before running:
 
-**Configuration notes**
-- **URDF**: the robot description file is `JackBotUrdf.urdf`; ensure the path in `config.py` (`urdf_path`) is correct.
-- **Timing**: step timing and tick rate are configured in `config.py` (`standard_duration`, `standard_tickpersec`).
+- `sim = True` to enable PyBullet simulation
+- `sim = False` to use hardware control
+- `arduinoConnection = True` to use Arduino
+- `arduinoConnection = False` to use ESP32
+- `port` and `baudrate` for Arduino
+- `esp32_ip` and `esp32_port` for ESP32
+- `urdf_path = "JackBotUrdf.urdf"`
 
-**Suggested next steps**
-- Add a `requirements.txt` or `pyproject.toml` for reproducible installs.
-- Document hardware wiring and expected serial/ESP packet formats if you plan to use hardware.
-- Provide an example `config.py` for common setups (sim vs hardware).
+## Project structure
 
-**License & Contributing**
-- No license file included. Add `LICENSE` if you want to publish or share.
-- Contributions: open an issue or PR with improvements; add tests for kinematics if possible.
\ No newline at end of file
+- `main.py` — main application entry point
+- `Controller.py` — Pygame-based controller and input display
+- `kinematics.py` — IKPy forward/inverse kinematics
+- `simulation.py` — PyBullet simulation wrapper
+- `GlobalVariables.py` — shared runtime state and comms
+- `DataTypes.py` — typed arrays and control intent
+- `RobotState/idle.py` — idle robot state
+- `RobotState/walking.py` — walking robot state
+- `EspCommunication.py` — ESP32 communication
+- `ArduinoCommunication.py` — Arduino communication
+- `JackBotUrdf.urdf` — robot model file
+
+## Notes
+
+- `GlobalVariables.py` initializes either `Simulation()` or the selected hardware comm class.
+- `DataTypes.py` declares `PosArray`, `DegArray`, `RadArray`, `RobotCommand`, and `ControlIntent`.
+- `Controller.py` updates `gv.vector_dirmov` and `gv.robot_state` from joystick input.
+- `kinematics.py` loads leg chains from `JackBotUrdf.urdf` and computes IK.
+
+## Troubleshooting
+
+- If `pygame` installation fails on Windows, use Python 3.12 and upgrade `pip setuptools wheel` first.
+- If `python` is not found on Windows, install Python and enable "Add Python to PATH".
+- If the program crashes on startup, verify `JackBotUrdf.urdf` path and `config.py` settings.
+
+## Suggested improvements
+
+- Add a `requirements.txt` or `pyproject.toml`.
+- Add a `LICENSE` file before sharing the project.
+- Document hardware wiring and packet formats for ESP32/Arduino.
\ No newline at end of file
diff --git a/RobotState.py b/RobotState.py
new file mode 100644
index 0000000..fb304cd
--- /dev/null
+++ b/RobotState.py
@@ -0,0 +1,319 @@
+import GlobalVariables as gv
+import config as cfg
+import DataTypes as dt
+import kinematics as kin
+import numpy as np
+import math
+import time
+
+# Leg_Pair 1 {leg[num] = 0,2,4}
+# Leg Pair 2 {leg[num] = 1,3,5}
+
+def emitCalculation(target_rad: dt.RadArray):
+    if gv.shared_sim != None:
+        gv.shared_sim.updatePos(target_rad)
+        gv.shared_sim.step()
+    if gv.robotCommunication != None:
+        gv.robotCommunication.send_motion(target_rad)
+    gv.current_rad = target_rad
+
+def initPos():
+    # Init Position and gv.current_rad
+    gv.current_rad = gv.init_deg.to_rad()
+    gv.current_pos = gv.center_points
+    target_rad: dt.RadArray = kin.ikpyInverse(gv.center_points)
+    emitCalculation(target_rad)
+
+
+def walking(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
+    # init Values
+    ticks = int(duration * tickpersec)
+    tick_duration = 1 / tickpersec
+    tick_pos: dt.PosArray  # aktuelle XYZ-Positionen
+    current_pos_copy: dt.PosArray = gv.current_pos
+    vx, vy, omega = gv.vector_dirmov
+
+    # Apply config scaling
+    vx *= cfg.translation_gain
+    vy *= cfg.translation_gain
+    omega *= cfg.rotation_gain
+
+    target_pos_temp = []
+
+    for i in range(6):
+        cx, cy, cz = gv.center_points[i]
+
+        # relative position (assuming body center = 0,0)
+        rx = cx
+        ry = cy
+
+        # rotation component
+        v_rot_x = -omega * ry
+        v_rot_y =  omega * rx
+
+        # combine translation + rotation
+        v_x = vx + v_rot_x
+        v_y = vy + v_rot_y
+
+        # normalize combined vector (important!)
+        length = (v_x**2 + v_y**2) ** 0.5
+        if length > 1.0:
+            v_x /= length
+            v_y /= length
+
+        target_pos_temp.append([
+            cx + v_x * cfg.step_length,
+            cy + v_y * cfg.step_length,
+            cz
+        ])
+    target_pos: dt.PosArray = target_pos_temp
+
+    def interpolate(t, p0, p1, p2):
+        return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
+
+    # Smooth Step
+    for tick in range(ticks + 1):
+        loop_start = time.perf_counter()
+        t = tick / ticks
+        tick_pos_temp = []
+
+        for leg_id in range(6):
+            if gv.leg_state[leg_id] == "drag":
+                # lineares Gleiten in die Center-Position
+                tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)
+
+            elif gv.leg_state[leg_id] == "step":
+                # Mittlerer Kontrollpunkt für Bezier-Kurve
+                mid_point = [
+                    (current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
+                    (current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
+                    max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
+                    + cfg.step_height,
+                ]
+                x = interpolate(
+                    t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
+                )
+                y = interpolate(
+                    t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
+                )
+                z = interpolate(
+                    t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
+                )
+                tick_pos_temp.append([x, y, z])
+
+        tick_pos = dt.PosArray(tick_pos_temp)
+        # IK mit letzter Winkelstellung als Startpunkt
+        emitCalculation(kin.ikpyInverse(tick_pos))
+        print("Tick Position: \n", tick_pos)
+        if (cfg.sim == True):
+            gv.shared_sim.step()
+
+        # Timing anpassen, damit Loop gleichmäßig bleibt
+        elapsed = time.perf_counter() - loop_start
+        sleep_time = tick_duration - elapsed
+        if sleep_time > 0:
+            time.sleep(sleep_time)
+
+    # Endposition sichern
+    #emitCalculation(kin.ikpyInverse(target_pos))
+    #print("END TargetPos:\n", target_pos, "\n\n\n")
+
+    # Update current pos
+    gv.current_pos = tick_pos
+    gv.current_rad = kin.ikpyInverse(tick_pos)
+    #emitCalculation(kin.ikpyInverse(tick_pos))
+
+    # Gait-Zustand wechseln
+    if gv.leg_state[0] == "step":
+        gv.leg_state = np.array(["drag", "step", "drag", "step", "drag", "step"])
+    else:
+        gv.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
+
+def walking_four(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
+    # init Values
+    ticks = duration * tickpersec
+    tick_duration = 1 / tickpersec
+    tick_pos: dt.PosArray  # aktuelle XYZ-Positionen
+    current_rad_copy: dt.RadArray = gv.current_rad
+    current_pos_copy: dt.PosArray = gv.current_pos
+    dirmov_copy = gv.vector_dirmov
+
+    # Zielpunkte für jede Beinspitze berechnen
+    target_pos_temp = []
+    for i in range(6):
+        target_pos_temp.append(
+            [
+                gv.center_points[i][0] + dirmov_copy[0] * cfg.step_length,
+                gv.center_points[i][1] + dirmov_copy[1] * cfg.step_length,
+                gv.center_points[i][2]
+            ]
+        )
+    target_pos: dt.PosArray = target_pos_temp
+
+    def interpolate(t, p0, p1, p2):
+        return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
+
+    # Smooth Step
+    for tick in range(int(ticks) + 1):
+        loop_start = time.perf_counter()
+        t = tick / ticks
+        tick_pos_temp = []
+
+        for leg_id in range(6):
+            if gv.leg_state[leg_id] == "drag":
+                # lineares Gleiten in die Center-Position
+                tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)
+
+            elif gv.leg_state[leg_id] == "step":
+                # Mittlerer Kontrollpunkt für Bezier-Kurve
+                mid_point = [
+                    (current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
+                    (current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
+                    max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
+                    + cfg.step_height,
+                ]
+                x = interpolate(
+                    t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
+                )
+                y = interpolate(
+                    t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
+                )
+                z = interpolate(
+                    t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
+                )
+                tick_pos_temp.append([x, y, z])
+
+        tick_pos = dt.PosArray(tick_pos_temp)
+        # IK mit letzter Winkelstellung als Startpunkt
+        emitCalculation(kin.ikpyInverse(tick_pos))
+        print("Tick Position: \n", tick_pos)
+
+        # Timing anpassen, damit Loop gleichmäßig bleibt
+        elapsed = time.perf_counter() - loop_start
+        sleep_time = tick_duration - elapsed
+        if sleep_time > 0:
+            time.sleep(sleep_time)
+
+    # Endposition sichern
+    #emitCalculation(kin.ikpyInverse(target_pos))
+    print("END TargetPos:\n", target_pos, "\n\n\n")
+
+    # Gait-Zustand wechseln
+    if gv.leg_state[0] == "step":
+        gv.leg_state = np.array(["drag", "drag", "step", "drag", "drag", "drag"])
+    elif gv.leg_state[2] == "step":
+        gv.leg_state = np.array(["drag", "drag", "drag", "step", "drag", "drag"])
+    elif gv.leg_state[3] == "step":
+        gv.leg_state = np.array(["drag", "drag", "drag", "drag", "drag", "step"])
+    elif gv.leg_state[5] == "step":
+        gv.leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
+
+def wave_emote(cycles=3, duration=1.5, tickpersec=20,
+                   height=40, amplitude=25, inward_offset=10):
+    """
+    Greeting wave using front leg (leg 0)
+
+    Motion:
+    - Z: lifts leg up
+    - Y: waves left/right
+    - X: slightly pulled inward to avoid IK limits
+    """
+
+    ticks = int(duration * tickpersec)
+    tick_duration = 1 / tickpersec
+
+    # Safe copy
+    base_pos = dt.PosArray(np.copy(gv.current_pos.data))
+
+    leg_id = 0  # front leg
+
+    for cycle in range(cycles):
+        for tick in range(ticks):
+            loop_start = time.perf_counter()
+
+            t = tick / ticks
+            tick_pos = np.copy(base_pos.data)
+
+            cx, cy, cz = base_pos[leg_id]
+
+            # smooth outward-only wave
+            y_wave = math.sin(4 * math.pi * t)
+            y_offset = amplitude * (0.5 * (y_wave + 1))
+
+            # vertical lift
+            z_offset = height * math.sin(math.pi * t)
+
+            # clamp sideways motion
+            max_y_dev = 30
+            new_y = cy + y_offset
+            new_y = max(cy - max_y_dev, min(cy + max_y_dev, new_y))
+
+            tick_pos[leg_id] = [
+                cx + 10,     # small forward bias (IMPORTANT)
+                new_y,
+                cz + z_offset
+            ]
+
+            tick_pos = dt.PosArray(tick_pos)
+
+            emitCalculation(kin.ikpyInverse(tick_pos))
+
+            # Timing
+            elapsed = time.perf_counter() - loop_start
+            sleep_time = tick_duration - elapsed
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    # Return to base pose
+    emitCalculation(kin.ikpyInverse(base_pos))
+    gv.current_pos = base_pos
+
+def laola_wave_emote(cycles=3, duration=1.5, tickpersec=cfg.standard_tickpersec, height=10, amplitude=30):
+    ticks = int(duration * tickpersec)
+    tick_duration = 1 / tickpersec
+
+    # Safe copy of base position
+    base_pos = dt.PosArray(np.copy(gv.current_pos.data))
+
+    # Legs that perform the wave
+    wave_legs = [1, 4]
+
+    for cycle in range(cycles):
+        for tick in range(ticks):
+            loop_start = time.perf_counter()
+            t = tick / ticks  # normalized 0 → 1
+            tick_pos = np.copy(base_pos.data)
+
+            for leg_id in wave_legs:
+                cx, cy, cz = base_pos[leg_id]
+
+                # Phase shift for Laola wave
+                phase = 0 if leg_id == 1 else math.pi
+
+                # Sideways motion (Y) — reduced amplitude to avoid overextension
+                y_offset = amplitude * math.sin(2 * math.pi * t + phase)
+
+                # Vertical motion (Z) — full up/down oscillation
+                z_offset = height * math.sin(2 * math.pi * t + phase)
+
+                # Apply movement in YZ plane, X stays fixed
+                tick_pos[leg_id] = [
+                    cx,
+                    cy + y_offset,
+                    cz + z_offset
+                ]
+
+            tick_pos = dt.PosArray(tick_pos)
+
+            # IK + send command
+            emitCalculation(kin.ikpyInverse(tick_pos))
+
+            # Timing control
+            elapsed = time.perf_counter() - loop_start
+            sleep_time = tick_duration - elapsed
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    # Return to original stance
+    emitCalculation(kin.ikpyInverse(base_pos))
+    gv.current_pos = base_pos
\ No newline at end of file
diff --git a/config.py b/config.py
index 1c6bf42..ea46a56 100644
--- a/config.py
+++ b/config.py
@@ -16,8 +16,10 @@ urdf_path = "JackBotUrdf.urdf"
 
 # Dimensions in m
 #robot_height = -0.1
-step_height = 0.05
-step_length = 0.08
+step_height = 0.03
+step_length = 0.04
+translation_gain = 1.0
+rotation_gain = 2.0
 # Leglength in m
 # L1 = 0.068
 # L2 = 0.0602
@@ -25,5 +27,5 @@ step_length = 0.08
 
 # standard_tickpersec = # 20 Fluessige Bewegung
 standard_tickpersec:float = 25 # [ticks/s]
-standard_duration:float = 0.4  # [s],ip
+standard_duration:float = 0.8  # [s]
 standard_tickduration:float = 1 / standard_tickpersec
diff --git a/kinematics.py b/kinematics.py
index 23f6010..83b99d2 100644
--- a/kinematics.py
+++ b/kinematics.py
@@ -1,6 +1,5 @@
 from ikpy.chain import Chain
 from ikpy.link import OriginLink, URDFLink
-import matplotlib.pyplot as plt
 import numpy as np
 import math
 import time
diff --git a/main.py b/main.py
index a9d3316..20a58c3 100644
--- a/main.py
+++ b/main.py
@@ -1,62 +1,60 @@
+from threading import Thread
+from multiprocessing import Process, Manager, Queue
+import time
+
+# Selfmade Libraries
+import Controller as ctr
+import RobotState as rs
+
 # Global Variables
 import GlobalVariables as gv
 
-# main.py
-import time
-import threading
 
-from DataTypes import ControlIntent
-from RobotState.RobotState import RobotContext
-from Controller import controller_loop
-from RobotState.idle import IdleState
-from RobotState.walking import WalkingState
+def robot_control():
+    # Connection
+    if gv.robotCommunication != None:
+        gv.robotCommunication.start()
 
-import DataTypes as dt
+    # Start Position
+    rs.initPos()
+    time.sleep(1)
 
-def main():
-    intent = ControlIntent()
-    lock = threading.Lock()
-    ctx = RobotContext()
+    ############################## Main Loop ##############################
+    tick = 0.05
+    next_time = time.time()
 
-    ctx.center_points = dt.PosArray([
-        [50, 50, -80],
-        [50,-50, -80],
-        [0, 70, -80],
-        [0,-70, -80],
-        [-50,50, -80],
-        [-50,-50,-80]
-    ])
-    ctx.current_pos = ctx.center_points.copy()
+    while gv.control_pause == False:
+        next_time += tick
 
-    controller_thread = threading.Thread(
-        target=controller_loop,
-        args=(intent, lock),
-        daemon=True
-    )
-    controller_thread.start()
+        if gv.emote == "wave":
+            rs.initPos()
+            time.sleep(0.3)
+            rs.wave_emote()
+            gv.emote = None
+            gv.robot_state = "idle"
+            continue
 
-    states = {
-        "idle": IdleState(),
-        "walking": WalkingState()
-    }
+        # Robot
+        if gv.robot_state == "idle":
+            rs.initPos()
+            time.sleep(0.2)
+        elif gv.robot_state == "walking":
+            rs.walking()
 
-    current = states["idle"]
-    current.on_enter(ctx)
+        # Simulation
+        if gv.shared_sim != None:
+            gv.shared_sim.step()
 
-    last = time.perf_counter()
+        sleep_time = next_time - time.time()
+        if sleep_time > 0:
+            time.sleep(sleep_time)
 
-    while not intent.quit:
-        now = time.perf_counter()
-        dt_s = now - last
-        last = now
-
-        next_state = current.update(ctx, intent, dt_s)
-        if next_state:
-            current.on_exit(ctx)
-            current = states[next_state]
-            current.on_enter(ctx)
-
-        time.sleep(0.01)
 
 if __name__ == "__main__":
-    main()
+    controller_queue = Queue()
+    robot_queue = Queue()
+
+    controller_thread = Thread(target=ctr.controller)
+    controller_thread.start()
+    robot_control_thread = Thread(target=robot_control)
+    robot_control_thread.start()
\ No newline at end of file