--- a/wiggle.py	Wed Sep 10 21:23:01 2014 +0200
+++ b/wiggle.py	Wed Sep 10 21:50:01 2014 +0200
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 
-import random
+import math
 
 import serial
 import kubik
@@ -10,32 +10,44 @@
 FPS = 6
 
 
-def wiggle(robot):
-    while True:
-        for leg in robot.legs:
-            leg.move_to((
-                leg.home[0] + random.randint(-30, 10),
-                leg.home[1] + random.randint(-30, 10),
-                leg.home[2] + random.randint(-40, 20),
-            ))
-        for wait in range(3):
-            yield;
 
 def watch(robot):
     state = 0
+    count = 0
     avg_accel, min_accel, max_accel = robot.get_accel()
     while True:
-        wiggler = wiggle(robot)
         avg_accel, min_accel, max_accel = robot.get_accel()
-        print avg_accel, min_accel, max_accel
-        print state
+        total_avg_accel = math.sqrt(sum(a**2 for a in avg_accel))
+        if total_avg_accel > 30:
+            pick = 1
+        elif total_avg_accel < 25:
+            pick = -1
+        else:
+            pick = 0
+        print pick, state
         if state == 0:
-            if max_accel[2] > 30 and min_accel[2] < 24:
+            if pick == 1:
                 state = 1
+                for leg in robot.legs:
+                    leg.move_to((40, -40 * leg.y_dir, 20));
         elif state == 1:
-            next(wiggler)
-            if max_accel[2] < 28 and min_accel[2] > 26:
+            if pick == 0:
+                count += 1
+            else:
+                count -= 1
+            if count > 10:
+                state = 2
+                count = 0
+        elif state == 2:
+            for leg in robot.legs:
+                leg.move_to(leg.home);
+            state = 3
+            count = 0
+        elif state == 3:
+            count += 1
+            if count > 10:
                 state = 0
+                count = 0
         yield