--- a/kubik.py	Thu Jul 31 17:07:08 2014 +0200
+++ b/kubik.py	Wed Aug 06 18:10:24 2014 +0200
@@ -1,31 +1,8 @@
-import math
-
 import serial
-import maestro
 import pygame
 
-
-def _solve_triangle(a, b, c):
-    """Calculate an angle of a triangle using the law of cosines."""
-    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 _length(a, b):
-    """Calculate the length of a vector."""
-    return math.sqrt(a ** 2 + b ** 2)
-
-
-def _normalize_degrees(angle):
-    """Convert an angle to range +-180 degree range."""
-    while angle > 180:
-        angle -= 360
-    while angle < -180:
-        angle += 360
-    return angle
+import maestro
+import leg
 
 
 class HipServo(maestro.Servo):
@@ -51,11 +28,7 @@
     max_degrees = 136
 
 
-class Leg(object):
-    """A signle leg, containing 3 servos: hip, knee and ankle, and three
-       connections between them: coxa, femur and tibia.
-    """
-
+class Leg(leg.Leg):
     # Lengths of leg segments, in mm
     coxa = 16.0  # between the hip and the knee
     femur = 50.0  # between the knee and the ankle
@@ -63,80 +36,6 @@
     base = 45.0, 45.0  # from the center of body
     home = 45.0, 45.0, -35.0
 
-    def __init__(self, hip, knee, ankle, front=True):
-        self.hip = hip
-        self.knee = knee
-        self.ankle = ankle
-        self.front = front
-        if self.front:
-            self.y_direction = 1
-        else:
-            self.y_direction = -1
-        self.hip_offset = -45 * self.y_direction
-        self.home = (
-            self.home[0], self.home[1] * self.y_direction, self.home[2])
-        self.position = list(self.home)
-
-    def __repr__(self):
-        return "<Leg (%r, %r, %r)>" % (
-            self.position[0],
-            self.position[1],
-            self.position[2],
-        )
-
-    def __iter__(self):
-        """Iterate over all joints."""
-        return iter([self.hip, self.knee, self.ankle])
-
-    def _inverse_kinematics(self, (x, y, z)):
-        """Calculate the 3 angles of the leg, in radians."""
-        f = _length(x, y) - self.coxa
-        d = min(self.femur + self.tibia, _length(f, z))
-        hip = math.atan2(y, x)
-        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
-        ankle = _solve_triangle(self.femur, self.tibia, d)
-        knee = _normalize_degrees(-math.degrees(knee))
-        ankle = _normalize_degrees(math.degrees(ankle) - 90)
-        hip = _normalize_degrees(math.degrees(hip) + self.hip_offset)
-        return hip, knee, ankle
-
-    def move_to(self, position):
-        """Move the foot to the specified point.
-
-        If any of the coordinates is None, no movement will be done in that
-        axis.
-        """
-        position = list(position)
-        for i, coord in enumerate(position):
-            if coord is None:
-                position[i] = self.position[i]
-        self.position = position
-
-    def move_by(self, (dx, dy, dz)):
-        """Move the foot by the specified amount."""
-        x, y, z = self.position
-        self.move_to((x + dx, y + dy, z + dz))
-
-    def rotate(self, degrees):
-        """Rotate the leg around the center of the body horizontally."""
-        radians = math.radians(degrees * self.y_direction)
-        x, y, z = self.position
-        bx = self.base[0] + x
-        by = self.base[1] + y * self.y_direction
-        r = _length(bx, by)
-        a = math.atan2(bx, by)
-        rx = r * math.sin(a + radians) - self.base[0]
-        ry = r * math.cos(a + radians) - self.base[1]
-        self.move_to((rx, ry * self.y_direction, None))
-
-    def update(self, shift):
-        dx, dy, dz = shift
-        x, y, z = self.position
-        hip, knee, ankle = self._inverse_kinematics((x + dx, y + dy, z + dz))
-        self.hip.move_degrees(hip)
-        self.knee.move_degrees(knee)
-        self.ankle.move_degrees(ankle)
-
 
 class Robot(object):
     def __init__(self):