--- a/nkubik/nkubik.ino	Mon Oct 13 23:31:07 2014 +0200
+++ b/nkubik/nkubik.ino	Tue Oct 21 00:10:54 2014 +0200
@@ -4,6 +4,8 @@
 void setup() {
     Serial.begin(115200);
     servo_setup();
+    sensor_setup();
+    beep_setup();
 }
 
 
@@ -19,6 +21,14 @@
     return value;
 }
 
+void _serial_write_word(int16_t value) {
+    /* Take an int and send it through the serial port as two bytes. */
+    int16_t high = (value & 0xff00) >> 8;
+    int16_t low = value & 0xff;
+    Serial.write(high);
+    Serial.write(low);
+}
+
 void do_serial_command() {
     static unsigned char serial_command = 0;
     /* Read and execute a command from the serial port's buffer. */
@@ -36,6 +46,19 @@
                 servo_move(servo, pos / 1000.0);
             }
             break;
+        case 0xfe:
+            if (Serial.available() < 2) {
+                return;
+            } else {
+                unsigned int frequency = Serial.read() << 3;
+                unsigned int duration = Serial.read() << 3;
+                beep(frequency, duration);
+            }
+            break;
+        case 0xfd:
+            Serial.write(0xff);
+            _serial_write_word(sensor_read());
+            break;
     }
     serial_command = 0;
 }

--- a/robot.py	Mon Oct 13 23:31:07 2014 +0200
+++ b/robot.py	Tue Oct 21 00:10:54 2014 +0200
@@ -48,9 +48,11 @@
 
     def move_leg(self, leg, (x, y)):
         self.leg_positions[leg] = x, y
-        y = y * self.legs[leg][2]
+        y *= self.legs[leg][2]
         hip = math.pi / 4 - math.atan2(y, x)
         knee = -math.asin((_norm(x, y) - self.coxa) / self.tibia)
+        #knee = math.atan2(self.tibia, _norm(x, y) - self.coxa) - math.pi / 2
+        print hip, knee
         self.move_servo(self.legs[leg][1], hip)
         self.move_servo(self.legs[leg][0], knee)
 
@@ -69,6 +71,6 @@
     r.update()
     for i in range(20):
         for leg in range(4):
-            r.move_leg_by(leg, (-r.legs[leg][3] * 2, 2))
+            r.move_leg_by(leg, (-r.legs[leg][3] * 0, 1))
         r.update()
-        time.sleep(0.2)
+        time.sleep(0.1)