     analogWrite(R_REVERSE, 0);
 }
 
+void set_pan(int val) {
+    if (val >= 0 && val <= 180){
+        pan_pos = val;
+        pan.write(pan_pos);
+    }
+}
+
+void set_tilt(int val) {
+    if (val >= 0 && val <= 180){
+        tilt_pos = val;
+        tilt.write(tilt_pos);
+    }
+}
+
 void log(String message) {
     if (DEBUG) {
         Serial.print(message);

            else if (strcmp(token, "stop") == 0) {
                log("stop\n\r");
                stop();
            }
            else {
                log("Bad command\n\r");

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

    // Enable both drive motors
    digitalWrite(L_ENABLE, HIGH);
    digitalWrite(R_ENABLE, HIGH);
    
    init_servos();
    stop_servos();
}

void loop() {
    read_commands();
}

void init_servos() {
    pan.attach(pan_servo);
    tilt.attach(tilt_servo);

    pan.write(pan_center);
    tilt.write(tilt_center);

}

void stop_servos() {
    delay(50);

    pan.detach();
    tilt.detach();

    delay(50);

    digitalWrite(pan_servo, LOW);
    digitalWrite(tilt_servo, LOW);
}

void read_commands() {
