diff --git a/firmware/demokit/demokit.pde b/firmware/demokit/demokit.pde
index 888aabc..feb112f 100644
--- a/firmware/demokit/demokit.pde
+++ b/firmware/demokit/demokit.pde
@@ -48,73 +48,76 @@
 		     "0000000012345678");
 Servo servos[3];
 
-CapSense   touch_robot = CapSense(TOUCH_SEND, TOUCH_RECV);        // 10M ohm resistor on demo shield
+// 10M ohm resistor on demo shield
+CapSense   touch_robot = CapSense(TOUCH_SEND, TOUCH_RECV);
 
 void setup();
 void loop();
 
 void init_buttons()
 {
-	pinMode( BUTTON1, INPUT );
-	pinMode( BUTTON2, INPUT );
-	pinMode( BUTTON3, INPUT );
-	pinMode( JOY_SWITCH, INPUT );
+	pinMode(BUTTON1, INPUT);
+	pinMode(BUTTON2, INPUT);
+	pinMode(BUTTON3, INPUT);
+	pinMode(JOY_SWITCH, INPUT);
 
-	digitalWrite( BUTTON1, HIGH );  // enable the internal pullups
-	digitalWrite( BUTTON2, HIGH );
-	digitalWrite( BUTTON3, HIGH );
-	digitalWrite( JOY_SWITCH, HIGH );
+	// enable the internal pullups
+	digitalWrite(BUTTON1, HIGH);
+	digitalWrite(BUTTON2, HIGH);
+	digitalWrite(BUTTON3, HIGH);
+	digitalWrite(JOY_SWITCH, HIGH);
 }
 
 
 void init_relays()
 {
-	pinMode( RELAY1, OUTPUT );
-	pinMode( RELAY2, OUTPUT );
+	pinMode(RELAY1, OUTPUT);
+	pinMode(RELAY2, OUTPUT);
 }
 
 
 void init_leds()
 {
-	digitalWrite( LED1_RED,   1 );
-	digitalWrite( LED1_GREEN, 1 );
-	digitalWrite( LED1_BLUE,  1 );
+	digitalWrite(LED1_RED, 1);
+	digitalWrite(LED1_GREEN, 1);
+	digitalWrite(LED1_BLUE, 1);
 
-	pinMode( LED1_RED,    OUTPUT );
-	pinMode( LED1_GREEN,  OUTPUT );
-	pinMode( LED1_BLUE,   OUTPUT );
+	pinMode(LED1_RED, OUTPUT);
+	pinMode(LED1_GREEN, OUTPUT);
+	pinMode(LED1_BLUE, OUTPUT);
 
-	digitalWrite( LED2_RED,   1 );
-	digitalWrite( LED2_GREEN, 1 );
-	digitalWrite( LED2_BLUE,  1 );
+	digitalWrite(LED2_RED, 1);
+	digitalWrite(LED2_GREEN, 1);
+	digitalWrite(LED2_BLUE, 1);
 
-	pinMode( LED2_RED,    OUTPUT );
-	pinMode( LED2_GREEN,  OUTPUT );
-	pinMode( LED2_BLUE,   OUTPUT );
+	pinMode(LED2_RED, OUTPUT);
+	pinMode(LED2_GREEN, OUTPUT);
+	pinMode(LED2_BLUE, OUTPUT);
 
-	digitalWrite( LED3_RED,   1 );
-	digitalWrite( LED3_GREEN, 1 );
-	digitalWrite( LED3_BLUE,  1 );
+	digitalWrite(LED3_RED, 1);
+	digitalWrite(LED3_GREEN, 1);
+	digitalWrite(LED3_BLUE, 1);
 
-	pinMode( LED3_RED,    OUTPUT );
-	pinMode( LED3_GREEN,  OUTPUT );
-	pinMode( LED3_BLUE,   OUTPUT );
+	pinMode(LED3_RED, OUTPUT);
+	pinMode(LED3_GREEN, OUTPUT);
+	pinMode(LED3_BLUE, OUTPUT);
 }
 
