--- a/nkubik/leg.h	Mon Sep 29 21:33:19 2014 +0200
+++ b/nkubik/leg.h	Mon Oct 13 23:31:07 2014 +0200
@@ -1,10 +1,18 @@
 #ifndef LEG_H
 #define LEG_H
 
-const unsigned char LEG_HIP[4] =  {1, 6, 3, 4};
-const unsigned char LEG_KNEE[4] = {0, 7, 2, 5};
-const char LEG_X[4] =             {-1, -1, 1, 1};
-const char LEG_Y[4] =             {1, -1, 1, -1};
+/*
+ * Legs:
+ * 0 -- front left
+ * 1 -- front right
+ * 2 -- hind left
+ * 3 -- hind right
+ */
+
+const unsigned char LEG_KNEE[4] =  {0, 3, 1, 2};
+const unsigned char LEG_HIP[4] = {6, 7, 4, 5};
+const char LEG_X[4] =             {1, -1, 1, -1};
+const char LEG_Y[4] =             {1, 1, -1, -1};
 const double COXA = 38.0; // mm
 const double TIBIA = 38.0; // mm
 const double HOME_X = 26.870057685088803;

--- a/nkubik/leg.ino	Mon Sep 29 21:33:19 2014 +0200
+++ b/nkubik/leg.ino	Mon Oct 13 23:31:07 2014 +0200
@@ -1,4 +1,5 @@
 #include "misc.h"
+#include "leg.h"
 
 
 double _solve_triangle(double a, double b, double c) {
@@ -18,6 +19,8 @@
 bool _inverse_kinematics(double x, double y, double *hip, double *knee) {
     // Calculate angles for knee and ankle, and put them in those variables.
     // Return true on success, and false if x and y are out of range.
+    *hip = PI4 - atan2(y, x);
+    *knee = 0; //asin((_norm(x, y) - COXA)/TIBIA);
     return true;
 }
 

--- a/nkubik/misc.h	Mon Sep 29 21:33:19 2014 +0200
+++ b/nkubik/misc.h	Mon Oct 13 23:31:07 2014 +0200
@@ -3,6 +3,7 @@
 
 #define NaN (0.0/0.0)
 const double PI2 = PI / 2;
+const double PI4 = PI / 4;
 const double TAU = PI * 2;
 
 #endif

--- a/nkubik/nkubik.ino	Mon Sep 29 21:33:19 2014 +0200
+++ b/nkubik/nkubik.ino	Mon Oct 13 23:31:07 2014 +0200
@@ -1,15 +1,41 @@
 #include "servos.h"
-#include "leg.h"
-#include "clock.h"
 
 
 void setup() {
+    Serial.begin(115200);
     servo_setup();
-    Serial.begin(9600);
-    Serial.println("Started.");
 }
 
 
 void loop() {
-    tick(TICK);
+    do_serial_command();
+}
+
+int16_t _serial_read_word() {
+    /* Read two bytes from the serial port and make an int from them. */
+    int16_t high = Serial.read();
+    int16_t low = Serial.read();
+    int16_t value = high << 8 | low;
+    return value;
 }
+
+void do_serial_command() {
+    static unsigned char serial_command = 0;
+    /* Read and execute a command from the serial port's buffer. */
+    switch (serial_command) {
+        case 0x00:
+            /* No command, read one. */
+            if (Serial.available() < 1) { return; }
+            serial_command = Serial.read();
+            return;
+        case 0xff:
+            /* Move all servos by 1/1000 of radians. */
+            if (Serial.available() < SERVOS * 2) { return; }
+            for (unsigned char servo = 0; servo < SERVOS; ++servo) {
+                double pos = _serial_read_word();
+                servo_move(servo, pos / 1000.0);
+            }
+            break;
+    }
+    serial_command = 0;
+}

--- a/nkubik/servos.ino	Mon Sep 29 21:33:19 2014 +0200
+++ b/nkubik/servos.ino	Mon Oct 13 23:31:07 2014 +0200
@@ -6,9 +6,10 @@
 #define SERVO_FREQ_MIN 600
 #define SERVO_FREQ_MAX 2400
 
-static const unsigned char SERVO_PINS[SERVOS] = { 6, 7, 8, 9, 10, 11, 12, 14};
-static const char SERVO_REVERSE[SERVOS] =       { 1, 1, 1, 1,  1,  1,  1,  1};
-static const char SERVO_TRIM[SERVOS] =          { 0, 0, 0, 0,  0,  0,  0,  0};
+// Since we have a LED on pin 13, we use pin 5 instead.
+static const unsigned char SERVO_PINS[SERVOS] = { 6, 7, 8, 9, 10, 11, 12, 5};
+static const char SERVO_REVERSE[SERVOS] =       {1, -1, 1, -1,  1, -1, -1, 1};
+static const char SERVO_TRIM[SERVOS] =          {5, 0, 0, 0,  0,  0,  0, 0};
 static Servo servos[SERVOS];
 
 
@@ -21,7 +22,7 @@
     while (rads > TAU) {
         rads -= TAU;
     }
-    while (rads < TAU) {
+    while (rads < 0) {
         rads += TAU;
     }
     int freq = (SERVO_FREQ_MAX -

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.py	Mon Oct 13 23:31:07 2014 +0200
@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import math
+import struct
+import time
+
+import serial
+
+
+PORT = '/dev/ttyUSB0'
+
+
+def _norm(*args):
+    return math.sqrt(sum(a**2 for a in args))
+
+class Robot(object):
+    legs = [
+        (0, 6, 1, -1),  # fl
+        (3, 7, 1, 1),   # fr
+        (1, 4, -1, -1), # hl
+        (2, 5, -1, 1),  # hr
+    ]
+    coxa = 38.0  # mm
+    tibia = 38.0  # mm
+    home = 26.870057685088803, 26.870057685088803
+
+    def __init__(self):
+        self.serial = serial.Serial(PORT, 115200, timeout=1)
+        self.positions = [0] * 8
+        self.leg_positions = [
+            (self.home[0], self.home[1] * self.legs[leg][2])
+            for leg in range(4)
+        ]
+
+    def update(self):
+        format = '>B' + 'h' * 8
+        command = 0xff
+        positions = [int(pos * 1000) for pos in self.positions]
+        packet = struct.pack(format, command, *positions)
+        self.serial.write(packet)
+
+    def move_servo(self, servo, radians=None, degrees=None):
+        if degrees is not None:
+            radians = math.radians(degrees)
+        if radians is not None:
+            self.positions[servo] = radians
+
+    def move_leg(self, leg, (x, y)):
+        self.leg_positions[leg] = x, y
+        y = y * self.legs[leg][2]
+        hip = math.pi / 4 - math.atan2(y, x)
+        knee = -math.asin((_norm(x, y) - self.coxa) / self.tibia)
+        self.move_servo(self.legs[leg][1], hip)
+        self.move_servo(self.legs[leg][0], knee)
+
+    def move_leg_by(self, leg, (dx, dy)):
+        x, y = self.leg_positions[leg]
+        self.move_leg(leg, (x + dx, y + dy))
+
+    def move_home(self):
+        for leg in range(4):
+            self.move_leg(leg, (self.home[0],
+                                self.home[1] * self.legs[leg][2]))
+
+if __name__ == '__main__':
+    r = Robot()
+    r.move_home()
+    r.update()
+    for i in range(20):
+        for leg in range(4):
+            r.move_leg_by(leg, (-r.legs[leg][3] * 2, 2))
+        r.update()
+        time.sleep(0.2)