--- a/kubik.py	Fri Aug 22 10:08:53 2014 +0200
+++ b/kubik.py	Fri Aug 22 13:32:53 2014 +0200
@@ -20,7 +20,7 @@
 
 
 class Robot(robot.Robot):
-    accel_trim = [91.75, 89.5, 90.5]
+    accel_trim = [139, 136, 142]
 
     def __init__(self, serial):
         self.legs = (

--- a/robot.py	Fri Aug 22 10:08:53 2014 +0200
+++ b/robot.py	Fri Aug 22 13:32:53 2014 +0200
@@ -8,7 +8,7 @@
 
 class Robot(object):
     legs = ()
-    accel_trim = [90.0, 90.0, 90.0]
+    accel_trim = [128.0, 128.0, 128.0]
 
     def __init__(self):
         self.shift = [0.0, 0.0, 0.0]

--- a/servo.py	Fri Aug 22 10:08:53 2014 +0200
+++ b/servo.py	Fri Aug 22 13:32:53 2014 +0200
@@ -36,7 +36,7 @@
         serial.write(command)
 
     def get_sensors(self):
-        command = '\xfd'
+        command = '\xf0'
         self.serial.write(command)
         while self.serial.read(1) != '\xff':
             pass

--- a/servo/servo.ino	Fri Aug 22 10:08:53 2014 +0200
+++ b/servo/servo.ino	Fri Aug 22 13:32:53 2014 +0200
@@ -10,10 +10,12 @@
 // 38400
 // 9600
 
+
 Servo servos[MAX_SERVO];
 int servo;
 int pos;
-int command;
+int command = 0;
+
 
 void setup() {
     Serial.begin(SERIAL_SPEED);
@@ -26,38 +28,61 @@
     }
 }
 
+
 void loop() {
-    if (!(command = Serial.read()) & 0x80) {
-        /* Synchronize to the beginning of the command. */
-        return;
-    };
     switch (command) {
+        case 0x00:
+            /* No command, read one. */
+            if (Serial.available() < 1) {
+                return;
+            }
+            command = Serial.read();
+            break;
         case 0xff:
-            while ((servo = Serial.read()) == -1) {
-                /* Just keep trying. */
+            /* Move a single servo by degrees. */
+            if (Serial.available() < 2) {
+                return;
             }
-            while ((pos = Serial.read()) == -1) {
-                /* Just keep trying. */
-            }
+            servo = Serial.read();
+            pos = Serial.read();
             if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) {
                 servos[servo].write(pos);
             }
+            command = 0;
             break;
         case 0xfe:
+            /* Move all servos by degrees. */
+            if (Serial.available() < MAX_SERVO) {
+                return;
+            }
             for (servo = 0; servo < MAX_SERVO; servo += 1) {
-                while ((pos = Serial.read()) == -1) {
-                    /* Just keep trying. */
-                }
+                pos = Serial.read();
                 if (pos >= 0 && pos <= 180) {
                     servos[servo].write(pos);
                 }
             }
+            command = 0;
             break;
-        case 0xfd:
+        case 0xfc:
+            /* Move all servos by microseconds. */
+            if (Serial.available() < MAX_SERVO * 2) {
+                return;
+            }
+            for (servo = 0; servo < MAX_SERVO; servo += 1) {
+                pos = Serial.read() | Serial.read() << 8;
+                if (pos >= 600 && pos <= 2400) {
+                    servos[servo].writeMicroseconds(pos);
+                }
+            }
+            command = 0;
+            break;
+        case 0xf0:
+            /* Read the accelerometer sensor. */
             Serial.write(0xff);
-            Serial.write(map(analogRead(A0), 0, 1023, 0, 255));
-            Serial.write(map(analogRead(A1), 0, 1023, 0, 255));
-            Serial.write(map(analogRead(A2), 0, 1023, 0, 255));
+            Serial.write(map(analogRead(A2), 0, 676, 0, 255));
+            Serial.write(map(analogRead(A1), 0, 676, 0, 255));
+            Serial.write(map(analogRead(A0), 0, 676, 0, 255));
+            command = 0;
             break;
     }
 }