-void init_joystick( int threshold );
+void init_joystick(int threshold);
 
 byte b1, b2, b3, b4, c;
 void setup()
 {
-	Serial.begin( 115200 );
+	Serial.begin(115200);
 	Serial.print("\r\nStart");
 
 	init_leds();
 	init_relays();
 	init_buttons();
-	init_joystick( 5 );      // initialize with thresholding enabled, dead zone of 5 units  
+	init_joystick( 5 );
 
-	touch_robot.set_CS_AutocaL_Millis(0xFFFFFFFF);    // autocalibrate OFF
+	// autocalibrate OFF
+	touch_robot.set_CS_AutocaL_Millis(0xFFFFFFFF);
 
 	servos[0].attach(SERVO1);
 	servos[0].write(90);
@@ -150,31 +153,26 @@
 		char c0;
 
 		if (len > 0) {
-			// XXX: assumes only one command per packet
-			Serial.print(msg[0], HEX);
-			Serial.print(":");
-			Serial.print(msg[1], HEX);
-			Serial.print(":");
-			Serial.println(msg[2], HEX);
+			// assumes only one command per packet
 			if (msg[0] == 0x2) {
 				if (msg[1] == 0x0)
-					analogWrite( LED1_RED, 255 - msg[2]);
+					analogWrite(LED1_RED, 255 - msg[2]);
 				else if (msg[1] == 0x1)
-					analogWrite( LED1_GREEN, 255 - msg[2]);
+					analogWrite(LED1_GREEN, 255 - msg[2]);
 				else if (msg[1] == 0x2)
-					analogWrite( LED1_BLUE, 255 - msg[2]);
+					analogWrite(LED1_BLUE, 255 - msg[2]);
 				else if (msg[1] == 0x3)
-					analogWrite( LED2_RED, 255 - msg[2]);
+					analogWrite(LED2_RED, 255 - msg[2]);
 				else if (msg[1] == 0x4)
-					analogWrite( LED2_GREEN, 255 - msg[2]);
+					analogWrite(LED2_GREEN, 255 - msg[2]);
 				else if (msg[1] == 0x5)
-					analogWrite( LED2_BLUE, 255 - msg[2]);
+					analogWrite(LED2_BLUE, 255 - msg[2]);
 				else if (msg[1] == 0x6)
-					analogWrite( LED3_RED, 255 - msg[2]);
+					analogWrite(LED3_RED, 255 - msg[2]);
 				else if (msg[1] == 0x7)
-					analogWrite( LED3_GREEN, 255 - msg[2]);
+					analogWrite(LED3_GREEN, 255 - msg[2]);
 				else if (msg[1] == 0x8)
-					analogWrite( LED3_BLUE, 255 - msg[2]);
+					analogWrite(LED3_BLUE, 255 - msg[2]);
 				else if (msg[1] == 0x10)
 					servos[0].write(map(msg[2], 0, 255, 0, 180));
 				else if (msg[1] == 0x11)
@@ -183,12 +181,10 @@
 					servos[2].write(map(msg[2], 0, 255, 0, 180));
 			} else if (msg[0] == 0x3) {
 				if (msg[1] == 0x0)
-					digitalWrite( RELAY1, msg[2] ? HIGH : LOW );
+					digitalWrite(RELAY1, msg[2] ? HIGH : LOW);
 				else if (msg[1] == 0x1)
-					digitalWrite( RELAY2, msg[2] ? HIGH : LOW );
-
+					digitalWrite(RELAY2, msg[2] ? HIGH : LOW);
 			}
-
 		}
 
 		msg[0] = 0x1;
@@ -226,7 +222,6 @@
 		}
 
 		switch (count++ % 0x10) {
-
 		case 0:
 			val = analogRead(TEMP_SENSOR);
 			msg[0] = 0x4;
@@ -251,7 +246,6 @@
 			acc.write(msg, 3);
 			break;
 
-			/* captoutched needs to be asynchonous */
 		case 0xc:
 			touchcount = touch_robot.capSense(5);
 
@@ -268,6 +262,7 @@
 			break;
 		}
 	} else {
+		// reset outputs to default values on disconnect
 		analogWrite(LED1_RED, 255);
 		analogWrite(LED1_GREEN, 255);
 		analogWrite(LED1_BLUE, 255);
@@ -282,7 +277,6 @@
 		servos[0].write(90);
 		digitalWrite(RELAY1, LOW);
 		digitalWrite(RELAY2, LOW);
-
 	}
 
 	delay(10);
