--- a/kubik.py	Wed Jul 02 00:18:57 2014 +0200
+++ b/kubik.py	Wed Jul 02 09:23:31 2014 +0000
@@ -45,18 +45,16 @@
 
 class Leg(object):
     # Lengths of leg segments, in mm
-    coxa = 13  # between the hip and the knee
-    femur = 30  # between the knee and the ankle
-    tibia = 36  # between the ankle and the foot
+    coxa = 12.5  # between the hip and the knee
+    femur = 30.0  # between the knee and the ankle
+    tibia = 35.0  # between the ankle and the foot
 
     def __init__(self, hip, knee, ankle, hip_offset=0):
         self.hip = hip
         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
+        self.position = 30.0, 30.0, -35.0
 
     def __iter__(self):
         """Iterate over all joints."""
@@ -82,36 +80,6 @@
         self.knee.move_degrees(knee)
         self.ankle.move_degrees(ankle)
 
-    def set_target(self, target, time=0):
-        """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:
-            return True
-        if self.time_left == 0:
-            self.move_to(self.target)
-            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):
@@ -120,33 +88,68 @@
         m.init()
 
         self.legs = [
-            Leg(HipServo(m, 1), KneeServo(m, 2),
-                AnkleServo(m, 0), 45),
-            Leg(HipServo(m, 4), KneeServo(m, 5, reverse=True),
-                AnkleServo(m, 3, reverse=True), -45),
-            Leg(HipServo(m, 7, reverse=True), KneeServo(m, 8),
-                AnkleServo(m, 6), -45),
-            Leg(HipServo(m, 10, reverse=True), KneeServo(m, 11, reverse=True),
-                AnkleServo(m, 9, reverse=True), 45),
+            Leg(HipServo(m, 1, trim=250),
+                KneeServo(m, 2, trim=-200),
+                AnkleServo(m, 0, trim=0),
+                45),
+            Leg(HipServo(m, 4, trim=-100),
+                KneeServo(m, 5, reverse=True, trim=-200),
+                AnkleServo(m, 3, reverse=True, trim=-400),
+                -45),
+            Leg(HipServo(m, 7, reverse=True, trim=150),
+                KneeServo(m, 8, trim=100),
+                AnkleServo(m, 6, trim=0),
+                -45),
+            Leg(HipServo(m, 10, reverse=True, trim=-250),
+                KneeServo(m, 11, reverse=True, trim=-100),
+                AnkleServo(m, 9, reverse=True, trim=100),
+                45),
         ]
 
     def home(self):
         """Move all legs to the initial position."""
         for leg in self.legs:
-            for joint in leg:
-                joint.set_speed(0)
-                joint.set_accel(0)
-                joint.move_degrees(0)
+            leg.move_to((30, 30, -35))
 
     def run(self):
         try:
-            for leg in self.legs:
-                leg.set_target((30, 0, -20))
             while True:
-                dt = 0.00125
-                time.sleep(dt)
-                for leg in self.legs:
-                    leg.advance(dt)
+                for i in range(-35, -25):
+                    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(-25, -35, -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, -35))
+                    self.legs[1].move_to((20-i, 0, -35))
+                    self.legs[2].move_to((20+i, 0, -35))
+                    self.legs[3].move_to((20+i, 0, -35))
+                for i in range(20, 0, -1):
+                    time.sleep(0.0125)
+                    self.legs[0].move_to((20-i, 0, -35))
+                    self.legs[1].move_to((20-i, 0, -35))
+                    self.legs[2].move_to((20+i, 0, -35))
+                    self.legs[3].move_to((20+i, 0, -35))
+                for i in range(0, 20):
+                    time.sleep(0.0125)
+                    self.legs[0].move_to((20, i, -35))
+                    self.legs[1].move_to((20, i, -35))
+                    self.legs[2].move_to((20, i, -35))
+                    self.legs[3].move_to((20, i, -35))
+                for i in range(20, 0, -1):
+                    time.sleep(0.0125)
+                    self.legs[0].move_to((20, i, -35))
+                    self.legs[1].move_to((20, i, -35))
+                    self.legs[2].move_to((20, i, -35))
+                    self.legs[3].move_to((20, i, -35))
         finally:
             self.home()