--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trot.py	Wed Oct 01 17:50:29 2014 +0200
@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+class Creep(object):
+    step_height = 5.0  # How high to raise the legs.
+    stride = 16.0  # How long steps to take.
+    swing = 12, 9  # How much to shift the body x and y.
+    swing_speed = 3  # How fast to shift the body.
+    posture = 10, 16  # How much to change posture depending on direction.
+    leg_order = {
+        'forward': {None: (1, 3), 1: (0, 2), 0: (1, 3)},
+        'backward': {None: (1, 3), 1: (0, 2), 0: (1, 3)},
+        'left': {None: (1, 3), 1: (0, 2), 0: (1, 3)},
+        'right': {None: (1, 3), 1: (0, 2), 0: (1, 3)},
+    }
+
+    def __init__(self, robot):
+        self.robot = robot
+        self.on_ground = set(self.robot.legs)
+        self.speed = [0.0, 1.0]
+        self.rotation = 0.0
+        self.gait = 'forward'
+        self.continous = True
+
+    def _raise_leg(self, leg):
+        """Raises the leg off the ground."""
+        self.on_ground.remove(leg)
+        leg.move_to((None, None, self.step_height))
+        for frame in range(2):
+            yield
+
+    def _lower_leg(self, leg):
+        """Puts the leg on the ground."""
+        leg.move_to((None, None, leg.home[2] + 1))
+        for frame in range(2):
+            yield
+        leg.move_to((None, None, leg.home[2]))
+        self.on_ground.add(leg)
+        yield
+
+    def _advance_leg(self, leg):
+        """Moves the leg forward."""
+        leg.move_to((
+            (
+                leg.home[0] - self.stride * self.speed[0] * leg.x_dir -
+                self.posture[0] * abs(self.speed[0])
+            ),
+            (
+                leg.home[1] + self.stride * self.speed[1] +
+                leg.y_dir * self.posture[1] * abs(self.speed[1])
+            ),
+            None,
+        ))
+        leg.rotate_by(self.rotation * leg.x_dir * leg.y_dir * 10)
+        for frame in range(4):
+            yield
+
+    def stomp_anim(self):
+        """This animation is for making steps and shifting the body."""
+        leg_index = None
+        while True:
+            while not self.gait:
+                yield
+            leg_index, leg_index2 = self.leg_order[self.gait][leg_index]
+            leg = self.robot.legs[leg_index]
+            leg2 = self.robot.legs[leg_index2]
+            for frame, frame2 in zip(self._raise_leg(leg),
+                                     self._raise_leg(leg2)):
+                yield
+            for frame, frame2 in zip(self._advance_leg(leg),
+                                    self._advance_leg(leg2)):
+                yield
+            for frame, frame2 in zip(self._lower_leg(leg),
+                                    self._lower_leg(leg2)):
+                yield
+            for frame in range(10):
+                yield
+
+    def retract_anim(self):
+        """This animation is for moving the legs on the ground.
+
+        They should all move in the opposite direction than the speed.
+        """
+        while True:
+            if self.continous:
+                for leg in self.on_ground:
+                    leg.move_by((self.speed[0] * 0.5 * leg.x_dir,
+                                 -self.speed[1], 0))
+                    if self.rotation:
+                        leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir)
+            yield
+
+if __name__ == '__main__':
+    import serial
+    import kubik
+
+    FPS = 50
+
+    robot = kubik.Robot(serial.Serial('/dev/rfcomm0', 115200))
+    creep = Creep(robot)
+    robot.run_animations([
+        creep.stomp_anim(),
+        creep.retract_anim(),
+    ], FPS)