--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/python/robot.py	Mon May 15 16:36:08 2017 +0200
@@ -0,0 +1,97 @@
+# -*- coding: utf-8 -*-
+
+import math
+
+from servos import Servos
+from machine import I2C, Pin
+
+
+PI2 = math.pi / 2
+PI4 = math.pi / 4
+SQRT2 = math.sqrt(2)
+
+
+def _solve_triangle(a, b, c):
+    a, b, c = abs(a), abs(b), abs(c)
+    if a + b < c or a + c < b or b + c < a:
+        raise ValueError("Impossible triangle: %r, %r, %r" % (a, b, c))
+    cos = float(a ** 2 + b ** 2 - c ** 2) / (2 * a * b)
+    return math.acos(cos)
+
+
+def _norm(a, b):
+    return math.sqrt(a ** 2 + b ** 2)
+
+
+class Leg:
+    # Lengths of leg segments, in mm
+    coxa =  36    # between the hip and the knee
+    femur = 39    # between the knee and the ankle
+    tibia = 55    # between the ankle and the foot
+    base =  21    # from the center of body
+
+    def __init__(self, joints, direction, servos):
+        self.servos = servos
+        self.joints = joints
+        self.x_dir, self.y_dir = direction
+        self.home = (self.coxa + self.femur) / math.sqrt(2)
+        self.x = 0
+        self.y = 0
+        self.z = 0
+
+    def move_to(self, x, y, z):
+        print("Moving leg %d, %d to %d, %d, %d" % (self.x_dir, self.y_dir,
+            x, y, z))
+        x += self.home
+        y += self.home
+        z -= self.tibia
+        f = _norm(x, y) - self.coxa
+        d = _norm(f, z)
+        if d > self.femur + self.tibia:
+            raise ValueError("Out of reach")
+        hip = PI4 - math.atan2(y, x)
+        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
+        ankle = PI2 - _solve_triangle(self.femur, self.tibia, d)
+        for joint, angle in zip(self.joints, (hip, knee, ankle)):
+            self.servos.position(
+                joint,
+                radians=PI2 - angle * self.x_dir * self.y_dir
+            )
+        self.x = x - self.home
+        self.y = y - self.home
+        self.z = z + self.tibia
+
+    def move_by(self, dx=0, dy=0, dz=0):
+        self.move_to(self.x + dx, self.y + dy, self.z + dz)
+
+    def rotate_by(self, radians):
+        x = self.x + self.base
+        y = self.y + self.base
+        nx = x * math.cos(radians) - y * math.sin(radians) - self.base
+        ny = x * math.sin(radians) + y * math.cos(radians) - self.base
+        self.move_to(nx, ny, self.z)
+
+
+class Robot:
+    def __init__(self):
+        i2c = I2C(-1, Pin(5, Pin.OUT, Pin.PULL_UP),
+                  Pin(4, Pin.OUT, Pin.PULL_UP), freq=100000)
+        self.servos = Servos(i2c)
+        self.legs = (
+            Leg((0, 1, 2), (1,  -1), self.servos),
+            Leg((7, 6, 5), (-1,  -1), self.servos),
+            Leg((8, 9, 10), (-1,  1), self.servos),
+            Leg((15, 14, 13), (1,  1), self.servos),
+        )
+
+    def move_by(self, dx=0, dy=0, dz=0):
+        for leg in self.legs:
+            leg.move_by(dx * leg.x_dir, dy * leg.y_dir, dz)
+
+    def rotate_by(self, radians):
+        for leg in self.legs:
+            leg.rotate_by(radians * leg.x_dir * leg.y_dir)
+
+    def home(self):
+        for leg in self.legs:
+            leg.move_to(0, 0, 0)