--- a/creep.py	Fri Aug 22 14:22:20 2014 +0200
+++ b/creep.py	Sat Aug 23 20:17:19 2014 +0200
@@ -19,6 +19,7 @@
         self.speed = [0.0, 1.0]
         self.rotation = 0.0
         self.gait = 'forward'
+        self.continous = True
 
     def _raise_leg(self, leg):
         """Raises the leg off the ground."""
@@ -54,6 +55,16 @@
 
     def _shift_body(self, shift):
         """Shifts the body to the specified position."""
+        if not self.continous:
+            for leg in self.on_ground:
+                leg.move_by((
+                    self.speed[0] * 0.5 * leg.x_dir * self.stride / 2,
+                    -self.speed[1] * self.stride / 2,
+                    0,
+                ))
+                if self.rotation:
+                    leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir *
+                                  self.stride / 2)
         shift = list(shift)
         while self.robot.shift[:2] != shift:
             for i in range(2):
@@ -91,18 +102,19 @@
         They should all move in the opposite direction than the speed.
         """
         while True:
-            for leg in self.on_ground:
-                leg.move_by((self.speed[0] * 0.5 * leg.x_dir,
-                             -self.speed[1], 0))
-                if self.rotation:
-                    leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir)
+            if self.continous:
+                for leg in self.on_ground:
+                    leg.move_by((self.speed[0] * 0.5 * leg.x_dir,
+                                 -self.speed[1], 0))
+                    if self.rotation:
+                        leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir)
             yield
 
 if __name__ == '__main__':
     import serial
     import kubik
 
-    FPS = 24
+    FPS = 32
 
     robot = kubik.Robot(serial.Serial('/dev/rfcomm0', 9600))
     creep = Creep(robot)

--- a/servo/servo.ino	Fri Aug 22 14:22:20 2014 +0200
+++ b/servo/servo.ino	Sat Aug 23 20:17:19 2014 +0200
@@ -3,8 +3,21 @@
 */
 
 #include <Servo.h>
+
+// Number of servos in legs.
 #define MAX_SERVO 12
+
+// Home position of the servos.
 #define HOME_POS 90
+
+// Range of the servos.
+#define SERVO_FREQ_MIN 600
+#define SERVO_FREQ_MAX 2400
+
+// 3.3V maximum for the acceleration sensor.
+#define ACCEL_MAX 676
+
+// Speed of the serial port.
 #define SERIAL_SPEED 9600
 // 115200
 // 38400
@@ -12,31 +25,25 @@
 
 
 Servo servos[MAX_SERVO];
-int servo;
-int pos;
-int command = 0;
+int serial_command = 0;
 
 
-void setup() {
-    Serial.begin(SERIAL_SPEED);
-    for (servo = 0; servo < MAX_SERVO; servo += 1) {
-        /* First 2 pins are used for the serial. */
-        servos[servo].attach(servo + 2, 600, 2400);
-        servos[servo].write(HOME_POS);
-        /* Don't set all servos at once, save power. */
-        delay(150);
-    }
+int _serial_read_word() {
+    return Serial.read() | Serial.read() << 8;
 }
 
 
-void loop() {
-    switch (command) {
+void do_serial_command() {
+    int servo;
+    int pos;
+
+    switch (serial_command) {
         case 0x00:
             /* No command, read one. */
             if (Serial.available() < 1) {
                 return;
             }
-            command = Serial.read();
+            serial_command = Serial.read();
             break;
         case 0xff:
             /* Move a single servo by degrees. */
@@ -48,7 +55,6 @@
             if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) {
                 servos[servo].write(pos);
             }
-            command = 0;
             break;
         case 0xfe:
             /* Move all servos by degrees. */
@@ -61,7 +67,19 @@
                     servos[servo].write(pos);
                 }
             }
-            command = 0;
+            serial_command = 0;
+            break;
+        case 0xfd:
+            /* Move a single servo by microseconds. */
+            if (Serial.available() < 3) {
+                return;
+            }
+            servo = Serial.read();
+            pos = _serial_read_word();
+            if (servo >= 0 && servo < MAX_SERVO &&
+                pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
+                servos[servo].writeMicroseconds(pos);
+            }
             break;
         case 0xfc:
             /* Move all servos by microseconds. */
@@ -69,12 +87,11 @@
                 return;
             }
             for (servo = 0; servo < MAX_SERVO; servo += 1) {
-                pos = Serial.read() | Serial.read() << 8;
-                if (pos >= 600 && pos <= 2400) {
+                pos = _serial_read_word();
+                if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
                     servos[servo].writeMicroseconds(pos);
                 }
             }
-            command = 0;
             break;
         case 0xf0:
             /* Read the accelerometer sensor. */
@@ -82,7 +99,31 @@
             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;
+        case 0xf1:
+            /* Beep. */
+            if (Serial.available() < 3) {
+                return;
+            }
+            tone(A3, _serial_read_word(), Serial.read());
             break;
     }
+    serial_command = 0;
 }
+
+void setup() {
+    int servo;
+
+    Serial.begin(SERIAL_SPEED);
+    for (servo = 0; servo < MAX_SERVO; servo += 1) {
+        /* First 2 pins are used for the serial. */
+        servos[servo].attach(servo + 2, SERVO_FREQ_MIN, SERVO_FREQ_MAX);
+        servos[servo].write(HOME_POS);
+        /* Don't set all servos at once, save power. */
+        delay(150);
+    }
+}
+
+void loop() {
+    do_serial_command();
+}