--- a/python/gait.py	Thu Apr 07 09:15:45 2016 +0200
+++ b/python/gait.py	Thu Apr 07 09:40:26 2016 +0200
@@ -2,26 +2,18 @@
 
 
 class Creep:
-    speed = 0.75
+    speed = 1
     stride = 14
     shift = 8
     shift_steps = 2
     raise_leg = 10
-    raise_steps = 2
-    next_leg = (
-        (2, 0, 3, 1),   # forward
-        (1, 3, 0, 2),   # backward
-        (1, 2, 3, 0),   # clockwise
-        (3, 0, 1, 2),   # counter-clockwise
-        (2, 3, 1, 0),   # left
-        (3, 2, 0, 1),   # right
-    )
+    raise_steps = 3
 
     def __init__(self, robot):
         self.robot = robot
         self.shift_x = 0
         self.shift_y = 0
-        self.on_ground = list(robot.legs)
+        self.on_ground = set(robot.legs)
         self.dx = 0
         self.dy = 1
         self.rotation = 0
@@ -34,9 +26,7 @@
             dx = self.dx * leg.x_dir * self.speed
             dy = self.dy * leg.y_dir * self.speed
             leg.move_to(leg.x - dx, leg.y - dy, -self.height)
-            #leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
-        self.robot.update()
-        delay(250)
+            leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
 
     def step(self, leg):
         dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps
@@ -45,26 +35,48 @@
             self.robot.move_by(dx, dy, 0)
             self.shift_x += dx
             self.shift_y += dy
-            self.move()
+            yield
         self.on_ground.remove(leg)
         for step in range(self.raise_steps):
             leg.move_by(0, 0, self.raise_leg)
-            self.move()
+            yield
         leg.move_to(
             (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread,
             (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread,
             self.raise_leg * self.raise_steps - self.height
         )
-        self.move()
-        self.move()
+        yield
+        yield
         for step in range(self.raise_steps - 1):
             leg.move_by(0, 0, -self.raise_leg)
-            self.move()
-        self.on_ground.append(leg)
-        self.move()
+            yield
+        self.on_ground.add(leg)
+        yield
+
+    def step_order(self):
+        if abs(self.dy) > abs(self.dx):
+            if self.dy > 0:
+                return (2, 0, 3, 1)   # forward
+            else:
+                return (1, 3, 0, 2)   # backward
+        elif abs(self.dx) > abs(self.dy):
+            if self.dx < 0:
+                return (2, 3, 1, 0)   # left
+            else:
+                return (3, 2, 0, 1)   # right
+        elif abs(self.rotation) > 0.001:
+            if self.rotation > 0:
+                return (1, 2, 3, 0)   # clockwise
+            else:
+                return (3, 0, 1, 2)   # counter-clockwise
+        return (2, 0, 3, 1)   # forward
+
 
     def run(self, steps):
         for step in range(steps):
             leg = self.robot.legs[self.last_leg]
-            self.step(leg)
-            self.last_leg = self.next_leg[0][self.last_leg]
+            for frame in self.step(leg):
+                self.move()
+                self.robot.update()
+                delay(40) # 25 FPS
+            self.last_leg = self.step_order()[self.last_leg]