--- a/robot.py	Sat Aug 23 20:32:26 2014 +0200
+++ b/robot.py	Sun Aug 24 14:27:09 2014 +0200
@@ -60,6 +60,9 @@
             values[i] = int(float(sum(a[i] for a in accels)) / probes - self.accel_trim[i])
         return values
 
+    def beep(self, frequency, duration):
+        self.legs[0].hip.beep(frequency, duration)
+
     def _run_animations(self, animations, fps=12):
         clock = pygame.time.Clock()
         while animations:

--- a/servo.py	Sat Aug 23 20:32:26 2014 +0200
+++ b/servo.py	Sun Aug 24 14:27:09 2014 +0200
@@ -46,6 +46,13 @@
             pass
         return [ord(c) for c in self.serial.read(3)]
 
+    def beep(self, frequency, duration):
+        command = '\xf1'
+        self.serial.write(command)
+        self.serial.write('%c' % (frequency & 0xff))
+        self.serial.write('%c' % (frequency >> 8 & 0xff))
+        self.serial.write('%c' % duration)
+
     def calculate_angle(self):
         position = self.position
         if self.reverse:

--- a/servo/servo.ino	Sat Aug 23 20:32:26 2014 +0200
+++ b/servo/servo.ino	Sun Aug 24 14:27:09 2014 +0200
@@ -3,13 +3,11 @@
 */
 
 #include <Servo.h>
+#include <math.h>
 
 // Number of servos in legs.
 #define MAX_SERVO 12
 
-// Home position of the servos.
-#define HOME_POS 90
-
 // Range of the servos.
 #define SERVO_FREQ_MIN 600
 #define SERVO_FREQ_MAX 2400
@@ -17,22 +15,89 @@
 // 3.3V maximum for the acceleration sensor.
 #define ACCEL_MAX 676
 
+// How many acceleration measurements to cache.
+#define MAX_ACCEL_BUFFER 16
+
 // Speed of the serial port.
 #define SERIAL_SPEED 9600
 // 115200
 // 38400
 // 9600
 
+// Leg dimensions.
+#define LEG_COXA 22.5
+#define LEG_FEMUR 38.0
+#define LEG_TIBIA 30.0
+#define LEG_BASE 15.0
 
+
+int joints[12][2] {
+//  reverse, trim
+    {1, 50},
+    {-1, -50},
+    {1, 0},
+    {-1, 0},
+    {1, -80},
+    {-1, 0},
+    {1, 50},
+    {-1, 0},
+    {-1, 0},
+    {1, 0},
+    {1, 0},
+    {-1, 0},
+};
+int legs[4][5] = {
+//  hip, knee, ankle, x_direction, y_direction
+    {11, 4, 0, -1, -1},
+    {8, 7, 3, -1, 1},
+    {9, 6, 2, 1, 1},
+    {10, 5, 1, 1, -1},
+};
 Servo servos[MAX_SERVO];
 int serial_command = 0;
+int accel_buffer[MAX_ACCEL_BUFFER][3];
+int accel_buffer_index = 0;
+unsigned long last_tick = 0;
 
 
+void read_accel() {
+    accel_buffer[accel_buffer_index][0] = analogRead(A2);
+    accel_buffer[accel_buffer_index][1] = analogRead(A1);
+    accel_buffer[accel_buffer_index][2] = analogRead(A0);
+    accel_buffer_index = (accel_buffer_index + 1) % MAX_ACCEL_BUFFER;
+}
+
 int _serial_read_word() {
     /* Read two bytes from the serial port and make an int from them. */
     return Serial.read() | Serial.read() << 8;
 }
 
+double _norm(double a, double b) {
+    return sqrt(a * a + b * b);
+}
+
+double _triangle(double a, double b, double c) {
+    return facos((a * a + b * b - c * c) / (2 * a * b));
+}
+
+int _radians_to_microseconds(double angle) {
+    while (angle < 0) {
+        angle += 2;
+    }
+    return SERVO_FREQ_MIN + (SERVO_FREQ_MAX - SERVO_FREQ_MIN) * angle;
+}
+
+void move_leg(int leg, double x, double y, double z) {
+    double f = _norm(x, y) - LEG_COXA;
+    double d = _norm(f, z);
+    double hip = fatan2(y, x) + 0.25 * legs[leg][4];
+    double knee = fatan2(-z, f) - _triangle(LEG_FEMUR, d, LEG_TIBIA);
+    double ankle = _triangle(LEG_FEMUR, LEG_TIBIA, d) - 0.5;
+    servos[legs[leg][0]].writeMicroseconds(_radians_to_microseconds(hip));
+    servos[legs[leg][1]].writeMicroseconds(_radians_to_microseconds(knee));
+    servos[legs[leg][2]].writeMicroseconds(_radians_to_microseconds(ankle));
+}
+
 void do_servo_degrees() {
     /* Move a single servo by degrees. */
     int servo = Serial.read();
@@ -72,12 +137,31 @@
     }
 }
 
+void do_move_leg() {
+    /* Move the leg to a position in space. */
+    int leg = Serial.read();
+    int x = Serial.read() - 128;
+    int y = Serial.read() - 128;
+    int z = Serial.read() - 128;
+    if (leg >= 0 && leg <= 4) {
+        move_leg(leg, x, y, z);
+    }
+}
+
 void do_accel() {
     /* Report the readings of the acceleration sensor. */
+    int x = 0;
+    int y = 0;
+    int z = 0;
+    for (int i = 0; i < MAX_ACCEL_BUFFER; i += 1) {
+        x += accel_buffer[i][0];
+        y += accel_buffer[i][0];
+        z += accel_buffer[i][0];
+    }
     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));
+    Serial.write(map(x / MAX_ACCEL_BUFFER, 0, 676, 0, 255));
+    Serial.write(map(y / MAX_ACCEL_BUFFER, 0, 676, 0, 255));
+    Serial.write(map(z / MAX_ACCEL_BUFFER, 0, 676, 0, 255));
 }
 
 void do_beep() {
@@ -124,6 +208,13 @@
             }
             do_servos_microseconds();
             break;
+        case 0xfb:
+            /* Move a leg to a position in space. */
+            if (Serial.available() < 4) {
+                return;
+            }
+            do_move_leg();
+            break;
         case 0xf0:
             /* Read the accelerometer sensor. */
             do_accel();
@@ -146,12 +237,20 @@
     for (servo = 0; servo < MAX_SERVO; servo += 1) {
         /* First 2 pins are used for the serial. */
         servos[servo].attach(servo + 2, SERVO_FREQ_MIN, SERVO_FREQ_MAX);
-        servos[servo].write(HOME_POS);
+        servos[servo].writeMicroseconds(1500);
         /* Don't set all servos at once, save power. */
         delay(150);
     }
 }
 
 void loop() {
+    unsigned long now = millis();
+
     do_serial_command();
+/*
+    if (now > last_tick + 100 || now < last_tick) {
+        read_accel();
+        last_tick = now;
+    }
+*/
 }