--- a/kubik.py	Tue Jul 01 23:00:58 2014 +0200
+++ b/kubik.py	Wed Jul 02 00:00:09 2014 +0200
@@ -54,6 +54,9 @@
         self.knee = knee
         self.ankle = ankle
         self.hip_offset = hip_offset  # angle that points outward
+        self.target = None
+        self.time_left = None
+        self.position = 0, 0, 0
 
     def __iter__(self):
         """Iterate over all joints."""
@@ -61,26 +64,51 @@
 
     def _inverse_kinematics(self, (x, y, z)):
         """Calculate the 3 angles of the leg, in radians."""
-        hip = math.atan2(y, x)
         f = _length(x, y)
         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, (x, y, z)):
+    def move_to(self, position):
         """Move the foot to the specified point."""
-        hip, knee, ankle = (
-            math.degrees(angle)
-            for angle in self._inverse_kinematics((x, y, z))
-        )
-        knee = _normalize_degrees(-knee)
-        ankle = _normalize_degrees(ankle - 90)
-        hip = _normalize_degrees(hip + self.hip_offset)
+        self.position = position
+        hip, knee, ankle = self._inverse_kinematics(position)
         self.hip.move_degrees(hip)
         self.knee.move_degrees(knee)
         self.ankle.move_degrees(ankle)
 
+    def set_target(self, target, time):
+        """Move to the specified point in specified time."""
+        self.target = target
+        self.time_left = time
+
+    def advance(self, dt):
+        """Advance the leg so that it will reach its target on time.
+
+        Returns True if the leg is already in the right place.
+        """
+        if not self.target or not self.time_left:
+            return True
+        finished = False
+        x1, y1, z1 = self.position
+        x2, y2, z2 = self.target
+        step = float(dt) / self.time_left
+        x = x1 + (x2 - x1) * step
+        y = y1 + (y2 - y1) * step
+        z = z1 + (z2 - z1) * step
+        self.time_left -= dt
+        if self.time_left <= 0:
+            x, y, z = self.target
+            self.time_left = 0
+            finished = True
+        self.move_to((x, y, z))
+        return finished
+
 
 class Robot(object):
     def __init__(self):
@@ -109,43 +137,13 @@
 
     def run(self):
         try:
+            for leg in self.legs:
+                leg.set_target((30, 0, -20))
             while True:
-                for i in range(-30, -20):
-                    time.sleep(0.0125)
-                    self.legs[0].move_to((20, 0, i))
-                    self.legs[1].move_to((20, 0, i))
-                    self.legs[2].move_to((20, 0, i))
-                    self.legs[3].move_to((20, 0, i))
-                for i in range(-20, -30, -1):
-                    time.sleep(0.0125)
-                    self.legs[0].move_to((20, 0, i))
-                    self.legs[1].move_to((20, 0, i))
-                    self.legs[2].move_to((20, 0, i))
-                    self.legs[3].move_to((20, 0, i))
-                for i in range(0, 20):
-                    time.sleep(0.0125)
-                    self.legs[0].move_to((20-i, 0, -30))
-                    self.legs[1].move_to((20-i, 0, -30))
-                    self.legs[2].move_to((20+i, 0, -30))
-                    self.legs[3].move_to((20+i, 0, -30))
-                for i in range(20, 0, -1):
-                    time.sleep(0.0125)
-                    self.legs[0].move_to((20-i, 0, -30))
-                    self.legs[1].move_to((20-i, 0, -30))
-                    self.legs[2].move_to((20+i, 0, -30))
-                    self.legs[3].move_to((20+i, 0, -30))
-                for i in range(0, 20):
-                    time.sleep(0.0125)
-                    self.legs[0].move_to((20, i, -30))
-                    self.legs[1].move_to((20, i, -30))
-                    self.legs[2].move_to((20, i, -30))
-                    self.legs[3].move_to((20, i, -30))
-                for i in range(20, 0, -1):
-                    time.sleep(0.0125)
-                    self.legs[0].move_to((20, i, -30))
-                    self.legs[1].move_to((20, i, -30))
-                    self.legs[2].move_to((20, i, -30))
-                    self.legs[3].move_to((20, i, -30))
+                dt = 0.00125
+                time.sleep(dt)
+                for leg in self.legs:
+                    leg.advance(dt)
         finally:
             self.home()