+#define START_1 512
+#define START_2 800
+#define START_3 950
+#define GRIPPER_OPEN 800
+#define GRIPPER_CLOSED 450
+
 Dynamixel Dxl(1);
+
+void gripper(boolean open) {
+	if (open) {
+		Dxl.goalPosition(4,GRIPPER_OPEN);
+		Dxl.goalPosition(5,1024-GRIPPER_OPEN);
+	} else {
+		Dxl.goalPosition(4,GRIPPER_CLOSED);
+		Dxl.goalPosition(5,1024-GRIPPER_CLOSED);		
+	}
+}
 
 void setup() {
 	Serial2.begin(57600);
 	Dxl.begin(3);
+
+	for (int id=1; id<6; id++) {
+		Dxl.jointMode(id);
+		Dxl.ledOn(id);
+	} 
+
+	Dxl.goalPosition(1, START_1);
+	Dxl.goalPosition(2, START_2);
+	Dxl.goalPosition(3, START_3);
+	
+	for (int i=0; i<5; i++) {
+		gripper(i%2);
+		delay(500);
+	}
 }
 
 byte blockingRead() {

		byte cmd = blockingRead();
		byte id = blockingRead();

		(inByte == 'W') ? processWrite(cmd, id) : processRead(cmd, id);
	}		
}



}

void processWrite(byte cmd, byte id) {
	int val;
	readInt(&val);

  	switch (cmd) { 
		case 's': { // set speed
			Dxl.goalSpeed(id, val);
			break;
		} case 'p': { // move to position
			Dxl.goalPosition(id, val);
			break; 
		} case 'o': {
			gripper(true);
			break;
		} case 'c': {
			gripper(false);
			break;
		} case 'g': { // general write command
			int add; 
			if (!readInt(&add)) break; 
			Dxl.writeWord(id, add, val);
			break;
		
