--- a/creep.py	Wed Aug 27 14:55:56 2014 +0200
+++ b/creep.py	Wed Aug 27 18:55:27 2014 +0200
@@ -114,7 +114,7 @@
     import serial
     import kubik
 
-    FPS = 32
+    FPS = 50
 
     robot = kubik.Robot(serial.Serial('/dev/rfcomm0', 115200))
     creep = Creep(robot)

--- a/kubik.py	Wed Aug 27 14:55:56 2014 +0200
+++ b/kubik.py	Wed Aug 27 18:55:27 2014 +0200
@@ -21,7 +21,8 @@
 
 class Robot(robot.Robot):
     accel_trim = [139, 136, 142]
-    battery_max = 230
+    battery_max = 231
+    battery_min = 190
 
     def __init__(self, serial):
         self.legs = (

--- a/leg.py	Wed Aug 27 14:55:56 2014 +0200
+++ b/leg.py	Wed Aug 27 18:55:27 2014 +0200
@@ -90,7 +90,7 @@
         ry = r * math.cos(a + radians) - self.base[1]
         self.move_to((rx, -ry * self.y_dir, None))
 
-    def update(self, shift=(0, 0, 0)):
+    def update(self, shift=(0, 0, 0), send=True):
         """Send the positions to the servos."""
         dx, dy, dz = shift
         x, y, z = self.position
@@ -98,4 +98,5 @@
         angles =self._inverse_kinematics(position)
         for joint, angle in zip(self, angles):
             joint.move_degrees(angle)
-            joint.update()
+            if send:
+                joint.update()

--- a/robot.py	Wed Aug 27 14:55:56 2014 +0200
+++ b/robot.py	Wed Aug 27 18:55:27 2014 +0200
@@ -10,6 +10,7 @@
     legs = ()
     accel_trim = [128.0, 128.0, 128.0]
     battery_max = 255
+    battery_min = 0
 
     def __init__(self):
         self.shift = [0.0, 0.0, 0.0]
@@ -47,9 +48,13 @@
             self._unshift(),
         ], 40)
 
-    def update(self):
+    def update(self, at_once=True):
+        servos = []
         for leg in self.legs:
-            leg.update(self.shift)
+            leg.update(self.shift, send=not at_once)
+            servos.extend(leg)
+        if at_once:
+            self.legs[0].hip.update_all(servos)
 
     def get_accel(self):
         sensors = self.legs[0].hip.get_sensors()
@@ -60,7 +65,8 @@
         ]
 
     def get_battery(self):
-        return float(self.legs[0].hip.get_battery()) / self.battery_max
+        return float(self.legs[0].hip.get_battery() -
+                     self.battery_min) / (self.battery_max - self.battery_min)
 
     def beep(self, frequency, duration):
         self.legs[0].hip.beep(frequency, duration)

--- a/servo.py	Wed Aug 27 14:55:56 2014 +0200
+++ b/servo.py	Wed Aug 27 18:55:27 2014 +0200
@@ -11,7 +11,8 @@
     return angle
 
 def _word(value):
-    return chr(value & 0xff) + chr(value & 0xff00 >> 8)
+    value = int(value)
+    return chr(value & 0xff) + (chr((value & 0xff00) >> 8))
 
 class Servo(object):
     span = 180
@@ -29,13 +30,16 @@
     def update(self):
         angle = self.calculate_angle()
         command = '\xff%c%c' % (self.index, int(angle))
+        freq = self.calculate_freq()
+        print (self.index, freq)
+        command = '\xfd%c%s' % (self.index, _word(freq))
         self.serial.write(command)
 
     @classmethod
     def update_all(cls, servos):
         serial = servos[0].serial
         servos = sorted(servos, key=lambda servo: servo.index)
