             else if (strcmp(token, "stop") == 0) {
                 log("stop\n\r");
                 stop();
+            }
+            else if (strcmp(token, "s") == 0) {
+                char* servo = strtok(NULL, ",");
+                int position = atoi(strtok(NULL, ","));
+                
+                log("servo\n\r");
+                if (strcmp(servo, "pan")) {
+                    set_pan(position);
+                } else if (strcmp(servo, "tilt")) {
+                    set_tilt(position);
+                }
+            }
+            else if (strcmp(token, "stop_servos") == 0) {
+                log("stop servos\n\r");
+                stop_servos();
+            }
+            else if (strcmp(token, "start_servos") == 0) {
+                log("start servos\n\r");
+                init_servos();
             }
             else {
                 log("Bad command\n\r");

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

    digitalWrite(L_ENABLE, HIGH);
    digitalWrite(R_ENABLE, HIGH);

    pan.attach(pan_servo);
    tilt.attach(tilt_servo);

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

    delay(100);

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

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

void loop() {
    // read_commands();
}

void read_commands() {

const int R_FORWARD = 5;
const int R_REVERSE = 3;

const int pan_servo = 10;
const int tilt_servo = 11;

const int pan_center = 113;
const int tilt_center = 86;

const int pan_min = 10;
const int pan_max = 170;

const int tilt_min = 10;
const int tilt_max = 180;

int pan_pos = pan_center;
int tilt_pos = tilt_center;

Servo pan;
Servo tilt;
