--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/controller.ino	Wed Nov 19 21:39:25 2014 +0100
@@ -0,0 +1,13 @@
+#include "servos.h"
+#include "serial.h"
+
+
+void setup() {
+    serial_setup();
+    servo_setup();
+}
+
+
+void loop() {
+    do_serial_command();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/misc.h	Wed Nov 19 21:39:25 2014 +0100
@@ -0,0 +1,8 @@
+#ifndef MISC_H
+#define MISC_H
+
+const double PI2 = PI / 2;
+const double PI4 = PI / 4;
+const double TAU = PI * 2;
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/serial.h	Wed Nov 19 21:39:25 2014 +0100
@@ -0,0 +1,7 @@
+#ifndef SERIAL_H
+#define SERIAL_H
+
+void serial_setup(void);
+void do_serial_command(void);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/serial.ino	Wed Nov 19 21:39:25 2014 +0100
@@ -0,0 +1,43 @@
+void serial_setup() {
+    Serial.begin(115200);
+}
+
+
+int16_t _serial_read_word() {
+    /* Read two bytes from the serial port and make an int from them. */
+    int16_t high = Serial.read();
+    int16_t low = Serial.read();
+    int16_t value = high << 8 | low;
+    return value;
+}
+
+
+void _serial_write_word(int16_t value) {
+    /* Take an int and send it through the serial port as two bytes. */
+    int16_t high = (value & 0xff00) >> 8;
+    int16_t low = value & 0xff;
+    Serial.write(high);
+    Serial.write(low);
+}
+
+
+void do_serial_command() {
+    static unsigned char serial_command = 0;
+    /* Read and execute a command from the serial port's buffer. */
+    switch (serial_command) {
+        case 0x00:
+            /* No command, read one. */
+            if (Serial.available() < 1) { return; }
+            serial_command = Serial.read();
+            return;
+        case 0xff:
+            /* Move all servos by 1/1000 of radians. */
+            if (Serial.available() < SERVOS * 2) { return; }
+            for (unsigned char servo = 0; servo < SERVOS; ++servo) {
+                double pos = _serial_read_word();
+                servo_move(servo, pos / 1000.0);
+            }
+            break;
+    }
+    serial_command = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/servos.h	Wed Nov 19 21:39:25 2014 +0100
@@ -0,0 +1,11 @@
+#ifndef SERVOS_H
+#define SERVOS_H
+
+// Number of servos.
+#define SERVOS 12
+
+void servo_move(unsigned char servo, double rads);
+void servo_setup(void);
+void servo_shutdown(void);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/servos.ino	Wed Nov 19 21:39:25 2014 +0100
@@ -0,0 +1,67 @@
+#include "misc.h"
+#include "servos.h"
+#include <Servo.h>
+
+// Range of the servos.
+#define SERVO_FREQ_MIN 600
+#define SERVO_FREQ_MAX 2400
+
+static const unsigned char SERVO_PINS[SERVOS] = {
+// ankle, knee, hip
+    2, 3, 5,    // front left
+    9, 8, 6,    // hind left
+    10, 11, 13, // hind right
+    17, 16, 14  // front right
+};
+static const char SERVO_REVERSE[SERVOS] = {
+// ankle, knee, hip
+    1, 1, 1,    // front left
+    1, 1, 1,    // hind left
+    1, 1, 1,    // hind right
+    1, 1, 1     // front right
+};
+static const char SERVO_TRIM[SERVOS] = {
+// ankle, knee, hip
+    0, 0, 0,    // front left
+    0, 0, 0,    // hind left
+    0, 0, 0,    // hind right
+    0, 0, 0     // front right
+};
+static Servo servos[SERVOS];
+
+
+void servo_move(unsigned char servo, double rads) {
+    // Move a servo to a position in radians between -PI/2 and PI/2.
+    if (servo >= SERVOS) {
+        return;
+    }
+    rads = rads * SERVO_REVERSE[servo] + PI2;
+    while (rads > TAU) {
+        rads -= TAU;
+    }
+    while (rads < 0) {
+        rads += TAU;
+    }
+    int freq = (SERVO_FREQ_MAX -
+                SERVO_FREQ_MIN) * rads / PI + SERVO_FREQ_MIN;
+    freq = min(SERVO_FREQ_MAX,
+               max(SERVO_FREQ_MIN,
+                   freq + SERVO_TRIM[servo] * 10));
+    servos[servo].writeMicroseconds(freq);
+}
+
+void servo_setup() {
+    // Initialize all servos.
+    for (unsigned char servo = 0; servo < SERVOS; ++servo) {
+        servos[servo].attach(SERVO_PINS[servo],
+                             SERVO_FREQ_MIN, SERVO_FREQ_MAX);
+        delay(150);
+    }
+}
+
+void servo_shutdown() {
+    // Power down all servos.
+    for (unsigned char servo = 0; servo < SERVOS; ++servo) {
+        servos[servo].detach();
+    }
+}