     if (servo_state != 0) {
       Serial.println("Servo disabled");
       digitalWrite(LED_PIN, LOW);
+      rgbLED(255, 0, 0);
       beep(50);
       servo_state = 0;
     } 
     else {
       Serial.println("Servo enabled");
       digitalWrite(LED_PIN, HIGH);
+      rgbLED(0, 255, 0);
       beep(50);
       servo_state = 1;
     }
 
     while (digitalRead(BUTTON_PIN) == HIGH);
   }
+}
+
+void rgbLED(int red, int green, int blue) {
+  analogWrite(RED_LED_PIN, red);
+  analogWrite(GREEN_LED_PIN, green);
+  analogWrite(BLUE_LED_PIN, blue);  
 }
 
 void beep(unsigned char delayms) {

  // Initialize speaker
  pinMode(SPEAKER_PIN, OUTPUT);
  
  beep(50);
  beep(50);
  beep(50);

  Serial.println("Setup complete");

void setup() {
  Serial.begin(9600);

  // Initialize LED outputs
  pinMode(LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(BLUE_LED_PIN, OUTPUT);

  // Initialize button input
  pinMode(BUTTON_PIN, INPUT);
