--- a/kubik.py	Fri Sep 05 18:55:56 2014 +0200
+++ b/kubik.py	Wed Sep 10 20:37:27 2014 +0200
@@ -27,20 +27,20 @@
     def __init__(self, serial):
         self.legs = (
             Leg(Servo(serial, 11, reverse=True),
-                Servo(serial, 4, reverse=False, trim=-8),
-                Servo(serial, 0, reverse=False, trim=5),
+                Servo(serial, 4, reverse=False),
+                Servo(serial, 0, reverse=False),
                 (-1, -1)),
             Leg(Servo(serial, 8, reverse=True),
-                Servo(serial, 7, reverse=True, trim=0),
-                Servo(serial, 3, reverse=True, trim=0),
+                Servo(serial, 7, reverse=True),
+                Servo(serial, 3, reverse=True),
                 (-1, 1)),
             Leg(Servo(serial, 9, reverse=False),
-                Servo(serial, 6, reverse=False, trim=5),
+                Servo(serial, 6, reverse=False),
                 Servo(serial, 2, reverse=False),
                 (1, 1)),
             Leg(Servo(serial, 10, reverse=False),
                 Servo(serial, 5, reverse=True),
-                Servo(serial, 1, reverse=True, trim=-5),
+                Servo(serial, 1, reverse=True),
                 (1, -1)),
         )
         super(Robot, self).__init__()

--- a/servo/servo.ino	Fri Sep 05 18:55:56 2014 +0200
+++ b/servo/servo.ino	Wed Sep 10 20:37:27 2014 +0200
@@ -13,6 +13,7 @@
 // Range of the servos.
 #define SERVO_FREQ_MIN 600
 #define SERVO_FREQ_MAX 2400
+#define SERVO_PIN_MIN 2
 
 // 3.3V maximum for the acceleration sensor.
 #define ACCEL_MAX 676
@@ -35,14 +36,19 @@
 
 int joints[12][2] {
 //  reverse, trim
-    {1, 50},
+    // ankles
+    {1, -50},  // front right
+    {-1, 0}, // front left
+    {1, -50},   // hind left
+    {-1, 50},  // hind right
+
+    // knees
+    {1, -70},
     {-1, -50},
-    {1, 0},
-    {-1, 0},
-    {1, -80},
-    {-1, 0},
-    {1, 50},
-    {-1, 0},
+    {1, 30},
+    {-1, -50},
+
+    // hips
     {-1, 0},
     {1, 0},
     {1, 0},
@@ -50,10 +56,10 @@
 };
 int legs[4][5] = {
 //  hip, knee, ankle, x_direction, y_direction
-    {11, 4, 0, -1, -1},
-    {8, 7, 3, -1, 1},
-    {9, 6, 2, 1, 1},
-    {10, 5, 1, 1, -1},
+    {11, 4, 0, -1, -1},  // front right
+    {8, 7, 3, -1, 1},  // front left
+    {9, 6, 2, 1, 1},  // hind left
+    {10, 5, 1, 1, -1},  // hind right
 };
 Servo servos[MAX_SERVO];
 int serial_command = 0;
@@ -83,6 +89,13 @@
     return low | (high << 8);
 }
 
+void servo_write(int servo, int us) {
+    int trim = joints[servo][1];
+    int pos = min(max(us + trim, SERVO_FREQ_MIN), SERVO_FREQ_MAX);
+    servos[servo].writeMicroseconds(pos);
+}
+
+
 void do_servo_degrees() {
     /* Move a single servo by degrees. */
     int servo = Serial.read();
@@ -108,7 +121,7 @@
     int pos = _serial_read_word();
     if (servo >= 0 && servo < MAX_SERVO &&
         pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
-        servos[servo].writeMicroseconds(pos);
+        servo_write(servo, pos);
     }
 }
 
@@ -117,7 +130,7 @@
     for (int servo = 0; servo < MAX_SERVO; servo += 1) {
         int pos = _serial_read_word();
         if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
-            servos[servo].writeMicroseconds(pos);
+            servo_write(servo, pos);
         }
     }
 }
@@ -220,8 +233,9 @@
 void unfold() {
     for (int servo = MAX_SERVO - 1; servo >= 0; servo -= 1) {
         /* First 2 pins are used for the serial. */
-        servos[servo].attach(servo + 2, SERVO_FREQ_MIN, SERVO_FREQ_MAX);
-        servos[servo].writeMicroseconds(1500);
+        servos[servo].attach(servo + SERVO_PIN_MIN,
+                             SERVO_FREQ_MIN, SERVO_FREQ_MAX);
+        servo_write(servo, 1500);
         /* Don't set all servos at once, save power. */
         delay(150);
     }