-        command = '\xfc%s' % ''.join(_word(servo.calculate_freq())
+        command = '\xfe%s' % ''.join(chr(int(servo.calculate_angle()))
                                      for servo in servos)
         serial.write(command)
 

--- a/servo/servo.ino	Wed Aug 27 14:55:56 2014 +0200
+++ b/servo/servo.ino	Wed Aug 27 18:55:27 2014 +0200
@@ -3,7 +3,6 @@
 */
 
 #include <Servo.h>
-#include <math.h>
 
 // Time of a signle frame, in milliseconds.
 #define FRAME_TIME 10
@@ -68,40 +67,19 @@
     accel_buffer[accel_buffer_index][1] = analogRead(A1);
     accel_buffer[accel_buffer_index][2] = analogRead(A0);
     accel_buffer_index += 1;
-    if (accel_buffer_index > MAX_ACCEL_BUFFER) {
+    if (accel_buffer_index >= MAX_ACCEL_BUFFER) {
         accel_buffer_index = 0;
     }
 }
 
+
 int _serial_read_word() {
     /* Read two bytes from the serial port and make an int from them. */
-    return Serial.read() | Serial.read() << 8;
-}
-
-double _norm(double a, double b) {
-    return sqrt(a * a + b * b);
-}
-
-double _triangle(double a, double b, double c) {
-    return facos((a * a + b * b - c * c) / (2 * a * b));
-}
-
-int _radians_to_microseconds(double angle) {
-    while (angle < 0) {
-        angle += 2;
-    }
-    return SERVO_FREQ_MIN + (SERVO_FREQ_MAX - SERVO_FREQ_MIN) * angle;
-}
-
-void move_leg(int leg, double x, double y, double z) {
-    double f = _norm(x, y) - LEG_COXA;
-    double d = _norm(f, z);
-    double hip = fatan2(y, x) + 0.25 * legs[leg][4];
-    double knee = fatan2(-z, f) - _triangle(LEG_FEMUR, d, LEG_TIBIA);
-    double ankle = _triangle(LEG_FEMUR, LEG_TIBIA, d) - 0.5;
-    servos[legs[leg][0]].writeMicroseconds(_radians_to_microseconds(hip));
-    servos[legs[leg][1]].writeMicroseconds(_radians_to_microseconds(knee));
-    servos[legs[leg][2]].writeMicroseconds(_radians_to_microseconds(ankle));
+    int low;
+    int high;
+    low = Serial.read();
+    high = Serial.read();
+    return low | (high << 8);
 }
 
 void do_servo_degrees() {
@@ -143,17 +121,6 @@
     }
 }
 
-void do_move_leg() {
-    /* Move the leg to a position in space. */
-    int leg = Serial.read();
-    int x = Serial.read() - 128;
-    int y = Serial.read() - 128;
-    int z = Serial.read() - 128;
-    if (leg >= 0 && leg <= 4) {
-        move_leg(leg, x, y, z);
-    }
-}
-
 void do_accel() {
     /* Report the readings of the acceleration sensor. */
     int avg_x = 0;
@@ -232,13 +199,6 @@
             if (Serial.available() < MAX_SERVO * 2) { return; }
             do_servos_microseconds();
             break;
-        case 0xfb:
-            /* Move a leg to a position in space. */
-            if (Serial.available() < 4) {
-                return;
-            }
-            do_move_leg();
-            break;
         case 0xf0:
             /* Read the accelerometer sensor. */
             do_accel();
@@ -256,21 +216,55 @@
     serial_command = 0;
 }
 
-void setup() {
-    int servo;
-
-    Serial.begin(SERIAL_SPEED);
-    pinMode(A3, OUTPUT);
-    tone(A3, 440, 100);
-    delay(20);
-    tone(A3, 220, 150);
-    for (servo = 0; servo < MAX_SERVO; servo += 1) {
+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);
         /* Don't set all servos at once, save power. */
         delay(150);
     }
+    delay(50);
+    tone(A3, 440, 200);
+    delay(200);
+    tone(A3, 523, 100);
+    delay(150);
+    tone(A3, 880, 100);
+}
+
+void fold() {
+    /* Folds the robots and shut downs all servos. Doesn't return. */
+    for (int leg = 0; leg < 4; leg += 1) {
+        int d = legs[leg][3] * legs[leg][4];
+        servos[legs[leg][0]].write(90 + 45 * d);
+        servos[legs[leg][1]].write(90 - 90 * d);
+        servos[legs[leg][2]].write(90 + 90 * d);
+    }
+    delay(250);
+    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
+        servos[servo].detach();
+    }
+    while (1) {
+        delay(1000);
+    }
+}
+
+void check_battery() {
+    int reading = analogRead(A7);
+    if (reading < 762) {
+        /* At about 3.74V start an alarm. */
+        tone(A3, 880, 100);
+    }
+    if (reading < 758 + 500) {
+        /* At about 3.72V shut down. */
+        fold();
+    }
+}
+
+void setup() {
+    Serial.begin(SERIAL_SPEED);
+    pinMode(A3, OUTPUT);
+    unfold();
 }
 
 void loop() {
@@ -279,6 +273,7 @@
     do_serial_command();
     if (now > last_tick + FRAME_TIME || now < last_tick) {
         read_accel();
+        check_battery();
         last_tick = now;
     }
 }