--- a/servo/servo.ino	Sat Aug 23 20:17:19 2014 +0200
+++ b/servo/servo.ino	Sat Aug 23 20:32:26 2014 +0200
@@ -29,14 +29,64 @@
 
 
 int _serial_read_word() {
+    /* Read two bytes from the serial port and make an int from them. */
     return Serial.read() | Serial.read() << 8;
 }
 
+void do_servo_degrees() {
+    /* Move a single servo by degrees. */
+    int servo = Serial.read();
+    int pos = Serial.read();
+    if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) {
+        servos[servo].write(pos);
+    }
+}
+
+void do_servos_degrees() {
+    /* Move all servos by degrees. */
+    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
+        int pos = Serial.read();
+        if (pos >= 0 && pos <= 180) {
+            servos[servo].write(pos);
+        }
+    }
+}
+
+void do_servo_microseconds() {
+    /* Move a single servo by microseconds. */
+    int servo = Serial.read();
+    int pos = _serial_read_word();
+    if (servo >= 0 && servo < MAX_SERVO &&
+        pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
+        servos[servo].writeMicroseconds(pos);
+    }
+}
+
+void do_servos_microseconds() {
+    /* Move all servos by microseconds. */
+    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
+        int pos = _serial_read_word();
+        if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
+            servos[servo].writeMicroseconds(pos);
+        }
+    }
+}
+
+void do_accel() {
+    /* Report the readings of the acceleration sensor. */
+    Serial.write(0xff);
+    Serial.write(map(analogRead(A2), 0, 676, 0, 255));
+    Serial.write(map(analogRead(A1), 0, 676, 0, 255));
+    Serial.write(map(analogRead(A0), 0, 676, 0, 255));
+}
+
+void do_beep() {
+    /* Emit a beep of specified frequency and duration. */
+    tone(A3, _serial_read_word(), Serial.read());
+}
 
 void do_serial_command() {
-    int servo;
-    int pos;
-
+    /* Read and execute a command from the serial port's buffer. */
     switch (serial_command) {
         case 0x00:
             /* No command, read one. */
@@ -50,62 +100,40 @@
             if (Serial.available() < 2) {
                 return;
             }
-            servo = Serial.read();
-            pos = Serial.read();
-            if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) {
-                servos[servo].write(pos);
-            }
+            do_servo_degrees();
             break;
         case 0xfe:
             /* Move all servos by degrees. */
             if (Serial.available() < MAX_SERVO) {
                 return;
             }
-            for (servo = 0; servo < MAX_SERVO; servo += 1) {
-                pos = Serial.read();
-                if (pos >= 0 && pos <= 180) {
-                    servos[servo].write(pos);
-                }
-            }
             serial_command = 0;
+            do_servos_degrees();
             break;
         case 0xfd:
             /* Move a single servo by microseconds. */
             if (Serial.available() < 3) {
                 return;
             }
-            servo = Serial.read();
-            pos = _serial_read_word();
-            if (servo >= 0 && servo < MAX_SERVO &&
-                pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
-                servos[servo].writeMicroseconds(pos);
-            }
+            do_servo_microseconds();
             break;
         case 0xfc:
             /* Move all servos by microseconds. */
             if (Serial.available() < MAX_SERVO * 2) {
                 return;
             }
-            for (servo = 0; servo < MAX_SERVO; servo += 1) {
-                pos = _serial_read_word();
-                if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
-                    servos[servo].writeMicroseconds(pos);
-                }
-            }
+            do_servos_microseconds();
             break;
         case 0xf0:
             /* Read the accelerometer sensor. */
-            Serial.write(0xff);
-            Serial.write(map(analogRead(A2), 0, 676, 0, 255));
-            Serial.write(map(analogRead(A1), 0, 676, 0, 255));
-            Serial.write(map(analogRead(A0), 0, 676, 0, 255));
+            do_accel();
             break;
         case 0xf1:
             /* Beep. */
             if (Serial.available() < 3) {
                 return;
             }
-            tone(A3, _serial_read_word(), Serial.read());
+            do_beep();
             break;
     }
     serial_command = 0;