--- a/kubik.py	Wed Aug 06 18:31:07 2014 +0200
+++ b/kubik.py	Wed Aug 06 18:37:08 2014 +0200
@@ -3,6 +3,7 @@
 
 import maestro
 import leg
+import robot
 
 
 class HipServo(maestro.Servo):
@@ -37,13 +38,14 @@
     home = 45.0, 45.0, -35.0
 
 
-class Robot(object):
+class Robot(robot.Robot):
     def __init__(self):
+        super(Robot, self).__init__()
         s = serial.Serial('/dev/ttyAMA0', 115200)
         m = maestro.Maestro(s)
         m.init()
 
-        self.legs = [
+        self.legs = (
             Leg(HipServo(m, 1, reverse=True, trim=250),
                 KneeServo(m, 2, reverse=True, trim=-200),
                 AnkleServo1(m, 0, reverse=True, trim=0, center=1500 * 4),
@@ -60,68 +62,7 @@
                 KneeServo(m, 11, reverse=False, trim=-100),
                 AnkleServo2(m, 9, reverse=False, trim=100, center=1500 * 4),
                 (-1, -1)),
-        ]
-        self.shift = [0.0, 0.0, 0.0]
-
-    def shift_by(self, (dx, dy, dz), reverse=False):
-        if reverse:
-            dx, dy, dz = -dx, -dy, -dz
-        self.shift[0] += dx
-        self.shift[1] += dy
-        self.shift[2] += dz
-
-    def _home(self):
-        """Move all legs to their initial position."""
-        shifts = [
-            (2, -2, 0),
-            (2, 2, 0),
-            (-2, 2, 0),
-            (-2, -2, 0),
-        ]
-        for leg, shift in zip(self.legs, shifts):
-            x, y, z = leg.position
-            if (x, y) != (leg.home[0], leg.home[1]):
-                self.shift = shift
-                leg.move_to((None, None, 5))
-                for i in range(2):
-                    yield
-                leg.move_to((leg.home[0], leg.home[1], None))
-                for i in range(2):
-                    yield
-                leg.move_to((None, None, leg.home[2] + 1))
-                for i in range(4):
-                    yield
-            leg.move_to(leg.home)
-        self.shift = [0, 0, 0]
-
-    def home(self):
-        self._run_animations([self._home()], 40)
-
-    def update(self):
-        x, y, z = self.shift
-        for leg_left, leg_right in zip(self.legs[:2], self.legs[2:]):
-            leg_left.update((x, y, z))
-            leg_right.update((-x, y, z))
-
-    def _run_animations(self, animations, fps=12):
-        clock = pygame.time.Clock()
-        while animations:
-            for animation in list(animations):
-                try:
-                    new = next(animation)
-                    if new:
-                        animations.extend(new)
-                except StopIteration:
-                    animations.remove(animation)
-            self.update()
-            clock.tick(fps)
-
-    def run_animations(self, animations, fps=12):
-        try:
-            self._run_animations(animations, fps)
-        finally:
-            self.home()
-
+        )
 
 if __name__ == "__main__":
     robot = Robot()

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.py	Wed Aug 06 18:37:08 2014 +0200
@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import itertools
+
+import pygame
+
+
+class Robot(object):
+    legs = ()
+
+    def __init__(self):
+        self.shift = [0.0, 0.0, 0.0]
+
+    def _home(self):
+        """Move all legs to their initial position."""
+        for leg in self.legs:
+            x, y, z = leg.position
+            hx, hy, hz = leg.home
+            if (x, y) != (hx, hy):
+                leg.move_to((None, None, 5))
+                for i in range(4):
+                    yield
+                leg.move_to((hx, hy, None))
+                for i in range(4):
+                    yield
+                leg.move_to((None, None, hz + 1))
+                for i in range(4):
+                    yield
+            leg.move_to((None, None, hz))
+
+    def _unshift(self):
+        while self.shift != [0, 0, 0]:
+            for i in range(3):
+                if self.shift[i] >= 1:
+                    self.shift[i] -= 1
+                elif self.shift[i] <= -1:
+                    self.shift[i] += 1
+            yield
+        self.shift = [0, 0, 0]
+
+    def home(self):
+        self._run_animations([
+            self._home(),
+            self._unshift(),
+        ], 40)
+
+    def update(self):
+        for leg in self.legs:
+            leg.update(self.shift)
+
+    def _run_animations(self, animations, fps=12):
+        clock = pygame.time.Clock()
+        while animations:
+            for animation in list(animations):
+                try:
+                    new = next(animation)
+                    if new is not None:
+                        animations.remove(animation)
+                        animations.append(itertools.chain(new, animation))
+                except StopIteration:
+                    animations.remove(animation)
+            self.update()
+            clock.tick(fps)
+
+    def run_animations(self, animations, fps=12):
+        try:
+            self._run_animations(animations, fps)
+        finally:
+            self.home()