@@ -290,125 +284,111 @@
 
 // ==============================================================================
 // Austria Microsystems i2c Joystick
-
-/*
-  If a threshold is provided, the dead zone will be programmed such that interrupts will not
-  be generated unless the threshold is exceeded.
-
-  Note that if you use that mode, you will have to use passage of time with no new interrupts
-  to detect that the stick has been released and has returned to center.
-  
-  If you need to explicitly track return to center, pass 0 as the threshold.  "Center" will
-  still bounce around a little 
-*/
-
-
-void init_joystick( int threshold )
+void init_joystick(int threshold)
 {
-  byte status = 0;
-  
-  pinMode( JOY_SWITCH, INPUT );
-  digitalWrite( JOY_SWITCH, HIGH );    // enable the internal pullup
-  
-  pinMode( JOY_nINT, INPUT );
-  digitalWrite( JOY_nINT, HIGH );      // enable the internal pullup
+	byte status = 0;
 
-  pinMode( JOY_nRESET, OUTPUT );
+	pinMode(JOY_SWITCH, INPUT);
+	digitalWrite(JOY_SWITCH, HIGH);
 
-  digitalWrite( JOY_nRESET, 1 );
-  delay(1);
-  digitalWrite( JOY_nRESET, 0 );
-  delay(1);
-  digitalWrite( JOY_nRESET, 1 );
+	pinMode(JOY_nINT, INPUT);
+	digitalWrite(JOY_nINT, HIGH);
 
-  Wire.begin();
-  
-  do {
-    status = read_joy_reg( 0x0f );        // XXX need timeout
-  } while ((status & 0xf0) != 0xf0);
-  
-  write_joy_reg( 0x2e, 0x86 );            // invert magnet polarity setting, per datasheet
+	pinMode(JOY_nRESET, OUTPUT);
 
-  calibrate_joystick( threshold );        // calibrate & set up dead zone area  
+	digitalWrite(JOY_nRESET, 1);
+	delay(1);
+	digitalWrite(JOY_nRESET, 0);
+	delay(1);
+	digitalWrite(JOY_nRESET, 1);
+
+	Wire.begin();
+
+	do {
+		status = read_joy_reg(0x0f);
+	} while ((status & 0xf0) != 0xf0);
+
+	// invert magnet polarity setting, per datasheet
+	write_joy_reg(0x2e, 0x86);
+
+	calibrate_joystick(threshold);
 }
 
 
 int offset_X, offset_Y;
 
