--- a/creep.py	Thu Aug 07 17:56:57 2014 +0200
+++ b/creep.py	Thu Aug 07 17:59:42 2014 +0200
@@ -1,62 +1,111 @@
-import kubik
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
 
-STRIDE = 36
-SPEED = 2
-SWING = 24
-SWING_SPEED = 4
-HEIGHT = -35
-STEP = -5
-OVERLAP = 0
-FPS = 24
+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, 1: 0, 0: 2, 2: 3, 3: 1},
+        'rotate_left': {None: 0, 0: 1, 1: 2, 2: 3, 3: 0},
+        'rotate_right': {None: 0, 0: 3, 3: 2, 2: 1, 1: 0},
+    }
 
+    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'
 
-def _swing(robot, delay, steps):
-    for i in range(delay):
-        yield
-    pause = STRIDE / SPEED + 3 - SWING * 2 / SWING_SPEED
-    for step in range(steps):
-        for i in range(pause):
+    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
-        for i in range(-SWING, SWING, SWING_SPEED):
-            robot.shift = [-i, 0, 0]
-            yield
-        for i in range(pause):
-            yield
-        for i in range(SWING, -SWING, -SWING_SPEED):
-            robot.shift = [-i, 0, 0]
+        leg.move_to((None, None, leg.home[2]))
+        self.on_ground.add(leg)
+
+    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 _creep(leg, side, delay, steps):
-    for i in range(delay):
-        yield
-    for step in range(steps):
-        leg.move_to((None, None, STEP))
-        for i in range(4):
-            yield
-        leg.move_to((None, STRIDE, None))
-        for i in range(2):
-            yield
-        leg.move_to((None, None, HEIGHT))
-        for y in range(STRIDE, -STRIDE, -SPEED):
-            leg.move_to((None, y, None))
+    def _shift_body(self, shift):
+        """Shifts the body to the specified position."""
+        shift = list(shift)
+        while self.robot.shift[:2] != shift:
+            for i in range(2):
+                if self.robot.shift[i] > shift[i]:
+                    self.robot.shift[i] -= self.swing_speed
+                elif self.robot.shift[i] < shift[i]:
+                    self.robot.shift[i] += self.swing_speed
             yield
 
+    def stomp_anim(self):
+        """This animation is for making steps and shifting the body."""
+        leg_index = None
+        while True:
+            while not self.gait:
+                # Don't raise the legs if not moving.
+                try:
+                    yield self._shift_body((0, 0)).next()
+                except StopIteration:
+                    yield
+            leg_index = self.leg_order[self.gait][leg_index]
+            leg = self.robot.legs[leg_index]
+            for frame in self._shift_body((leg.x_dir * self.swing[0],
+                                           -leg.y_dir * self.swing[1])):
+                yield
+            for frame in self._raise_leg(leg):
+                yield
+            for frame in self._advance_leg(leg):
+                yield
+            for frame in self._lower_leg(leg):
+                yield
 
-def creep(robot, steps):
-    delay = (2 * STRIDE / SPEED + 6) / 4
-    animations = [
-        _swing(robot, 0, steps),
-        _creep(robot.legs[0], 1, delay * 0 + OVERLAP, steps),
-        _creep(robot.legs[1], 1, delay * 1 - OVERLAP, steps),
-        _creep(robot.legs[3], -1, delay * 2 + OVERLAP, steps),
-        _creep(robot.legs[2], -1, delay * 3 - OVERLAP, steps),
-    ]
-    return animations
+    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:
+            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 kubik
+
+    FPS = 24
+
     robot = kubik.Robot()
-    robot.home()
-    animations = creep(robot, 10)
-    robot.run_animations(animations, FPS)
+    creep = Creep(robot)
+    robot.run_animations([
+        creep.stomp_anim(),
+        creep.retract_anim(),
+    ], FPS)

--- a/joy.py	Thu Aug 07 17:56:57 2014 +0200
+++ b/joy.py	Thu Aug 07 17:59:42 2014 +0200
@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import os
+
+import pygame
+
+import kubik
+import creep
+
+
+FPS = 24
+
+
+class Walk(creep.Creep):
+    def __init__(self, robot):
+        super(Walk, self).__init__(robot)
+        self.speed = [0.0, 0.0]
+        self.rotation = 0.0
+        self.gait = ''
+
+    def joy_anim(self):
+        """This animation is for reading user input."""
+        while True:
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT:
+                    raise SystemExit()
+                elif event.type == pygame.JOYBUTTONDOWN:
+                    if event.button == 10:
+                        self.gait = 'forward'
+                    elif event.button == 11:
+                        if self.rotation > 0:
+                            self.gait = 'rotate_left'
+                        else:
+                            self.gait = 'rotate_right'
+                    elif event.button == 0:
+                        serial = self.robot.legs[0].hip.serial
+                        serial.write('0xfd')
+                        sensor = ord(serial.read(1))
+                        print "Sensor value: %d" % sensor
+                    else:
+                        print "Button %d" % event.button
+                elif event.type == pygame.JOYBUTTONUP:
+                    if event.button in (10, 11):
+                        self.gait = ''
+                elif event.type == pygame.JOYAXISMOTION:
+                    if event.axis == 0:
+                        self.speed[0] = event.value
+                        self.rotation = 0.0
+                    elif event.axis == 1:
+                        self.speed[1] = -event.value
+                        self.rotation = 0.0
+                    elif event.axis == 2:
+                        self.robot.shift[2] = event.value * 12
+                    elif event.axis == 3:
+                        self.rotation = event.value
+                        self.speed = [0.0, 0.0]
+            yield
+
+
+
+if __name__ == '__main__':
+    # Initialize PyGame.
+    os.environ['SLD_VIDEODRIVER'] = 'dummy'
+    pygame.display.init()
+    pygame.joystick.init()
+    for joy in range(pygame.joystick.get_count()):
+        pygame.joystick.Joystick(joy).init()
+
+    # Initialize the robot.
+    robot = kubik.Robot()
+    robot.home()
+    walk = Walk(robot)
+    clock = pygame.time.Clock()
+    robot.run_animations([
+        walk.stomp_anim(),
+        walk.retract_anim(),
+        walk.joy_anim(),
+    ], FPS)