diff --git a/code.py b/code.py
index fc0920f..1ac3ab0 100644
--- a/code.py
+++ b/code.py
@@ -4,11 +4,23 @@ import usb_cdc
 import pwmio
 import digitalio
 
-pwm_X = pwmio.PWMOut(board.GP17, frequency=1000, duty_cycle=0)
-pwm_Y = pwmio.PWMOut(board.GP18, frequency=1000, duty_cycle=0)
-pen = digitalio.DigitalInOut(board.GP16)
-pen.direction = digitalio.Direction.OUTPUT
-pen.value = False  # Set pen to up position
+settling_time = 0.01 # Measured on the scope
+pen_time = 0.1
+
+# GP20 is purple
+pwm_Y = pwmio.PWMOut(board.GP20, frequency=1000, duty_cycle=0)
+# GP10 is blue
+pwm_X = pwmio.PWMOut(board.GP10, frequency=1000, duty_cycle=0)
+pen = digitalio.DigitalInOut(board.GP8)
+
+def set_pen_state(enable_low):
+    if enable_low:
+        pen.direction = digitalio.Direction.OUTPUT
+        pen.value = False  # Set to LOW
+    else:
+        pen.direction = digitalio.Direction.INPUT # Set to HI-Z
+    time.sleep(pen_time)
+        
 
 def message_handler(msg):
     parts = msg.split(',')
@@ -16,9 +28,11 @@ def message_handler(msg):
         print("Invalid message format")
         return
     print(f"X: {parts[0]}, Y: {parts[1]}, Pen: {parts[2]}")
+    pen_state = True if int(parts[2]) == 1 else False
+    set_pen_state(pen_state)
+    # Set pen before setting the PWM values to not get weird lines. 
     pwm_X.duty_cycle = int(parts[0])
     pwm_Y.duty_cycle = int(parts[1])
-    pen.value = True if int(parts[2]) == 1 else False
 
 print("Booted")
 while True:
@@ -26,6 +40,6 @@ while True:
         data = usb_cdc.data.read(usb_cdc.data.in_waiting)
         message = data.decode('utf-8').strip()
         message_handler(message)
-        time.sleep(0.5)
+        time.sleep(settling_time)
         usb_cdc.data.write(b"OK\n")
-    time.sleep(0.1)
\ No newline at end of file
+    time.sleep(0.01)
\ No newline at end of file