-void calibrate_joystick( int dz )
+void calibrate_joystick(int dz)
 {
-  char iii;
-  int x_cal = 0;
-  int y_cal = 0;
+	char iii;
+	int x_cal = 0;
+	int y_cal = 0;
 
-  write_joy_reg( 0x0f, 0x00 );          // Low Power Mode, 20ms auto wakeup
-                                        // INTn output enabled
-                                        // INTn active after each measurement
-                                        // Normal (non-Reset) mode
-  delay(1);
+	// Low Power Mode, 20ms auto wakeup
+	// INTn output enabled
+	// INTn active after each measurement
+	// Normal (non-Reset) mode
+	write_joy_reg(0x0f, 0x00);
+	delay(1);
 
-  read_joy_reg( 0x11 );                 // dummy read of Y_reg to reset interrupt
+	// dummy read of Y_reg to reset interrupt
+	read_joy_reg(0x11);
 
-  for( iii = 0; iii != 16; iii++ ) {    // read coords 16 times & average 
-    while( !joystick_interrupt() )      // poll for interrupt
-      ;
-    x_cal += read_joy_reg( 0x10 );      // X pos
-    y_cal += read_joy_reg( 0x11 );      // Y pos
-  }
-  
-  offset_X = -(x_cal>>4);               // divide by 16 to get average
-  offset_Y = -(y_cal>>4);
-  
-  //sprintf(msgbuf, "offsets = %d, %d\n", offset_X, offset_Y);
-  //Serial.print(msgbuf);
-  
-  write_joy_reg( 0x12,  dz - offset_X );  // Xp, LEFT threshold for INTn
-  write_joy_reg( 0x13, -dz - offset_X );  // Xn, RIGHT threshold for INTn
-  write_joy_reg( 0x14,  dz - offset_Y );  // Yp, UP threshold for INTn
-  write_joy_reg( 0x15, -dz - offset_Y );  // Yn, DOWN threshold for INTn
+	for(iii = 0; iii != 16; iii++) {
+		while(!joystick_interrupt()) {}
 
-  if ( dz )                             // dead zone threshold detect requested?
-    write_joy_reg( 0x0f, 0x04 );          // Low Power Mode, 20ms auto wakeup
-                                          // INTn output enabled
-                                          // INTn active when movement exceeds dead zone
-                                          // Normal (non-Reset) mode
+		x_cal += read_joy_reg(0x10);
+		y_cal += read_joy_reg(0x11);
+	}
+
+	// divide by 16 to get average
+	offset_X = -(x_cal>>4);
+	offset_Y = -(y_cal>>4);
+
+	write_joy_reg(0x12, dz - offset_X);  // Xp, LEFT threshold for INTn
+	write_joy_reg(0x13, -dz - offset_X);  // Xn, RIGHT threshold for INTn
+	write_joy_reg(0x14, dz - offset_Y);  // Yp, UP threshold for INTn
+	write_joy_reg(0x15, -dz - offset_Y);  // Yn, DOWN threshold for INTn
+
+	// dead zone threshold detect requested?
+	if (dz)
+		write_joy_reg(0x0f, 0x04);
 }
 
 
-void read_joystick( int *x, int *y )
+void read_joystick(int *x, int *y)
 {
-  *x = read_joy_reg( 0x10 ) + offset_X;
-  *y = read_joy_reg( 0x11 ) + offset_Y;  // reading Y clears the interrupt
+	*x = read_joy_reg(0x10) + offset_X;
+	*y = read_joy_reg(0x11) + offset_Y;  // reading Y clears the interrupt
 }
 
 char joystick_interrupt()
 {
-  return ( digitalRead( JOY_nINT ) == 0 ); 
+	return digitalRead(JOY_nINT) == 0;
 }
 
 
 #define  JOY_I2C_ADDR    0x40
 
-char read_joy_reg( char reg_addr )
+char read_joy_reg(char reg_addr)
 {
-  char c;
-  
-  Wire.beginTransmission( JOY_I2C_ADDR );
-  Wire.send( reg_addr );
-  Wire.endTransmission();
-  
-  Wire.requestFrom( JOY_I2C_ADDR, 1 );
-  
-  while(Wire.available())
-    c = Wire.receive();
-  
-  return c;
+	char c;
+
+	Wire.beginTransmission(JOY_I2C_ADDR);
+	Wire.send(reg_addr);
+	Wire.endTransmission();
+
+	Wire.requestFrom(JOY_I2C_ADDR, 1);
+
+	while(Wire.available())
+		c = Wire.receive();
+
+	return c;
 }
 
-void write_joy_reg( char reg_addr, char val )
+void write_joy_reg(char reg_addr, char val)
 {
-  Wire.beginTransmission( JOY_I2C_ADDR );
-  Wire.send( reg_addr );
-  Wire.send( val );
-  Wire.endTransmission();  
+	Wire.beginTransmission(JOY_I2C_ADDR);
+	Wire.send(reg_addr);
+	Wire.send(val);
+	Wire.endTransmission();
 }