diff --git a/.theia/launch.json b/.theia/launch.json
new file mode 100644
index 0000000..7e4253b
--- /dev/null
+++ b/.theia/launch.json
@@ -0,0 +1,8 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  "version": "0.2.0",
+  "configurations": [
+  
+  ]
+}
diff --git a/piconaut-caaar.ino b/piconaut-caaar.ino
index 9381678..b41dcf5 100644
--- a/piconaut-caaar.ino
+++ b/piconaut-caaar.ino
@@ -1,3 +1,6 @@
+#include <Adafruit_NeoPixel.h>
+#include <RP2040_PWM.h>
+
 const int numPins = 3;
 const int pwmPins[] = {A1, A2, A3};
 const float minDutyCycle[numPins] = {5.98, 6.03, 6.03};
@@ -5,6 +8,22 @@ const float maxDutyCycle[numPins] = {12.08, 12.02, 11.94};
 const float deadzone = 0.1;
 volatile float mappedDutyCycles[numPins] = {0.0, 0.0, 0.0};
 
+// WS2812B LEDs
+const int numLEDs = 6;
+const int ledPin = 10;
+Adafruit_NeoPixel strip(numLEDs, ledPin, NEO_GRB + NEO_KHZ800);
+
+// Servo
+const int servoPins[] = {11, 12};
+const int numServos = 2;
+const int servoFreq = 60;
+RP2040_PWM* servo1;
+RP2040_PWM* servo2;
+
+float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
 void setup() {
   Serial.begin(9600);
   delay(5000);
@@ -12,10 +31,18 @@ void setup() {
   for (int i = 0; i < numPins; i++) {
     pinMode(pwmPins[i], INPUT);
   }
-}
 
-float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
-  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+  // Set up WS2812B LEDs
+  strip.begin();
+  strip.show();
+  setLEDDefaultColors();
+
+  // Set up servo PWM
+  for (int i = 0; i < numServos; i++) {
+    pinMode(servoPins[i], OUTPUT);
+  }
+    servo1 = new RP2040_PWM(servoPins[0], servoFreq, 0);
+    servo2 = new RP2040_PWM(servoPins[1], servoFreq, 0);
 }
 
 void loop() {
@@ -34,7 +61,7 @@ void loop() {
     }
   }
 
-  delay(10); // Wait for 10ms before the next reading
+  delay(1);
 }
 
 // Running on core1
@@ -43,6 +70,7 @@ void setup1() {
 }
 
 void loop1() {
+  handleSerialCommands();  
   // Print the pin name and the mapped duty cycle in a format suitable for the Serial Plotter
   for (int i = 0; i < numPins; i++) {
     Serial.print("Pin ");
@@ -57,5 +85,67 @@ void loop1() {
     }
   }
 
-  delay(10); // Wait for 10ms before the next transmission
+  delay(1); // Wait for 1ms before the next transmission
+}
+
+void setLEDDefaultColors() {
+  for (int i = 0; i < 4; i++) {
+    strip.setPixelColor(i, strip.Color(255, 255, 255));
+  }
+  for (int i = 4; i < 6; i++) {
+    strip.setPixelColor(i, strip.Color(255, 0, 0));
+  }
+  strip.show();
+}
+
+void cycleLEDColors() {
+  static uint32_t color = strip.Color(255, 0, 0);
+  static uint8_t colorState = 0;
+
+  for (int i = 0; i < numLEDs; i++) {
+    strip.setPixelColor(i, color);
+  }
+  strip.show();
+
+  if (colorState == 0) {
+    if (color == strip.Color(255, 0, 0)) {
+      color = strip.Color(0, 255, 0);
+    } else if (color == strip.Color(0, 255, 0)) {
+      color = strip.Color(0, 0, 255);
+    } else {
+      color = strip.Color(255, 0, 0);
+    }
+  }
+}
+
+void handleSerialCommands() {
+  if (Serial.available() > 0) {
+    String command = Serial.readStringUntil('\n');
+    command.trim();
+
+    if (command.startsWith("l")) {
+      if (command == "l1") {
+        setLEDDefaultColors();
+      } else if (command == "l0") {
+        cycleLEDColors();
+      } else if (command.startsWith("l")) {
+        int ledNum;
+        int r, g, b;
+        if (sscanf(command.c_str(), "l%d %d,%d,%d", &ledNum, &r, &g, &b) == 4) {
+          if (ledNum >= 0 && ledNum < numLEDs) {
+            strip.setPixelColor(ledNum, strip.Color(r, g, b));
+            strip.show();
+          }
+        }
+      }
+    } else if (command.startsWith("s")) {
+      int servoNum;
+      int dutyCycle;
+      if (sscanf(command.c_str(), "s%d %d", &servoNum, &dutyCycle) == 2) {
+        if (servoNum >= 0 && servoNum < numServos && dutyCycle >= 0 && dutyCycle <= 255) {
+          analogWrite(servoPins[servoNum], dutyCycle);
+        }
+      }
+    }
+  }
 }