diff --git a/communications/Makefile b/communications/Makefile
deleted file mode 100644
index cd714da6eb59cc665f3559b63d0fa3035d3048ca..0000000000000000000000000000000000000000
--- a/communications/Makefile
+++ /dev/null
@@ -1,46 +0,0 @@
-PROJECT=libcomm
-########################################################
-# afegir tots els fitxers que s'han de compilar aquí
-########################################################
-SOURCES=src/dynamixel.c src/dynamixel_master.c src/serial_console.c
-
-OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/
-INCLUDE_DIR=./include/
-BUILD_DIR=./build/
-BIN_DIR=./bin/
-LIB_DIR=./lib/
-CC=avr-gcc
-OBJCOPY=avr-ar
-MMCU=atmega2561
-
-CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
-
-ARFLAGS=rsc
-
-.PHONY: show_banner all
-
-all: show_banner $(PROJECT).a 
-
-show_banner:
-	@echo "------------------------------------------------------";
-	@echo "       _____                                          ";
-	@echo "      /  _  \                                         ";
-	@echo "      | |_| |               The Humanoid Lab          ";
-	@echo "  ____\_____/____                                     ";
-	@echo " /               \    http://apollo.upc.es/humanoide/ ";
-	@echo "/   _         _   \                                   ";
-	@echo "|  | |       | |  |            $(PROJECT)             ";
-	@echo "|  | |       | |  |                                   ";
-	@echo "------------------------------------------------------";
-
-$(PROJECT).a: $(OBJS)
-	$(OBJCOPY) $(ARFLAGS) $(LIB_DIR)$(PROJECT).a $(OBJS)
-%.o:%.c
-	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
-examples:
-	$(MAKE) -C src/examples	
-clean:
-	-rm $(LIB_DIR)$(PROJECT).*
-	-rm $(OBJS)
-	$(MAKE) -C src/examples clean
diff --git a/communications/include/comm_cfg.h b/communications/include/comm_cfg.h
deleted file mode 100644
index 454edd881749c3ab5c5fa892726b6ca083a13006..0000000000000000000000000000000000000000
--- a/communications/include/comm_cfg.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#ifndef _COMM_CFG_H
-#define _COMM_CFG_H
-
-#ifndef DYN_MASTER_MAX_TX_BUFFER_LEN
-  #define DYN_MASTER_MAX_TX_BUFFER_LEN            128
-#endif
-
-#ifndef DYN_MASTER_MAX_RX_BUFFER_LEN
-  #define DYN_MASTER_MAX_RX_BUFFER_LEN            128
-#endif
-
-#ifndef DYN_MASTER_DEFAULT_BAUDRATE
-  #define DYN_MASTER_DEFAULT_BAUDRATE             1000000
-#endif
-
-#ifndef DYN_MASTER_DEFAULT_TIMEOUT_US
-  #define DYN_MASTER_DEFAULT_TIMEOUT_US           10000
-#endif
-
-#ifndef SERIAL_CONSOLE_MAX_BUFFER_LEN
-  #define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
-#endif
-
-#endif
diff --git a/communications/include/dynamixel.h b/communications/include/dynamixel.h
deleted file mode 100644
index 40736bd92d89d9ce75a9ecaf9376a34c8ac3931b..0000000000000000000000000000000000000000
--- a/communications/include/dynamixel.h
+++ /dev/null
@@ -1,106 +0,0 @@
-#ifndef _DYNAMIXEL_H
-#define _DYNAMIXEL_H
-
-#include "dynamixel_common.h"
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-#define     DYN_HEADER_OFF      0
-#define     DYN_ID_OFF          2
-#define     DYN_LENGTH_OFF      3
-#define     DYN_INST_OFF        4
-#define     DYN_ERROR_OFF       4
-#define     DYN_DATA_OFF        5
-
-void dyn_copy_packet(uint8_t *source, uint8_t *destination);
-inline uint8_t dyn_get_id(uint8_t *packet)
-{
-  return packet[DYN_ID_OFF];
-}
-inline uint8_t dyn_get_length(uint8_t *packet)
-{
-  return packet[DYN_LENGTH_OFF];
-}
-inline TDynInstruction dyn_get_instruction(uint8_t *packet)
-{
-  return packet[DYN_INST_OFF];
-}
-uint8_t dyn_check_checksum(uint8_t *packet);
-
-// instruction packet
-/*
-typedef struct{
-  uint8_t header[2];
-  uint8_t id;
-  uint8_t length;
-  uint8_t instruction;
-  uint8_t data[MAX_DATA_LENGTH];
-  uint8_t checksum;
-}TDynInst;
-*/
-/* ping instruction */
-void dyn_init_ping_packet(uint8_t *packet,uint8_t id);
-/* read instruction */
-void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length);
-inline uint8_t dyn_get_read_length(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF+1];
-}
-inline uint8_t dyn_get_read_address(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-/* write instruction */
-void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
-inline uint8_t dyn_get_write_address(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-inline uint8_t dyn_get_write_length(uint8_t *packet)
-{
-  return packet[DYN_LENGTH_OFF]-3;
-}
-uint8_t dyn_get_write_data(uint8_t *packet,uint8_t *data);
-/* registered write instruction */
-void dyn_init_reg_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
-inline uint8_t dyn_get_reg_write_address(uint8_t *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-inline uint8_t dyn_get_reg_write_length(uint8_t *packet)
-{
-  return packet[DYN_LENGTH_OFF]-3;
-}
-uint8_t dyn_get_reg_write_data(uint8_t *packet,uint8_t *data);
-/* action instruction */
-void dyn_init_action_packet(uint8_t *packet);
-/* reset instruction */
-void dyn_init_reset_packet(uint8_t *packet,uint8_t id);
-/* sync write instruction */
-void dyn_init_sync_write_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t address,uint8_t length,TWriteData *data);
-uint8_t dyn_sync_write_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length,uint8_t *data);
-/* bulk read instruction */
-void dyn_init_bulk_read_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t *address,uint8_t *length);
-uint8_t dyn_bulk_read_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length);
-
-// status packet
-/*
-typedef struct{
-  uint8_t header[2];
-  uint8_t id;
-  uint8_t length;
-  uint8_t error;
-  uint8_t data[MAX_DATA_LENGTH];
-  uint8_t checksum;
-}TDynStatus;
-*/
-
-void dyn_init_status_packet(uint8_t *packet,uint8_t id,TDynError error,uint8_t length,uint8_t *data);
-inline TDynError dyn_get_status_error(uint8_t *packet)
-{
-  return packet[DYN_ERROR_OFF];
-}
-/* read instruction status packet */
-uint8_t dyn_get_read_status_data(uint8_t *packet,uint8_t *data);
-
-#endif
diff --git a/communications/include/dynamixel_common.h b/communications/include/dynamixel_common.h
deleted file mode 100644
index ea125c550f3593b27a54b4f6469a947ac5f14446..0000000000000000000000000000000000000000
--- a/communications/include/dynamixel_common.h
+++ /dev/null
@@ -1,49 +0,0 @@
-#ifndef _DYNAMIXEL_COMMON_H
-#define _DYNAMIXEL_COMMON_H
-
-#define     MAX_DATA_LENGTH     255
-
-#define     MAX_HEADER_LENGTH   6
-
-// errors
-#define     DYN_SUCCESS         0
-#define     DYN_BAD_FORMAT      1
-#define     DYN_NO_DEVICE       2
-#define     DYN_TIMEOUT         3
-#define     DYN_COMM_ERROR      4
-
-typedef struct{
-  unsigned char *data_addr;
-}TWriteData;
-
-typedef enum{no_return=0x00,
-             return_only_read=0x01,
-             return_all=0x02} return_level_t;
-
-// possible packet types
-typedef enum{DYN_PING=0x01,
-             DYN_READ=0x02,
-             DYN_WRITE=0x03,
-             DYN_REG_WRITE=0x04,
-             DYN_ACTION=0x05,
-             DYN_RESET=0x06,
-             DYN_SYNC_READ=0x82,
-             DYN_SYNC_WRITE=0x83,
-             DYN_BULK_READ=0x92,
-             DYN_BULK_WRITE=0x93} TDynInstruction;
-
-// boradcast ID
-#define     DYN_BROADCAST_ID    0xFE
-
-// status packet
-typedef enum{DYN_NO_ERROR=0x00,
-             DYN_INST_ERROR=0x40,
-             DYN_OVERLOAD_ERROR=0x20,
-             DYN_CHECKSUM_ERROR=0x10,
-             DYN_RANGE_ERROR=0x08,
-             DYN_OVERTEMP_ERROR=0x04,
-             DYN_ANGLE_ERROR=0x02,
-             DYN_VOLTAGE_ERROR=0x01} TDynError;
-
-typedef enum{DYN_VER1=0x01,DYN_VER2=0x02} TDynVersion;
-#endif
diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h
deleted file mode 100644
index 9fa62737f3c26e3e75d2bfbc888cc93d2524dd66..0000000000000000000000000000000000000000
--- a/communications/include/dynamixel_master.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _DYNAMIXEL_MASTER_H
-#define _DYNAMIXEL_MASTER_H
-
-#include "dynamixel.h"
-
-/* public functions */
-void dyn_master_init(void);
-void dyn_master_set_rx_timeout(uint16_t timeout_ms);
-void dyn_master_set_return_level(return_level_t level);
-return_level_t dyn_master_get_return_level(void);
-void dyn_master_set_baudrate(uint32_t baudrate);
-uint32_t dyn_master_get_baudrate(void);
-void dyn_master_scan(uint8_t *num,uint8_t *ids);
-uint8_t dyn_master_ping(uint8_t id);
-uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data);
-uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data);
-uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
-uint8_t dyn_master_write_byte(uint8_t id, uint16_t address, uint8_t data);
-uint8_t dyn_master_write_word(uint8_t id, uint16_t address, uint16_t data);
-uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
-uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
-uint8_t dyn_master_action(void);
-uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data);
-uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_t *length, TWriteData *data);
-
-#endif
diff --git a/communications/include/serial_console.h b/communications/include/serial_console.h
deleted file mode 100644
index 2624cfa550762791faee2eec945068763002ddb4..0000000000000000000000000000000000000000
--- a/communications/include/serial_console.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _SERIAL_CONSOLE_H
-#define _SERIAL_CONSOLE_H
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-void serial_console_init(uint32_t baudrate);
-
-#endif
diff --git a/communications/src/dynamixel.c b/communications/src/dynamixel.c
deleted file mode 100644
index cdff1dd42c5842829bfd1b1528bd6a85d620fcc9..0000000000000000000000000000000000000000
--- a/communications/src/dynamixel.c
+++ /dev/null
@@ -1,241 +0,0 @@
-#include "dynamixel.h"
-
-// provate functions
-void dyn_set_checksum(uint8_t *packet)
-{
-  uint8_t checksum=0x00;
-  uint8_t i;
-
-  checksum+=packet[DYN_ID_OFF];
-  checksum+=packet[DYN_LENGTH_OFF];
-  checksum+=packet[DYN_INST_OFF];
-  for(i=0;i<packet[DYN_LENGTH_OFF]-2;i++)
-    checksum+=packet[DYN_DATA_OFF+i];
-  packet[DYN_DATA_OFF+i]=~checksum;
-}
-
-uint8_t dyn_check_checksum(uint8_t *packet)
-{
-  uint8_t checksum=0x00;
-  uint8_t i;
-
-  checksum+=packet[DYN_ID_OFF];
-  checksum+=packet[DYN_LENGTH_OFF];
-  checksum+=packet[DYN_INST_OFF];
-  for(i=0;i<packet[DYN_LENGTH_OFF]-2;i++)
-    checksum+=packet[DYN_DATA_OFF + i];
-  checksum+=packet[DYN_DATA_OFF+i];
-
-  return checksum;
-}
-
-// public functions
-void dyn_copy_packet(uint8_t *source, uint8_t *destination)
-{
-  uint8_t i,length;
-
-  length=source[DYN_LENGTH_OFF]+4;
-  for(i=0;i<length;i++)
-    destination[i]=source[i];
-}
-
-/* ping instruction*/
-void dyn_init_ping_packet(uint8_t *packet,uint8_t id)
-{
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=id;
-  packet[DYN_LENGTH_OFF]=0x02;
-  packet[DYN_INST_OFF]=DYN_PING;
-  dyn_set_checksum(packet);
-}
-
-/* read instruction */
-void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length)
-{
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=id;
-  packet[DYN_LENGTH_OFF]=0x04;
-  packet[DYN_INST_OFF]=DYN_READ;
-  packet[DYN_DATA_OFF]=address;
-  packet[DYN_DATA_OFF+1]=length;
-  dyn_set_checksum(packet);
-}
-
-/* write instruction */
-void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
-  uint8_t i;
-
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=id;
-  packet[DYN_LENGTH_OFF]=0x03+length;
-  packet[DYN_INST_OFF]=DYN_WRITE;
-  packet[DYN_DATA_OFF]=address;
-  for(i=0;i<length;i++)
-    packet[DYN_DATA_OFF+i+1]=data[i];
-  dyn_set_checksum(packet);
-}
-
-uint8_t dyn_get_write_data(uint8_t *packet,uint8_t *data)
-{
-  uint8_t i;
-
-  for(i=0;i<packet[DYN_LENGTH_OFF]-3;i++)
-    data[i]=packet[DYN_DATA_OFF+i+1];
-
-  return packet[DYN_LENGTH_OFF]-3;
-}
-
-/* registered write instruction */
-void dyn_init_reg_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
-  uint8_t i;
-
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=id;
-  packet[DYN_LENGTH_OFF]=0x03+length;
-  packet[DYN_INST_OFF]=DYN_REG_WRITE;
-  packet[DYN_DATA_OFF]=address;
-  for(i=0;i<length;i++)
-    packet[DYN_DATA_OFF+i+1]=data[i];
-  dyn_set_checksum(packet);
-}
-
-uint8_t dyn_get_reg_write_data(uint8_t *packet,uint8_t *data)
-{
-  uint8_t i;
-
-  for(i=0;i<packet[DYN_LENGTH_OFF]-3;i++)
-    data[i]=packet[DYN_DATA_OFF+i+1];
-
-  return packet[DYN_LENGTH_OFF]-3;
-}
-
-/* action instruction */
-void dyn_init_action_packet(uint8_t *packet)
-{
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=0xFE;
-  packet[DYN_LENGTH_OFF]=0x02;
-  packet[DYN_INST_OFF]=DYN_ACTION;
-  dyn_set_checksum(packet);
-}
-
-/* reset instruction */
-void dyn_init_reset_packet(uint8_t *packet,uint8_t id)
-{
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=id;
-  packet[DYN_LENGTH_OFF]=0x02;
-  packet[DYN_INST_OFF]=DYN_RESET;
-  dyn_set_checksum(packet);
-}
-
-/* sync write instruction */
-void dyn_init_sync_write_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t address,uint8_t length,TWriteData *data)
-{
-  uint8_t i,j;
-
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=0xFE;
-  packet[DYN_LENGTH_OFF]=0x04+(length+1)*num_servos;
-  packet[DYN_INST_OFF]=DYN_SYNC_WRITE;
-  packet[DYN_DATA_OFF]=address;
-  packet[DYN_DATA_OFF+1]=length;
-  for(i=0;i<num_servos;i++)
-  {
-    packet[DYN_DATA_OFF+2+(length+1)*i]=servo_ids[i];
-    for(j=0;j<length;j++)
-      packet[DYN_DATA_OFF+2+(length+1)*i+j+1]=data[i].data_addr[j];
-  }
-  dyn_set_checksum(packet);
-}
-
-uint8_t dyn_sync_write_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length,uint8_t *data)
-{
-  uint8_t num_servos,i,j;
-
-  num_servos=(packet[DYN_LENGTH_OFF]-0x04)/(packet[DYN_DATA_OFF+1]+1);
-  for(i=0;i<num_servos;i++)
-    if(packet[DYN_DATA_OFF+2+(packet[DYN_DATA_OFF+1]+1)*i]==id)
-    {
-      (*address)=packet[DYN_DATA_OFF];
-      (*length)=packet[DYN_DATA_OFF+1];
-      for(j=0;j<packet[DYN_DATA_OFF+1];j++)
-        data[j]=packet[DYN_DATA_OFF+2+(packet[DYN_DATA_OFF+1]+1)*i+j+1];
-      return 0x01;
-    }
-  return 0x00;
-}
-
-/* bulk read instruction */
-void dyn_init_bulk_read_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t *address,uint8_t *length)
-{
-  uint8_t i;
-
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=0xFE;
-  packet[DYN_LENGTH_OFF]=0x03;
-  packet[DYN_INST_OFF]=DYN_BULK_READ;
-  packet[DYN_DATA_OFF]=0x00;
-  for(i=0;i<num_servos;i++)
-  {
-    packet[DYN_DATA_OFF+1+i*3]=length[i];
-    packet[DYN_DATA_OFF+1+i*3+1]=servo_ids[i];
-    packet[DYN_DATA_OFF+1+i*3+2]=address[i];
-    packet[DYN_LENGTH_OFF]+=0x03;
-  }
-  dyn_set_checksum(packet);
-}
-
-uint8_t dyn_bulk_read_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length)
-{
-  uint8_t num_servos,i,prev_id=0x00;
-
-  num_servos=(packet[DYN_LENGTH_OFF]-0x03)/3;
-  for(i=0;i<num_servos;i++)
-  {
-    if(packet[DYN_DATA_OFF+1+i*3+1]==id)
-    {
-      (*address)=packet[DYN_DATA_OFF+1+i*3+2];
-      (*length)=packet[DYN_DATA_OFF+1+i*3];
-      return prev_id;
-    }
-    else
-      prev_id=packet[DYN_DATA_OFF+1+i*3+1];
-  }
-  return 0xFF;
-}
-
-void dyn_init_status_packet(uint8_t *packet,uint8_t id,TDynError error,uint8_t length,uint8_t *data)
-{
-  uint8_t i;
-
-  packet[DYN_HEADER_OFF]=0xFF;
-  packet[DYN_HEADER_OFF+1]=0xFF;
-  packet[DYN_ID_OFF]=id;
-  packet[DYN_LENGTH_OFF]=0x02+length;
-  packet[DYN_ERROR_OFF]=error;
-  for(i=0;i<length;i++)
-    packet[DYN_DATA_OFF+i]=data[i];
-  dyn_set_checksum(packet); 
-}
-
-/* read instruction status packet */
-uint8_t dyn_get_read_status_data(uint8_t *packet,uint8_t *data)
-{
-  uint8_t i;
-
-  for(i=0;i<packet[DYN_LENGTH_OFF]-0x02;i++)
-    data[i]=packet[DYN_DATA_OFF+i];
-
-  return packet[DYN_LENGTH_OFF]-0x02;
-}
diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c
deleted file mode 100644
index c1b445ac1b8fc594fa0d31aba5c2a31e909c09a3..0000000000000000000000000000000000000000
--- a/communications/src/dynamixel_master.c
+++ /dev/null
@@ -1,524 +0,0 @@
-#include "comm_cfg.h"
-#include "dynamixel_master.h"
-
-/* private variables */
-uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
-uint8_t dyn_master_sent_bytes;
-volatile uint8_t dyn_master_sent_done;
-uint8_t dyn_master_rx_buffer[DYN_MASTER_MAX_RX_BUFFER_LEN];
-uint8_t dyn_master_received_bytes;
-volatile uint8_t dyn_master_packet_ready;
-return_level_t dyn_master_return_level;
-uint8_t dyn_master_rx_no_answer;
-uint8_t dyn_master_rx_num_packets;
-uint32_t dyn_master_baudrate;
-/* private timeout variables */
-uint16_t dyn_master_rx_timeout_us;
-
-/* private timeout functions */
-void dyn_master_start_timeout(void)
-{
-  uint32_t count;
-
-  count=(((dyn_master_rx_timeout_us*(F_CPU/1000000))/1024));
-
-  TCNT0=255-count;
-  /* enable timer */
-  TIFR0|=(1<<TOV0);// clear interrutp flag
-  TCCR0B=(1<<CS02)|(1<<CS00);// set prescaler to 1024
-}
-
-void dyn_master_cancel_timeout(void)
-{
-  /* disable timer */
-  TCCR0B=0x00;
-  TIFR0|=(1<<TOV0);// clear interrutp flag
-  TCNT0=0x00; 
-}
-
-uint8_t dyn_master_timeout(void)
-{
-  if(TIFR0&(1<<TOV0))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-/* private functions */
-void dyn_master_set_tx_mode(void)
-{
-  PORTE&=~0x08;
-  PORTE|=0x04;
-}
-
-void dyn_master_set_rx_mode(void)
-{
-  PORTE&=~0x04;
-  PORTE|=0x08;
-}
-
-/* interrupt handlers */
-ISR(USART0_TX_vect)
-{
-  if(dyn_master_sent_bytes==dyn_master_tx_buffer[3]+4)
-  {
-    dyn_master_set_rx_mode();
-    dyn_master_sent_done=1;
-  }
-  else
-  {
-    UDR0=dyn_master_tx_buffer[dyn_master_sent_bytes];
-    dyn_master_sent_bytes++;
-  }
-}
-
-ISR(USART0_RX_vect)
-{
-  static uint8_t length;
-
-  cli();// disable any other interrupt
-  dyn_master_start_timeout();// each time a byte is received, restart the timeout
-  dyn_master_rx_buffer[dyn_master_received_bytes]=UDR0;
-  switch(dyn_master_received_bytes)
-  {
-    case 0: if(dyn_master_rx_buffer[dyn_master_received_bytes]==0xFF)
-              dyn_master_received_bytes++;
-            break;
-    case 1: if(dyn_master_rx_buffer[dyn_master_received_bytes]==0xFF)
-              dyn_master_received_bytes++;
-            else
-              dyn_master_received_bytes--;
-            break;
-    case 2: dyn_master_received_bytes++;
-            break;
-    case 3: length=dyn_master_rx_buffer[dyn_master_received_bytes]+3;
-            dyn_master_received_bytes++;
-            break;
-    default: if(dyn_master_received_bytes==length)
-             {
-               dyn_master_received_bytes=0;
-               dyn_master_packet_ready=1;
-               dyn_master_cancel_timeout();// cancel timeout when reception is complete
-             }
-             else
-               dyn_master_received_bytes++;
-             break;
-  }
-  sei();// enable all interrutps
-}
-
-uint8_t dyn_master_wait_transmission(void)
-{
-  while(dyn_master_sent_done==0);
-  
-  return DYN_SUCCESS;
-}
-
-uint8_t dyn_master_send(void)
-{
-  uint8_t error;
-
-  // wait until any previous transmission ends
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-    return error;
-  // set the transfer
-  dyn_master_sent_bytes=1;
-  UDR0=dyn_master_tx_buffer[0];
-
-  return DYN_SUCCESS;
-} 
-
-uint8_t dyn_master_wait_reception(void)
-{
-  // set timeout 
-  dyn_master_start_timeout();
-  // wait for the status packet
-  if(dyn_master_rx_no_answer)
-  {
-    dyn_master_rx_no_answer=0x00;
-    return DYN_SUCCESS;
-  }
-  else
-  {
-    while(dyn_master_packet_ready==0x00)
-    {
-      if(dyn_master_timeout())
-      {
-        dyn_master_cancel_timeout();
-        return DYN_TIMEOUT;
-      }
-    }
-    dyn_master_rx_num_packets--;
-    if(dyn_master_rx_num_packets==0x00)
-      dyn_master_cancel_timeout();
-    dyn_master_packet_ready=0x00;
-    // check the input packet checksum
-    if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
-      return dyn_get_status_error(dyn_master_rx_buffer);
-    else
-      return DYN_CHECKSUM_ERROR;
-  }
-}
-
-/* public functions */
-void dyn_master_init(void)
-{
-  /* initialize internal variables */
-  dyn_master_sent_done=0x01;
-  dyn_master_packet_ready=0x00;
-  dyn_master_sent_bytes=0x00;
-  dyn_master_received_bytes=0x00;
-  dyn_master_rx_no_answer=0x00;
-  dyn_master_rx_num_packets=0x00;
-  dyn_master_return_level=return_all;
-
-  /* configure the IO ports */
-  DDRE|=0x0E;
-  DDRE&=0xFE;
-
-  DDRC=0x7F;
-  PORTC=0x7F;
-
-  /* initialize the serial port */
-  // set UART register A
-  //Bit 7: USART Receive Complete
-  //Bit 6: USART Transmit Complete
-  //Bit 5: USART Data Resigter Empty 
-  //Bit 4: Frame Error
-  //Bit 3: Data OverRun
-  //Bit 2: Parity Error
-  //Bit 1: Double The USART Transmission Speed
-  //Bit 0: Multi-Processor Communication Mode
-  UCSR0A = 0b01000010;
-
-  // set UART register B
-  // bit7: enable rx interrupt
-  // bit6: enable tx interrupt
-  // bit4: enable rx
-  // bit3: enable tx
-  // bit2: set sendding size(0 = 8bit)
-  UCSR0B = 0b11011000;
-
-  // set UART register C
-  // bit6: communication mode (1 = synchronize, 0 = asynchronize)
-  // bit5,bit4: parity bit(00 = no parity) 
-  // bit3: stop bit(0 = stop bit 1, 1 = stop bit 2)
-  // bit2,bit1: data size(11 = 8bit)
-  UCSR0C = 0b00000110;
-  dyn_master_set_baudrate((uint32_t)DYN_MASTER_DEFAULT_BAUDRATE);
-
-  /* initialize the timeout timer */
-  dyn_master_rx_timeout_us=DYN_MASTER_DEFAULT_TIMEOUT_US;
-  TCCR0A=0x00;// normal mode, output disabled on both channels
-  TCCR0B=0x00;// normal mode, prescaler 0 (timer disabled)
-  TIMSK0=0x00;// all interrupts disabled
-  TCNT0=0x00;
-
-  dyn_master_set_rx_mode();
-}
-
-void dyn_master_set_rx_timeout(uint16_t timeout_us)
-{
-  if(timeout_us>16000)
-    dyn_master_rx_timeout_us=16000;
-  else
-    dyn_master_rx_timeout_us=timeout_us;
-}
-
-void dyn_master_set_return_level(return_level_t level)
-{
-  dyn_master_return_level=level;
-}
-
-return_level_t dyn_master_get_return_level(void)
-{
-  return dyn_master_return_level;
-}
-
-void dyn_master_set_baudrate(uint32_t baudrate)
-{
-  uint16_t divisor;
-
-  divisor = (uint16_t)((uint32_t)2000000 / baudrate) - 1;
-  UBRR0H = (uint8_t)((divisor & 0xFF00) >> 8);
-  UBRR0L = (uint8_t)(divisor & 0x00FF);
-}
-
-uint32_t dyn_master_get_baudrate(void)
-{
-  return dyn_master_baudrate;
-}
-
-void dyn_master_scan(uint8_t *num,uint8_t *ids)
-{
-  uint8_t i;
-
-  *num=0;
-  for(i=0;i<254;i++)
-  {
-    if(dyn_master_ping(i)==DYN_SUCCESS)// the device exists
-    {
-      ids[*num]=i;
-      (*num)++;
-    }
-  }
-}
-
-uint8_t dyn_master_ping(uint8_t id)
-{
-  uint8_t error;
-
-  // generate the ping packet for the desired device
-  dyn_init_ping_packet(dyn_master_tx_buffer,id);
-  dyn_master_rx_num_packets=0x01;
-  dyn_master_rx_no_answer=0x00;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the replay within the given timeout
-  error=dyn_master_wait_reception();
-
-  return error;
-}
-
-uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
-{
-  return dyn_master_read_table(id,address,1,data);
-}
-
-uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
-{
-  uint8_t error;
-  uint8_t data_int[2];
-
-  // generate the ping packet for the desired device
-  error=dyn_master_read_table(id,address,2,data_int);
-  (*data)=data_int[0]+data_int[1]*256;
-
-  return error;
-}
-
-uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
-{
-  uint8_t error;
-
-  // generate the read packet for the desired device
-  dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
-  dyn_master_rx_num_packets=0x01;
-  if(dyn_master_return_level==no_return || id==DYN_BROADCAST_ID)
-    dyn_master_rx_no_answer=0x01;
-  else
-    dyn_master_rx_no_answer=0x00;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the replay within the given timeout
-  if(dyn_master_return_level!=no_return && id!=DYN_BROADCAST_ID)
-  {
-    if((error=dyn_master_wait_reception())==DYN_SUCCESS)
-    {
-      if(dyn_get_read_status_data(dyn_master_rx_buffer,data)!=length)// not enough data
-        error=DYN_INST_ERROR;
-    }
-  } 
-
-  return error;
-}
-
-uint8_t dyn_master_write_byte(uint8_t id, uint16_t address, uint8_t data)
-{
-  return dyn_master_write_table(id,address,1,&data);
-}
-
-uint8_t dyn_master_write_word(uint8_t id, uint16_t address, uint16_t data)
-{
-  uint8_t data_int[2];
-
-  data_int[0]=data%256;
-  data_int[1]=data/256;
-  return dyn_master_write_table(id,address,2,data_int);
-}
-
-uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
-  uint8_t error;
-
-  // generate the write packet for the desired device
-  dyn_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
-  dyn_master_rx_num_packets=0x01;
-  if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID)
-    dyn_master_rx_no_answer=0x00;
-  else
-    dyn_master_rx_no_answer=0x01;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the replay within the given timeout
-  if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID)
-    error=dyn_master_wait_reception();
-
-  return error;
-}
-
-uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
-  uint8_t error;
-
-  // generate the registered write packet for the desired device
-  dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
-  dyn_master_rx_num_packets=0x01;
-  if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID)
-    dyn_master_rx_no_answer=0x00;
-  else
-    dyn_master_rx_no_answer=0x01;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the replay within the given timeout
-  if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID)
-    error=dyn_master_wait_reception();
-
-  return error;
-}
-
-uint8_t dyn_master_action(void)
-{
-  uint8_t error;
-
-  // generate the action packet for the desired device
-  dyn_init_action_packet(dyn_master_tx_buffer);
-  dyn_master_rx_num_packets=0x01;
-  dyn_master_rx_no_answer=0x01;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-
-  return error;
-}
-
-uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data)
-{
-  uint8_t error;
-
-  // generate the write packet for the desired device
-  dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
-  dyn_master_rx_num_packets=0x01;
-  dyn_master_rx_no_answer=0x01;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-
-  return error;
-}
-
-uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_t *length, TWriteData *data)
-{
-  uint8_t error,i,ver1_address[255],ver1_length[255];
-
-  // generate the read packet for the desired device
-  for(i=0;i<num;i++)
-  {
-    ver1_address[i]=address[i];
-    ver1_length[i]=length[i];
-  }
-  dyn_init_bulk_read_packet(dyn_master_tx_buffer,num,ids,ver1_address,ver1_length);
-  dyn_master_rx_num_packets=num;
-  if(dyn_master_return_level==no_return)
-    dyn_master_rx_no_answer=0x01;
-  else
-    dyn_master_rx_no_answer=0x00;
-  // enable transmission
-  dyn_master_set_tx_mode();
-  // send the data
-  if((error=dyn_master_send())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
-  // wait for the replay within the given timeout
-  if(dyn_master_return_level!=no_return)
-  {
-    for(i=0;i<num;i++)
-    {
-      if((error=dyn_master_wait_reception())==DYN_SUCCESS)
-      {
-        if(dyn_get_read_status_data(dyn_master_rx_buffer,data[i].data_addr)!=length[i])// not enough data
-          error=DYN_INST_ERROR;
-      }
-    }
-  }
-
-  return error;
-}
diff --git a/communications/src/examples/Makefile b/communications/src/examples/Makefile
deleted file mode 100644
index fd3b35d327a4feadee4160ce1945fbf9222837d0..0000000000000000000000000000000000000000
--- a/communications/src/examples/Makefile
+++ /dev/null
@@ -1,40 +0,0 @@
-PROJECT=comm_ex
-########################################################
-# afegir tots els fitxers que s'han de compilar aquí
-########################################################
-SOURCES=main.c
-
-OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/
-INCLUDE_DIR=../../include/
-COMM_DIR=../../lib/
-CC=avr-gcc
-OBJCOPY=avr-objcopy
-MMCU=atmega2561
-
-LIBS=$(COMM_DIR)libcomm.a
-
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
-
-LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
-
-HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
-
-.PHONY: all
-
-all: $(PROJECT).hex
-
-$(PROJECT).hex: $(PROJECT).elf
-	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
-$(PROJECT).elf: $(OBJS)
-	$(CC) $(LDFLAGS) $(OBJS) $(LIB_DIRS) $(LIBS) -o $(PROJECT).elf
-%.o:%.c
-	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
-
-download: $(MAIN_OUT_HEX)
-	avrdude -c avr109 -P /dev/ttyUSB0 -b 19200 -p m128 -U flash:w:$(PROJECT).hex
-	avrdude -c avr109 -P /dev/ttyUSB0 -b 19200 -p m128 -U eeprom:w:$(PROJECT)_eeprom.hex
-
-clean:
-	-rm $(PROJECT).*
-	-rm $(OBJS)
diff --git a/communications/src/examples/main.c b/communications/src/examples/main.c
deleted file mode 100755
index 6188a35168e74d4c7ab8c2dc24e99b65db3eeac3..0000000000000000000000000000000000000000
--- a/communications/src/examples/main.c
+++ /dev/null
@@ -1,31 +0,0 @@
-#include <avr/io.h>
-#include <util/delay.h>
-#include <avr/interrupt.h>
-#include "dynamixel_master.h"
-#include "serial_console.h"
-#include <stdio.h>
-
-int16_t main(void) 
-{
-  unsigned char num;
-  unsigned char ids[32];
-
-  dyn_master_init();
-  serial_console_init(57600);
-  sei();
-
-  while (1) 
-  {
-    dyn_master_scan(&num,ids);
-    if(num==1) 
-    {
-      printf("Found one device\n");
-      PORTC=0x3F;
-    }
-    else
-    {
-      printf("No device found\n");
-      PORTC=0x7F;
-    }
-  }
-}
diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
deleted file mode 100644
index 7842eb7d91bf8866bd014625a7a30f234339bbfd..0000000000000000000000000000000000000000
--- a/communications/src/serial_console.c
+++ /dev/null
@@ -1,99 +0,0 @@
-#include "comm_cfg.h"
-#include "serial_console.h"
-#include <stdio.h>
-
-/* private variables */
-volatile uint8_t serial_console_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
-volatile uint8_t serial_console_read_ptr;
-volatile uint8_t serial_console_write_ptr;
-volatile uint8_t serial_console_num_data;
-static FILE *device;
-
-/* interrupt handlers */
-SIGNAL(USART1_RX_vect)
-{
-  if(serial_console_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
-  {
-    serial_console_buffer[serial_console_write_ptr]=UDR1;
-    serial_console_write_ptr=(serial_console_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
-    serial_console_num_data++;
-  }
-}
-
-/* private functions */
-void serial_console_set_baudrate(uint32_t baudrate)
-{
-  uint16_t divisor;
-
-  divisor = (uint16_t)((uint32_t)2000000 / baudrate) - 1;
-  UBRR1H = (uint8_t)((divisor & 0xFF00) >> 8);
-  UBRR1L = (uint8_t)(divisor & 0x00FF);
-}
-
-int serial_console_putchar(char c,FILE *dev)
-{
-  if( c == '\n' )
-  {
-    while((UCSR1A&(1<<UDRE1))==0x00);
-    UDR1 = '\r';
-  }
-
-  while((UCSR1A&(1<<UDRE1))==0x00);
-  UDR1 = c;
-
-  return 0;
-}
-
-int serial_console_getchar(FILE *dev)
-{
-  int8_t rx;
-
-  while(serial_console_num_data==0);
-  rx=serial_console_buffer[serial_console_read_ptr];
-  serial_console_read_ptr=(serial_console_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
-  serial_console_num_data--;
-  if(rx=='\r')
-    rx='\n';
-
-  return rx;
-}
-
-/* pubic functions */
-void serial_console_init(uint32_t baudrate)
-{
-  // set UART register A
-  //Bit 7: USART Receive Complete
-  //Bit 6: USART Transmit Complete
-  //Bit 5: USART Data Resigter Empty 
-  //Bit 4: Frame Error
-  //Bit 3: Data OverRun
-  //Bit 2: Parity Error
-  //Bit 1: Double The USART Transmission Speed
-  //Bit 0: Multi-Processor Communication Mode
-  UCSR1A = 0b01000010;
-
-  // set UART register B
-  // bit7: enable rx interrupt
-  // bit6: enable tx interrupt
-  // bit4: enable rx
-  // bit3: enable tx
-  // bit2: set sendding size(0 = 8bit)
-  UCSR1B = 0b10011000;
-
-  // set UART register C
-  // bit6: communication mode (1 = synchronize, 0 = asynchronize)
-  // bit5,bit4: parity bit(00 = no parity) 
-  // bit3: stop bit(0 = stop bit 1, 1 = stop bit 2)
-  // bit2,bit1: data size(11 = 8bit)
-  UCSR1C = 0b00000110;
-
-  // initialize
-  UDR1 = 0xFF;
-  serial_console_read_ptr=0;
-  serial_console_write_ptr=0;
-  serial_console_num_data=0;
-
-  // set baudrate
-  serial_console_set_baudrate(baudrate);
-  device=fdevopen(serial_console_putchar,serial_console_getchar);
-}
diff --git a/controllers/Makefile b/controllers/Makefile
index 1c0c8bca77ce8044316e1d248b82b8849706f9f4..04d436420db8020725cf3adb28accb98ea038f9a 100755
--- a/controllers/Makefile
+++ b/controllers/Makefile
@@ -1,30 +1,53 @@
 #AVR-GCC  Makefile
 
+ROBOTIS_C_DIR ?=
+
+ifdef ROBOTIS_C_DIR
+    ROBOTIS_C_DIR_FOUND=true
+else
+    ROBOTIS_C_DIR_FOUND=false
+endif
+
 PROJECT=libcontrollers
-SOURCES=src/cm510.c src/gpio.c src/adc.c src/buzzer.c src/user_time.c
+SOURCES=src/cm510/cm510.c
 OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/ 
-BIN_DIR=./build/
-LIB_DIR=./lib/
-DEV_DIR=../dyn_devices/
-COMM_DIR=../communications/
-MAN_DIR=../motion/
-CONT_DIR=./
+SDIR=./src/ 
+IDIR=./include/
+BDIR=./build/
+LDIR=./lib/
 CC=avr-gcc
-OBJCOPY=avr-ar
+OBJCOPY=avr-ar rsc
 MMCU=atmega2561
 
-INC_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(DEV_DIR)include -I$(MAN_DIR)include
-
-LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a $(MAN_DIR)lib/libmotion_manager.a
+ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/
+ROBOTIS_LIBRARY_DIR=$(ROBOTIS_C_DIR)/lib/
 
 CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
-ARFLAGS=rsc
-
-.PHONY: all show_banner clean
-
-all: show_banner communications dyn_devices motion_manager $(PROJECT).a
+.PHONY: all check_robotis_c show_banner clean
+
+all: check_robotis_c show_banner $(PROJECT).a
+
+check_robotis_c:
+	@if ! $(ROBOTIS_C_DIR_FOUND); then                                                 \
+	    echo " IMPORTANT !!! ";                                                        \
+	    echo " ROBOTIS_C_DIR variable is not set! The software won't compile";         \
+	    echo " unless you specify the path to Robotis embedded C software.";           \
+	    echo "";                                                                       \
+	    echo " You should:";                                                           \
+	    echo "   1. Edit the makefile, at the begging set up ROBOTIS_C_DIR";           \
+	    echo "   2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \
+	    echo "";                                                                       \
+	    echo " Beware of:";                                                            \
+	    echo "   1. Do not use '(' in the directory name (as it comes from robotis) "; \
+	    echo "   2. Absolute path is preferred to avoid problems with submake calls"; \
+	    exit 2;                                                                        \
+	fi
+
+	@if [ ! -d $(ROBOTIS_C_DIR) ]; then                                                  \
+	   echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it.";  \
+	   exit 2;                                                                           \
+	fi
 
 show_banner:
 	@echo "------------------------------------------------------";
@@ -39,33 +62,11 @@ show_banner:
 	@echo "------------------------------------------------------";
 
 $(PROJECT).a: ${OBJS}
-	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS)
+	$(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS)
 
 %.o: %.c 
-	$(CC) -c $(CFLAGS) $(INC_DIRS) -o $@ $<
-
-communications:
-	$(MAKE) -C ../communications
-
-dyn_devices:
-	$(MAKE) -C ../dyn_devices
-
-motion_manager:
-	$(MAKE) -C ../motion
-
-examples: 
-	$(MAKE) -C src/examples
-	$(MAKE) -C ../communications examples
-	$(MAKE) -C ../dyn_devices examples
-	$(MAKE) -C ../motion examples
-
-download: 
-	$(MAKE) -C src/examples download
+	$(CC) -c $(CFLAGS) -I$(IDIR) -o $@ $<
 
 clean:
-	rm -f ${LIB_DIR}$(PROJECT).a
+	rm -f ${LDIR}$(PROJECT).a
 	rm -f $(OBJS) 
-	$(MAKE) -C src/examples clean
-	$(MAKE) -C ../communications clean
-	$(MAKE) -C ../dyn_devices clean
-	$(MAKE) -C ../motion clean
diff --git a/controllers/include/adc.h b/controllers/include/adc.h
deleted file mode 100644
index a5b45094cf4da6a4d3b5f3280aeef76c3c175a63..0000000000000000000000000000000000000000
--- a/controllers/include/adc.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef _ADC_H
-#define _ADC_H
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-// ADC channel identifiers
-typedef enum {ADC_VCC=0,ADC_PORT_1=1,ADC_PORT_2=2,ADC_PORT_3=3,ADC_PORT_4=4,ADC_PORT_5=5,ADC_PORT_6=6} adc_t;
-#define         NUM_ADC               7
-
-/**
- * \brief Function to initialize the ADC module
- *
- * This function initializes the ADC module of the CM510 controller. This includes 
- * configuring the portA as output to sequentially enable each of the ADC channels
- * before starting the conversion, configuring the operation of the ADC module of
- * the micro and start the first conversion.
- *
- * Since the power supply to the external device is disabled by default, and only 
- * activated when the conversion is going to start, it is necessary to wait for a while
- * before actually starting the conversion to enssure the correct operation of the 
- * device (12 us in this case).
- * 
- * A part from the ADC module itself, this module also uses most of the PORT A of the
- * microcontroller (the 6 most significant bits).
- *
- * \return this function always returns 0
- */
-void init_adc(void);
-/**
- * \brief Function to get the last conversion 
- *
- * This functions returns the digital codification in raw binary of the analog
- * voltage present at the given channel. In Asynchronous mode, this function
- * returns immediatelly with the last caonverted value of the desired channel, 
- * which can be up to 0.5 ms old in the worst case.
- * 
- * In synchronous mode, the conversion is started when this function is called,
- * and it does not return until the conversion id done, which takes around
- * 125 us to complete.
- * 
- * \param channel_id identifier of the ADC channel to be converted. It must be
- *                   one of the macros defined above.
- * 
- * \return 0 if the functions is successfull and 0xFF in case of any error.
- */
-uint16_t get_adc_channel(adc_t channel_id);
-uint16_t get_adc_avg_channel(adc_t channel_id);
-
-#endif
diff --git a/controllers/include/buzzer.h b/controllers/include/buzzer.h
deleted file mode 100644
index 93c96b8ac81afee2d095a0ca58ff3727bcb06c92..0000000000000000000000000000000000000000
--- a/controllers/include/buzzer.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifndef _BUZZER_H
-#define _BUZZER_H
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-// buzzer interface
-typedef enum {NOTE_DO=0,NOTE_RE=1,NOTE_MI=2,NOTE_FA=3,NOTE_SOL=4,NOTE_LA=5,NOTE_SI=6} note_t;
-#define     NUM_NOTES                7
-
-/**
- * \brief Function to initialize the buzzed module
- *
- * This function initializes the buzzer module of the CM510 controller. The buzzer 
- * uses a single GPIO pin (bit 5 of port B) which is a PWM output and can be used
- * to generate different frequencies. The PWM module uses the timer 1.
- *
- * This function also initializes the table with the correspondence between the
- * notes and the actual value that has to be loaded to the PWM module.
- *
- * \return this function always returns 0
- */
-void init_buzzer(void);
-/**
- * \brief Function to start playing a note with the buzzer
- *
- * This function is used to start playing the given note with the buzzer. The
- * note is identified with the macros defined above. Once the buzzer module
- * is started, it will continue playing the same note until the buzzer_stop()
- * or the buzzer_chnage_note() functions are called.
- *
- * \param note identifier of the note to be played.
- * 
- * \return 0 if the function completes successfully and 0xFF otherwise.
- */
-void buzzer_start(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
-/**
- * \brief Function to change the note is being played
- *
- * This function changes the note that is being played if the buzzer is
- * started. Otherwise it has no effect. The note is identified with one of 
- * the macors defined above.
- * 
- * \param note identifier of the note to be played.
- * 
- * \return 0 if the function completes successfully and 0xFF otherwise.
- */
-void buzzer_change_note(note_t note);
-void buzzer_change_on_time(uint16_t time_100ms);
-void buzzer_change_off_time(uint16_t time_100ms);
-/**
- * \brief Function to stop the buzzer module
- *
- * This functions stops playing any note that is being played immediatelly.
- *
- * \return this function always returns 0
- */
-void buzzer_stop(void);
-
-uint8_t buzzer_is_playing(void);
-uint8_t buzzer_is_playing_alarm(void);
-
-#endif
diff --git a/controllers/include/cm510.h b/controllers/include/cm510.h
index 8fee1b362e0ce6b1a6c33b0dde5d4a02b5ecb1dc..13c441dc9171989cfb1c88e4d08f1c95b535da36 100755
--- a/controllers/include/cm510.h
+++ b/controllers/include/cm510.h
@@ -18,17 +18,6 @@
 #ifndef _CM510_H
 #define _CM510_H
 
-#include "cont_cfg.h"
-#include "motion_cfg.h"
-#include "comm_cfg.h"
-#include "gpio.h"
-#include "adc.h"
-#include "buzzer.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "user_time.h"
-#include "serial_console.h"
-
 // general interface
 /**
  * \brief Function to initialize all the cm510 modules
@@ -47,8 +36,353 @@
  * 
  * After all the initializations are done, the global interrupts are enabled.
  *
+ * \param adc_mode identifier of the desired operation mode of the analog to
+ *                 digital converter. The possible values are SYNC and ASYNC.
+ *
  * \return 0 if all the initializations complete successfully and 0xFF otherwise.
  */
-void init_cm510(void);
+unsigned char init_cm510(unsigned char adc_mode);
+
+// leds interface
+#define LED_BAT       0x01
+#define LED_TxD       0x02
+#define LED_RxD       0x04
+#define LED_AUX       0x08
+#define LED_MANAGE    0x10
+#define LED_PROGRAM   0x20
+#define LED_PLAY      0x40
+/**
+ * \brief Function to initialize the LED's module
+ *
+ * This function initializes the GPIO pins associated to the LED's as outputs.
+ * The LED's are connected to the 7 lower bits of port C.
+ *
+ * \return this function always returns 0
+ */
+unsigned char init_leds(void);
+/**
+ * \brief Function to turn on a given LED
+ *
+ * This function turns on a given LED, what ever its current state. The LED
+ * is identified by one of the macros defined above.
+ *
+ * \param led_id identifier of the LED to turn on
+ * 
+ * \return 0 if the function is successfull, 0xFF otherwise
+ */
+unsigned char turn_led_on(unsigned char led_id);
+/**
+ * \brief Function to turn off a given LED
+ *
+ * This function turns off a given LED, what ever its current state. The LED
+ * is identified by one of the macros defined above.
+ *
+ * \param led_id identifier of the LED to turn off
+ * 
+ * \return 0 if the function is successfull, 0xFF otherwise
+ */
+unsigned char turn_led_off(unsigned char led_id);
+/**
+ * \brief Function to toggle the current state of a LED
+ *
+ * This function changes the current state of a given LED. If it is on, it is turned
+ * off, and viceversa. The LED is identified by one of the macros defined above.
+ * 
+ * \param led_id identifier of the LED to toggle
+ * 
+ * \return 0 if the function is successfull, 0xFF otherwise
+ */
+unsigned char toggle_led(unsigned char led_id);
+/**
+ * \brief Function to check wether the LED is on or not
+ *
+ * This function checks if a given LED is turned on or off. The LED is identified 
+ * by one of the macros defined above.
+ * 
+ * \param led_id identifier of the LED to check
+ * 
+ * \return 0 if the LED is turned off, 1 if the led is turned on and 0xFF in 
+ *         case of any error.
+ */
+unsigned char is_led_on(unsigned char led_id);
+// buttons interface
+#define BTN_UP      0x10
+#define BTN_DOWN    0x20
+#define BTN_LEFT    0x40
+#define BTN_RIGHT   0x80
+#define BTN_START   0x01
+#define NUM_BUTTONS 5
+/**
+ * \brief Function to initialize the buttons module
+ *
+ * This function initializes the GPIO pins associated to the buttons as inputs.
+ * The buttons are connected to the four most significant bits of port E and the 
+ * least significant bit of port D.
+ *
+ * This function also sets to NULL the callback functions for all the buttons and
+ * initializes the current state of the buttons.
+ * 
+ * \return this function always returns 0
+ */
+unsigned char init_buttons(void);
+/**
+ * \brief Function to check whether a button is pressed or not
+ *
+ * This functions checks if a given value is pressed or not. The desired button 
+ * is identified with one of the macros defined above.
+ * 
+ * \param button_id identifier of the button to assign a callback. This must be any of 
+ *                  the macros defined above.
+ * 
+ * \return 0 if the button is not pressed, 1 if it is pressed and 0xFF in 
+ *         case of any error.
+ */
+unsigned char is_button_pressed(unsigned char button_id);
+/**
+ * \brief Function to assign a callback function to a button
+ *
+ * This function assigns a callback function to a button that will be called whenever
+ * the button is pressed (falling edge). The prototype of the callback function is as
+ * follows:
+ * 
+ * void callback_function(void *param)
+ *
+ * The callback function accepts a generic pointer that can be used to pass the necessary
+ * information to the callback function, if necessary.
+ * 
+ * \param button_id identifier of the button to assign a callback. This must be any of 
+ *                  the macros defined above.
+ * 
+ * \param function pointer to the callback function to be assigned to the button.
+ * 
+ * \param param a pointer to any data needed inside the callback function to perform its
+ *              function.
+ * 
+ * \return 0 if the function completes successfully, 0xFF otherwise.
+ */
+unsigned char button_assign_callback(unsigned char button_id, void (*function)(void *param),void *param);
+// ADC interface
+// ADC channel identifiers
+#define	        ADC_PORT_1    1
+#define         ADC_PORT_2    2
+#define	        ADC_PORT_3    3
+#define	        ADC_PORT_4    4
+#define	        ADC_PORT_5    5
+#define	        ADC_PORT_6    6
+#define         NUM_ADC       6
+// ADC operation modes
+#define         SYNC          0
+#define         ASYNC         1
+/**
+ * \brief Function to initialize the ADC module
+ *
+ * This function initializes the ADC module of the CM510 controller. This includes 
+ * configuring the portA as output to sequentially enable each of the ADC channels
+ * before starting the conversion, configuring the operation of the ADC module of
+ * the micro and start the first conversion.
+ *
+ * The ADC module can work in two different mode:
+ * 
+ * * Synchronous mode: in this case the module is idle until the get_adc_channel() 
+ *                     function is called. At that time, the desired channel is 
+ *                     enabled and the conversion started. The function block until
+ *                     the conversion is done.
+ *
+ * * Asynchronous mode: in this case the module is always sequentially converting each
+ *                      of the channels and saving the converted values in internal
+ *                      memory. When the get_adc_channel_function() is called, it
+ *                      returns immediatelly with the last converted value of the
+ *                      desired channel. In the worst case, the sample will be 0.6 ms 
+ *                      old.
+ *
+ * Since the power supply to the external device is disabled by default, and only 
+ * activated when the conversion is going to start, it is necessary to wait for a while
+ * before actually starting the conversion to enssure the correct operation of the 
+ * device (12 us in this case).
+ * 
+ * A part from the ADC module itself, this module also uses most of the PORT A of the
+ * microcontroller (the 6 most significant bits).
+ *
+ * \param mode the desired operation mode of the ADC module. This value can be either
+ *             SYNC or ASYNC.
+ *
+ * \return this function always returns 0
+ */
+unsigned char init_adc(unsigned char mode);
+/**
+ * \brief Function to get the last conversion 
+ *
+ * This functions returns the digital codification in raw binary of the analog
+ * voltage present at the given channel. In Asynchronous mode, this function
+ * returns immediatelly with the last caonverted value of the desired channel, 
+ * which can be up to 0.5 ms old in the worst case.
+ * 
+ * In synchronous mode, the conversion is started when this function is called,
+ * and it does not return until the conversion id done, which takes around
+ * 125 us to complete.
+ * 
+ * \param channel_id identifier of the ADC channel to be converted. It must be
+ *                   one of the macros defined above.
+ * 
+ * \return 0 if the functions is successfull and 0xFF in case of any error.
+ */
+unsigned short int get_adc_channel(unsigned char channel_id);
+// buzzer interface
+#define     NOTE_DO    0
+#define     NOTE_RE    1
+#define     NOTE_MI    2
+#define     NOTE_FA    3
+#define     NOTE_SOL   4
+#define     NOTE_LA    5
+#define     NOTE_SI    6
+#define     NUM_NOTES  7
+/**
+ * \brief Function to initialize the buzzed module
+ *
+ * This function initializes the buzzer module of the CM510 controller. The buzzer 
+ * uses a single GPIO pin (bit 5 of port B) which is a PWM output and can be used
+ * to generate different frequencies. The PWM module uses the timer 1.
+ *
+ * This function also initializes the table with the correspondence between the
+ * notes and the actual value that has to be loaded to the PWM module.
+ *
+ * \return this function always returns 0
+ */
+unsigned char init_buzzer(void);
+/**
+ * \brief Function to start playing a note with the buzzer
+ *
+ * This function is used to start playing the given note with the buzzer. The
+ * note is identified with the macros defined above. Once the buzzer module
+ * is started, it will continue playing the same note until the buzzer_stop()
+ * or the buzzer_chnage_note() functions are called.
+ *
+ * \param note identifier of the note to be played.
+ * 
+ * \return 0 if the function completes successfully and 0xFF otherwise.
+ */
+unsigned char buzzer_start(unsigned char note);
+/**
+ * \brief Function to change the note is being played
+ *
+ * This function changes the note that is being played if the buzzer is
+ * started. Otherwise it has no effect. The note is identified with one of 
+ * the macors defined above.
+ * 
+ * \param note identifier of the note to be played.
+ * 
+ * \return 0 if the function completes successfully and 0xFF otherwise.
+ */
+unsigned char buzzer_change_note(unsigned char note);
+/**
+ * \brief Function to stop the buzzer module
+ *
+ * This functions stops playing any note that is being played immediatelly.
+ *
+ * \return this function always returns 0
+ */
+unsigned char buzzer_stop(void);
+// timer interface
+/**
+ * \brief Function to initialize the user timer module
+ *
+ * This function initializes the user timer by default, but it does not start it.
+ * This modules uses the timer 3 of the microcontroller, but it does not use any
+ * GPIO pin.
+ *
+ * This timer can be used to wait for a given ammount of time blocking the processor
+ * or else to schedule a given task some time in the future. To set the desired
+ * ammount of time use the timer_set_time(function). Once started with the 
+ * timer_start() function, the timer can be stopped (timer_stop()) or waites until 
+ * it is done (is_timer_done() or timer_wait()).
+ *
+ * A callback function can be assigned so that it will be called when the desired
+ * time has ellapsed. The declaration of this function must be as follows:
+ *
+ * void <function name>(void *param)
+ *
+ * \return this function always returns 0x00
+ */
+unsigned char init_timer(void);
+/**
+ * \brief Function to set the desired period
+ *
+ * This function sets the desired period of the timer in miliseconds. At the
+ * moment only periods inferior to 4 seconds (4000 ms) are supported. The 
+ * lower bound is around 1 msdur to functional issues. This function configures 
+ * the internal timer, but it does not start it. In order to do so, call the 
+ * timer_start() function.
+ * 
+ * \param time_ms the desired period of time in miliseconds.
+ *
+ * \return 0 if the function completes successfully and 0xFF otherwise
+ */
+unsigned char timer_set_time(unsigned short int time_ms);
+/**
+ * \brief Function to start the timer module
+ *
+ * This function actually starts the timer module. After calling this function,
+ * the desired period of time begins counting. This function has no effect if the
+ * timer has not been set with the timer_set_time() function.
+ * 
+ * \return 0 if the function completes successfully and 0xFF otherwise.
+ */
+unsigned char timer_start(void);
+/**
+ * \brief Function to assign a callback function to the timer
+ * 
+ * This function assigns a callback function to the timer. The assigned function
+ * will be called when the desired time is done. If not changed, the same function 
+ * will be called if the timer is started again.
+ * 
+ * It is not mandatory to assign a callback function to the timer. The declaration 
+ * of this callback function must be as follows:
+ * 
+ * void <function name>(void *param);
+ *
+ * \param function a pointer to the desired callback function. It can be set to 0x0000
+ *                 to remove a previously assigned callback function.
+ *
+ * \param param an optional pointer to any data required by the callback function. This
+ *              parameter can be set to 0x0000 if no data is required.
+ * 
+ * \return 0 if the function completes successfully and 0xFF otherwise
+ */
+unsigned char timer_set_callback(void (*function)(void *param),void *param);
+/**
+ * \brief Function to stop the current timer
+ *
+ * This function stops the timer in case it has been started. Otherwise nothing
+ * is done. If the timer is stopped before completion, the callback function will
+ * not be called, and the timer will report is has completed the desired counting.
+ * 
+ * If the timer is re-started after stopping it, it will count the whole period
+ * configured by the timer_set_time(function).
+ *
+ * \return 0 if the function completes successfully and 0xFF otherwise
+ */
+unsigned char timer_stop(void);
+/**
+ * \brief Function to check whether the timer is done or not
+ *
+ * This function checks if the desired period has ellapsed or not. This function
+ * only checks the current state of the timer and returns immediatelly. To wait
+ * for the completion of the timer, use the timer_wait() function. 
+ *
+ * \return if the timer nas not been started ot is has finished counting, this 
+ *         function returns 0x01, if the counter is still counting, it returns
+ *         0x00, and 0xFF in case of any error.
+ */
+unsigned char is_timer_done(void);
+/**
+ * \brief Function to wait for the completion of the timer
+ *
+ * This function blocks until the desired time has ellapsed. It blocks until the
+ * timer reaches the desired time. In order to check if the timer has ellapsed or
+ * not use the is_timer_done() function.
+ * 
+ * \return 0 if the functions completes successfully and 0xFF otherwise.
+ */
+unsigned char timer_wait(void);
 
 #endif
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
deleted file mode 100644
index e46833f4107bd561f7ab31449eb09f2758ac060b..0000000000000000000000000000000000000000
--- a/controllers/include/cm510_cfg.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef _CM510_CFG_H
-#define _CM510_CFG_H
-
-#include "buzzer.h"
-
-// controller configuration parameters
-#define ADC_MAX_NUM_SAMPLES                     16
-#define ADC_SAMPLE_PERIOD_MS                    10
-#define ADC_VOLTAGE_ALARM_TH                    559
-#define ADC_VOLTAGE_ALARM_WINDOW                20
-#define ADC_VOLTAGE_ALARM_NOTE                  NOTE_LA
-#define ADC_VOLTAGE_ALARM_TIME_ON               30
-#define ADC_VOLTAGE_ALARM_TIME_OFF              30
-
-// communication configuration parameters
-#define DYN_MASTER_MAX_TX_BUFFER_LEN            128
-#define DYN_MASTER_MAX_RX_BUFFER_LEN            128
-#define DYN_MASTER_DEFAULT_BAUDRATE             1000000
-#define DYN_MASTER_DEFAULT_TIMEOUT_US           10000
-#define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
-
-// motion configuration parameters
-#define MANAGER_MAX_NUM_SERVOS                  18
-#define MANAGER_MISSING_SERVOS_ALARM_NOTE       NOTE_SOL
-#define MANAGER_MISSING_SERVOS_ALARM_TIME_ON    10
-#define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF   100
-#define BALANCE_GYRO_CAL_NUM_SAMPLES            10
-#define BALANCE_GYRO_X_CHANNEL                  3
-#define BALANCE_GYRO_Y_CHANNEL                  4
-#define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE      NOTE_SOL
-#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON   10
-#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF  100
-#define BALANCE_MAX_CAL_GYRO_ERROR              20.0
-#define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
-#define BALANCE_BACKWARD_FALL_THD_VALUE         200
-#define BALANCE_KNEE_GAIN                       (4.0/54.0)
-#define BALANCE_ANKLE_ROLL_GAIN                 (4.0/18.0)
-#define BALANCE_HIP_PITCH_GAIN                  (4.0/20.0)
-#define BALANCE_ANKLE_PITCH_GAIN                (4.0/40.0)
-
-#endif
diff --git a/controllers/include/cont_cfg.h b/controllers/include/cont_cfg.h
deleted file mode 100644
index 985b7b9a4e2abfc84c834cdcba1f249ac229ad84..0000000000000000000000000000000000000000
--- a/controllers/include/cont_cfg.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef _CONT_CFG_H
-#define _CONT_CFG_H
-
-#include "buzzer.h"
-
-// number of samples to average
-#ifndef ADC_MAX_NUM_SAMPLES
-  #define ADC_MAX_NUM_SAMPLES         16
-#endif
-
-#ifndef ADC_SAMPLE_PERIOD_MS
-  #define ADC_SAMPLE_PERIOD_MS        10
-#endif 
-
-#ifndef ADC_VOLTAGE_ALARM_TH
-  #define ADC_VOLTAGE_ALARM_TH        559
-#endif
-
-#ifndef ADC_VOLTAGE_ALARM_WINDOW      
-  #define ADC_VOLTAGE_ALARM_WINDOW    20
-#endif
-
-#ifndef ADC_VOLTAGE_ALARM_NOTE
-  #define ADC_VOLTAGE_ALARM_NOTE      NOTE_LA
-#endif
-
-#ifndef ADC_VOLTAGE_ALARM_TIME_ON
-  #define ADC_VOLTAGE_ALARM_TIME_ON   30
-#endif
-
-#ifndef ADC_VOLTAGE_ALARM_TIME_OFF
-  #define ADC_VOLTAGE_ALARM_TIME_OFF  30
-#endif
-
-#endif
diff --git a/controllers/include/gpio.h b/controllers/include/gpio.h
deleted file mode 100644
index 994899170dd675a33a5552c74a97f4320801ed4e..0000000000000000000000000000000000000000
--- a/controllers/include/gpio.h
+++ /dev/null
@@ -1,101 +0,0 @@
-#ifndef _GPIO_H
-#define _GPIO_H
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-// leds interface
-typedef enum {LED_BAT=0x01,LED_TxD=0x02,LED_RxD=0x04,LED_AUX=0x08,LED_MANAGE=0x10,LED_PROGRAM=0x20,LED_PLAY=0x40} leds_t;
-#define NUM_LEDS      7
-
-// buttons interface
-typedef enum {BTN_UP=0x10,BTN_DOWN=0x20,BTN_LEFT=0x40,BTN_RIGHT=0x80,BTN_START=0x01} pushbuttons_t;
-#define NUM_BUTTONS 5
-
-/**
- * \brief Function to initialize the LED's module
- *
- * This function initializes the GPIO pins associated to the LED's as outputs.
- * The LED's are connected to the 7 lower bits of port C.
- *
- * \return this function always returns 0
- */
-void init_leds(void);
-/**
- * \brief Function to turn on a given LED
- *
- * This function turns on a given LED, what ever its current state. The LED
- * is identified by one of the macros defined above.
- *
- * \param led_id identifier of the LED to turn on
- * 
- * \return 0 if the function is successfull, 0xFF otherwise
- */
-void turn_led_on(leds_t led_id);
-/**
- * \brief Function to turn off a given LED
- *
- * This function turns off a given LED, what ever its current state. The LED
- * is identified by one of the macros defined above.
- *
- * \param led_id identifier of the LED to turn off
- * 
- * \return 0 if the function is successfull, 0xFF otherwise
- */
-void turn_led_off(leds_t led_id);
-/**
- * \brief Function to toggle the current state of a LED
- *
- * This function changes the current state of a given LED. If it is on, it is turned
- * off, and viceversa. The LED is identified by one of the macros defined above.
- * 
- * \param led_id identifier of the LED to toggle
- * 
- * \return 0 if the function is successfull, 0xFF otherwise
- */
-void toggle_led(leds_t led_id);
-/**
- * \brief Function to check wether the LED is on or not
- *
- * This function checks if a given LED is turned on or off. The LED is identified 
- * by one of the macros defined above.
- * 
- * \param led_id identifier of the LED to check
- * 
- * \return 0 if the LED is turned off, 1 if the led is turned on and 0xFF in 
- *         case of any error.
- */
-uint8_t is_led_on(leds_t led_id);
-
-/**
- * \brief Function to initialize the buttons module
- *
- * This function initializes the GPIO pins associated to the buttons as inputs.
- * The buttons are connected to the four most significant bits of port E and the 
- * least significant bit of port D.
- *
- * This function also sets to NULL the callback functions for all the buttons and
- * initializes the current state of the buttons.
- * 
- * \return this function always returns 0
- */
-void init_buttons(void);
-/**
- * \brief Function to check whether a button is pressed or not
- *
- * This functions checks if a given value is pressed or not. The desired button 
- * is identified with one of the macros defined above.
- * 
- * \param button_id identifier of the button to assign a callback. This must be any of 
- *                  the macros defined above.
- * 
- * \return 0 if the button is not pressed, 1 if it is pressed and 0xFF in 
- *         case of any error.
- */
-uint8_t is_button_pressed(pushbuttons_t button_id);
-
-uint8_t is_button_rising_edge(pushbuttons_t button_id);
-
-uint8_t is_button_falling_edge(pushbuttons_t button_id);
-
-#endif
diff --git a/controllers/include/user_time.h b/controllers/include/user_time.h
deleted file mode 100644
index ab080482202830d0982cb083051b1ee26e623483..0000000000000000000000000000000000000000
--- a/controllers/include/user_time.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _USER_TIME_H
-#define _USER_TIME_H
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-/* public functions */
-void init_user_time(void);
-void user_time_set_period(uint16_t period_ms);
-void user_time_set_one_time(uint16_t time_ms);
-uint8_t user_time_is_period_done(void);
-uint8_t user_time_is_done(void);
-void user_time_stop(void);
-
-#endif
diff --git a/controllers/src/adc.c b/controllers/src/adc.c
deleted file mode 100644
index be4e5efc8944cddb57337f488729b31ff71b683d..0000000000000000000000000000000000000000
--- a/controllers/src/adc.c
+++ /dev/null
@@ -1,167 +0,0 @@
-#include "cm510_cfg.h"
-#include "adc.h"
-#include "buzzer.h"
-#include <util/delay.h>
-
-/* external functions */
-void buzzer_start_alarm(note_t note, uint16_t on_time_100ms, uint16_t off_time_100ms);
-void buzzer_stop_alarm(void);
-uint8_t buzzer_is_playing_alarm(void);
-
-/* private variables */
-volatile uint16_t adc_ch_data[NUM_ADC][ADC_MAX_NUM_SAMPLES];
-volatile uint16_t adc_values[NUM_ADC];
-volatile uint16_t adc_avg_values[NUM_ADC];
-volatile uint8_t adc_current_ch;
-volatile uint8_t adc_current_sample;
-volatile uint8_t adc_voltage_alarm;
-
-/* private functions */
-void adc_set_channel(uint8_t ch_id)
-{
-  ADMUX&=0xF8;
-  ADMUX|=(ch_id&0x07);
-  PORTA=~(0x80>>ch_id);// enable the desired channel
-}
-
-void adc_set_sample_period(void)
-{
-  uint16_t prescaler_values[7]={1,8,32,64,128,256,1024};
-  uint16_t compare_value;
-  uint8_t i;
-
-  for(i=0;i<5;i++)
-  {
-    compare_value=(((F_CPU/1000)*ADC_SAMPLE_PERIOD_MS)/(prescaler_values[i]*NUM_ADC))-1;
-    if(compare_value<256)
-    {
-      OCR2A=(uint8_t)compare_value;
-      TCCR2B=i+1; 
-      break;
-    }
-  }
-}
-
-void adc_start_conv(void)
-{
-  ADCSRA|=(1<<ADSC);
-}
-
-uint8_t adc_is_conversion_done(void)
-{
-  if(ADCSRA&(1<<ADIF))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-uint8_t adc_is_period_done(void)
-{
-  if(TIFR2&(1<<OCF2A))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void adc_loop(void)
-{
-  uint8_t i;
-  uint16_t data;
-
-  if(adc_is_period_done())
-  {
-    TIFR2|=(1<<OCF2A)|(1<<OCF2B)|(1<<TOV2);// clear interrupt flag
-    TCNT2=0x00;// start from 0
-    // start a new conversion
-    adc_start_conv();
-  }
-  else
-  {
-    if(adc_is_conversion_done())
-    {
-      ADCSRA|=(1<<ADIF);// clear interrupt flag
-      data=(ADCL | (ADCH<<8));
-      adc_values[adc_current_ch]=data;
-      adc_ch_data[adc_current_ch][adc_current_sample]=data;
-      // compute the average for the current channel
-      data=0;
-      for(i=0;i<ADC_MAX_NUM_SAMPLES;i++)
-        data+=adc_ch_data[adc_current_ch][i];
-      data=data/ADC_MAX_NUM_SAMPLES;
-      adc_avg_values[adc_current_ch]=data;
-      if(adc_current_ch==NUM_ADC-1)// last channel
-        adc_current_sample=(adc_current_sample+1)%ADC_MAX_NUM_SAMPLES;
-      if(adc_current_ch==ADC_VCC)// monitor the Voltage supply
-      {
-//        voltage_mv=((uint32_t)data*(uint32_t)5000*(uint32_t)133)/((uint16_t)1024*(uint16_t)33);
-        // compare with hysteresis
-        if(adc_voltage_alarm==0x01)
-        {
-          if(data>=(ADC_VOLTAGE_ALARM_TH+ADC_VOLTAGE_ALARM_WINDOW))// voltage under 11 V
-          {
-            buzzer_stop_alarm();
-            adc_voltage_alarm=0x00;
-          }
-        }
-        else
-        {
-          if(data<(ADC_VOLTAGE_ALARM_TH-ADC_VOLTAGE_ALARM_WINDOW))// voltage under 11 V
-          {
-            buzzer_start_alarm(NOTE_LA,30,30);
-            adc_voltage_alarm=0x01;
-          }
-        }
-      }
-      adc_current_ch=(adc_current_ch+1)%NUM_ADC;
-      adc_set_channel(adc_current_ch);      
-    }
-  }
-}
-
-/* public functions */
-void init_adc(void)
-{
-  uint8_t i=0,j=0;
-
-  ADCSRA=(1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);// enable ADC set prescaler 1/128
-                                                                  // disable auto trigger, interrupts disabled
-  ADCSRB=0x00;// free running mode
-  ADMUX=0x00;// use external reference, right ajust the result
-  DIDR0=0x00;// enable all ADC channels
-  DIDR2=0x00;// enable all adc channels
-
-  // configure the GPIO to enable the sensors
-  DDRA  = 0xFC;
-  PORTA = 0xFC;// all sensors disabled
-
-  // initialize internal sample variables to 12V to avoid initial voltage alarm
-  for(i=0;i<NUM_ADC;i++)
-  {
-    adc_values[i]=0x0266;//equivalent to 12 V
-    adc_avg_values[i]=0x0266;//equivalent to 12 V
-    for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
-      adc_ch_data[i][j]=0x0266;
-  }
-  adc_current_ch=0;
-  adc_voltage_alarm=0x00;
-
-  // configure the timer 2 to perform periodic conversions (1 ms period)
-  TCCR2A=(1<<WGM21);// CTC mode, no output
-  TCNT2=0x00;// start from 0
-  TIMSK2=0x00;// interrupts not enabled
-  TIFR2=0x07;// clear any pending interrupt
-  adc_set_sample_period();// set the default sample period
-  
-  // enable the first ADC channel
-  adc_set_channel(adc_current_ch);
-}
-
-uint16_t get_adc_channel(adc_t channel_id)
-{
-  return adc_values[(uint8_t)channel_id];
-}
-
-uint16_t get_adc_avg_channel(adc_t channel_id)
-{
-  return adc_avg_values[(uint8_t)channel_id];
-}
diff --git a/controllers/src/buzzer.c b/controllers/src/buzzer.c
deleted file mode 100644
index 115cd534a008eb8e8198e3d11f7cd0465bee8f8b..0000000000000000000000000000000000000000
--- a/controllers/src/buzzer.c
+++ /dev/null
@@ -1,239 +0,0 @@
-#include "buzzer.h"
-#include "adc.h"
-#include "gpio.h"
-
-/* private variables */
-uint8_t buzzer_note_freq[NUM_NOTES];
-uint8_t buzzer_playing;
-uint8_t buzzer_playing_alarm;
-uint16_t buzzer_time_on_100ms;
-uint16_t buzzer_time_off_100ms;
-uint16_t buzzer_note;
-
-/* private functions */
-void buzzer_loop(void)
-{
-  static uint8_t playing=0x00;
-  static uint16_t current_time_on=0,current_time_off=0;
-
-  if(TIFR3&(1<<OCF3A))
-  {
-    TIFR3|=(1<<OCF3A);// clear any interrupt
-    TCNT3H=0x00;                   
-    TCNT3L=0x00;
-    if(playing==0x00)// currently not playing
-    {
-      if(buzzer_playing==0x01 || buzzer_playing_alarm==0x01)
-      {
-        playing=0x01;
-        current_time_on=buzzer_time_on_100ms-1;
-        current_time_off=buzzer_time_off_100ms;
-        if(current_time_on==0)
-        {
-          // disable PWM output
-          TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
-        }
-      }
-    }  
-    else// currently playing
-    {
-      if(buzzer_playing==0x01 || buzzer_playing_alarm==0x01)
-      {
-        if(current_time_on>0)
-        {
-          current_time_on--;
-          if(current_time_on==0)
-          {
-            // disable PWM output
-            TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
-          }
-        }
-        else
-        {
-          if(current_time_off>0)
-            current_time_off--;
-          else
-          {
-            // enable the output
-            TCCR1A&=(~(1<<COM1A0));
-            TCCR1A|=(1<<COM1A1);
-            current_time_on=buzzer_time_on_100ms;
-            current_time_off=buzzer_time_off_100ms;
-          }
-        }
-      }
-      else
-        playing=0x00;
-    }
-  }
-}
-
-void buzzer_stop_alarm(void)
-{
-  if(buzzer_playing_alarm==0x01)
-  {
-    // disable PWM output
-    TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
-    buzzer_playing_alarm=0x00;
-  }
-}
-
-void buzzer_start_alarm(note_t note, uint16_t on_time_100ms,uint16_t off_time_100ms)
-{
-  if(buzzer_playing_alarm)// the alram is not playing
-    buzzer_stop_alarm();
-  else if(buzzer_playing)// if the normal buzzer is playing, stop it.
-    buzzer_stop();
-  // set top count value
-  ICR1H = 0x00;
-  ICR1L = buzzer_note_freq[note];
-  // Set the compare value to half the period
-  OCR1AH = 0x00;
-  OCR1AL = buzzer_note_freq[note]>>1;
-  buzzer_time_on_100ms=on_time_100ms;
-  buzzer_time_off_100ms=off_time_100ms;
-  buzzer_note=note;
-  // enable PWM output
-  TCCR1A&=(~(1<<COM1A0));
-  TCCR1A|=(1<<COM1A1);
-  buzzer_playing_alarm=0x01;
-}
-
-/* public functions */
-void init_buzzer(void)
-{
-  // configure the GPIO
-  DDRB  |= 0x20;// configure bit 5 as output
-  PORTB &= 0xDF;// clear the ouput
-
-  // initialize timer 1 to work as PWM (Fast PWM mode)
-  TIMSK1=0x00;// disable all interrupts
-  TIFR1=0x2F;// clear any pending interrupt
-  TCNT1H=0x00;                   
-  TCNT1L=0x00;
-  // set PWM mode with ICR top-count
-  TCCR1A=0x00;
-  TCCR1A&=~(1<<WGM10);
-  TCCR1A|=(1<<WGM11);
-  TCCR1B=0x00;
-  TCCR1B|=(1<<WGM12);
-  TCCR1B|=(1<<WGM13);
-  // clear output compare value A
-  OCR1AH=0x00;
-  OCR1AL=0x00;
-  TCCR1B&=0xF8;// set the prescaler to 1024
-  TCCR1B|=0x05;// set the prescaler to 1024
-
-  // initialize timer3 for buzzer time control (100ms period)
-  TIMSK3=0x00;// disable all interrupts
-  TIFR3=0x2F;// clear any pending interrupt
-  TCNT3H=0x00;                   
-  TCNT3L=0x00;
-  // set PWM mode with ICR top-count (CTC mode)
-  TCCR3A=0x00;
-  TCCR3A&=~(1<<WGM30);
-  TCCR3A&=~(1<<WGM31);
-  TCCR3B=0x00;
-  TCCR3B|=(1<<WGM32);
-  TCCR3B&=~(1<<WGM33);
-  // clear output compare value A
-  OCR3AH=0x18;
-  OCR3AL=0x6A;
-  TCCR3B&=0xF8;// set the prescaler to 256
-  TCCR3B|=0x04;
-
-  // initialize the note frequencies
-  buzzer_note_freq[0]=29;
-  buzzer_note_freq[1]=26;
-  buzzer_note_freq[2]=24;
-  buzzer_note_freq[3]=22;
-  buzzer_note_freq[4]=20;
-  buzzer_note_freq[5]=18;
-  buzzer_note_freq[6]=16;
-  buzzer_playing=0x00;
-  buzzer_playing_alarm=0x00;
-  buzzer_time_on_100ms=0x01;
-  buzzer_time_off_100ms=0x01;
-  buzzer_note=NOTE_DO;
-}
-
-void buzzer_start(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms)
-{
-  if(buzzer_playing_alarm==0x00)
-  {
-    if(buzzer_playing==0x00)
-    {
-      // set top count value
-      ICR1H = 0x00;
-      ICR1L = buzzer_note_freq[note];
-      // Set the compare value to half the period
-      OCR1AH = 0x00;
-      OCR1AL = buzzer_note_freq[note]>>1;
-      buzzer_note=note;
-      buzzer_time_on_100ms=on_time_100ms;
-      buzzer_time_off_100ms=off_time_100ms;
-      // enable PWM output
-      TCCR1A&=(~(1<<COM1A0));
-      TCCR1A|=(1<<COM1A1);
-      buzzer_playing=0x01;
-    }
-  }
-}  
-
-void buzzer_change_note(note_t note)
-{
-  if(buzzer_playing_alarm==0x00)
-  {
-    if(buzzer_playing==0x01)
-    {
-      // set top count value
-      ICR1H = 0x00;
-      ICR1L = buzzer_note_freq[note];
-      // Set the compare value to half the period
-      OCR1AH = 0x00;
-      OCR1AL = buzzer_note_freq[note]>>1;
-      buzzer_note=note;
-    }
-  }
-}
-
-void buzzer_change_on_time(uint16_t time_100ms)
-{
-  if(buzzer_playing_alarm==0x00)
-  {
-    if(buzzer_playing==0x01)
-      buzzer_time_on_100ms=time_100ms;
-  }
-}
-
-void buzzer_change_off_time(uint16_t time_100ms)
-{
-  if(buzzer_playing_alarm==0x00)
-  {
-    if(buzzer_playing==0x01)
-      buzzer_time_off_100ms=time_100ms;
-  }
-}
-
-void buzzer_stop(void)
-{
-  if(buzzer_playing_alarm==0x00)
-  {
-    if(buzzer_playing==0x01)
-    {
-      // disable PWM output
-      TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
-      buzzer_playing=0x00;
-    }
-  }
-}
-
-uint8_t buzzer_is_playing(void)
-{
-  return buzzer_playing;
-}
-
-uint8_t buzzer_is_playing_alarm(void)
-{
-  return buzzer_playing_alarm;
-}
diff --git a/controllers/src/cm510.c b/controllers/src/cm510.c
deleted file mode 100755
index 316ab01dded22f064b6e417654f27ecc27bafcc7..0000000000000000000000000000000000000000
--- a/controllers/src/cm510.c
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- *  Copyright (c) 2011 Humanoid Lab
- * 
- *  This file is part of iri-libbioloid. iri-libbioloid is free software: you
- *  can redistribute it and/or modify it under the terms of the GNU Lesser
- *  Public License as published by the Free Software Foundation, either 
- *  version 3 of the License, or (at your option) any later version.
- * 
- *  iri-libbioloid is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU Lesser Public License for more details.
- * 
- *  You should have received a copy of the GNU Lesser Public License
- *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <inttypes.h>
-#include <util/delay.h>
-#include "cm510_cfg.h"
-#include "cm510.h"
-
-/* external user functions */
-extern void user_init(void);
-extern void user_loop(void);
-
-/* internal loop functions */
-extern void pushbuttons_loop(void);
-extern void adc_loop(void);
-extern void buzzer_loop(void);
-extern void manager_loop(void);
-extern void user_time_loop(void);
-
-// general interface
-void init_cm510(void)
-{
-  init_leds();
-  init_buttons();
-  init_adc();
-  init_buzzer();
-  init_user_time();
-}
-
-int16_t main(void)
-{
-  init_cm510();
-  sei();
-  manager_init(MANAGER_MAX_NUM_SERVOS);
-  /* call the user initialization function */
-  user_init();
-  // turn BAT_LED on to indicate the initialization is done
-  turn_led_on(LED_BAT);
-
-  while(1)
-  {
-    pushbuttons_loop();
-    adc_loop();
-    buzzer_loop();
-    manager_loop();
-    user_time_loop();
-    user_loop();
-  }
-}
diff --git a/controllers/src/cm510/cm510.c b/controllers/src/cm510/cm510.c
new file mode 100755
index 0000000000000000000000000000000000000000..b038601554ec3a25d7655ca539c2a7dc48cc09b8
--- /dev/null
+++ b/controllers/src/cm510/cm510.c
@@ -0,0 +1,580 @@
+/*
+ *  Copyright (c) 2011 Humanoid Lab
+ * 
+ *  This file is part of iri-libbioloid. iri-libbioloid is free software: you
+ *  can redistribute it and/or modify it under the terms of the GNU Lesser
+ *  Public License as published by the Free Software Foundation, either 
+ *  version 3 of the License, or (at your option) any later version.
+ * 
+ *  iri-libbioloid is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Lesser Public License for more details.
+ * 
+ *  You should have received a copy of the GNU Lesser Public License
+ *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "cm510.h"
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <inttypes.h>
+#include <util/delay.h>
+
+// global private variables
+typedef void (*func_ptr)(void *param);
+volatile unsigned short int timer_prescaler_values[6];
+
+// button private variables
+volatile func_ptr button_func_ptr[NUM_BUTTONS];
+void *button_func_param[NUM_BUTTONS];
+volatile unsigned char button_old_value[NUM_BUTTONS];
+
+// ADC private variables
+volatile unsigned short int adc_values[NUM_ADC];
+volatile unsigned char adc_current_channel;
+volatile unsigned char adc_mode;
+
+// buzzer private variables
+unsigned char buzzer_note_freq[NUM_NOTES];
+
+// timer private variables
+volatile func_ptr timer_func;
+void *timer_param;
+volatile unsigned char timer_done;
+volatile unsigned char timer_prescaler;
+
+// initialize the timer 0 for several internal functions (T=8ms)
+SIGNAL(TIMER4_OVF_vect)
+{
+  unsigned char button_new_value[NUM_BUTTONS];
+
+  // reset the timer
+  TCNT4H=0xC1;	                 // set the period to 1ms TCNT2
+  TCNT4L=0x7F;
+
+  // detect falling edge on the buttons
+  button_new_value[0]=((~PINE & BTN_UP)>>4);
+  if(button_new_value[0]==0x01 && button_old_value[0]==0x00)
+    if(button_func_ptr[0]!=0x0000)
+      button_func_ptr[0](button_func_param[0]);// call the callback function
+  button_old_value[0]=button_new_value[0];
+  button_new_value[1]=((~PINE & BTN_DOWN)>>5);
+  if(button_new_value[1]==0x01 && button_old_value[1]==0x00)
+    if(button_func_ptr[1]!=0x0000)
+      button_func_ptr[1](button_func_param[1]);// call the callback function
+  button_old_value[1]=button_new_value[1];
+  button_new_value[2]=((~PINE & BTN_LEFT)>>6);
+  if(button_new_value[2]==0x01 && button_old_value[2]==0x00)
+    if(button_func_ptr[2]!=0x0000)
+      button_func_ptr[2](button_func_param[2]);// call the callback function
+  button_old_value[2]=button_new_value[2];
+  button_new_value[3]=((~PINE & BTN_RIGHT)>>7);
+  if(button_new_value[3]==0x01 && button_old_value[3]==0x00)
+    if(button_func_ptr[3]!=0x0000)
+      button_func_ptr[3](button_func_param[3]);// call the callback function
+  button_old_value[3]=button_new_value[3];
+  button_new_value[4]=((~PIND & BTN_START));
+  if(button_new_value[4]==0x01 && button_old_value[4]==0x00)
+    if(button_func_ptr[4]!=0x0000)
+      button_func_ptr[4](button_func_param[4]);// call the callback function
+  button_old_value[4]=button_new_value[4];
+  if(adc_mode==ASYNC)
+  {
+    // convert the next channel
+    ADCSRA |= (1 << ADIF);                // AD-Conversion Interrupt Flag Clear
+    adc_values[adc_current_channel]=ADC;
+    adc_current_channel=(adc_current_channel+1)%NUM_ADC;
+    // enable next channel
+    PORTA=~(0x80>>adc_current_channel);
+    // select next channel
+    ADMUX=(adc_current_channel+1);
+    // wait for the signal to be stable
+    _delay_us(12);
+    // start conversion on first channel
+    ADCSRA|=(1 << ADSC);
+  }
+}
+
+void init_cm510_timer(void)
+{
+  // initialize timer 4
+  TCCR4B=((TCCR4B & ~0x07) | 0x02);// set the prescaler to 64 
+  TCNT4H=0xC1;	                 // set the period to 8ms TCNT4
+  TCNT4L=0x7F;
+  TIMSK4|=(0x01<<TOIE4);              // enable TCNT4 overflow
+}
+
+// general interface
+unsigned char init_cm510(unsigned char adc_mode)
+{
+  timer_prescaler_values[0]=0;
+  timer_prescaler_values[1]=1;
+  timer_prescaler_values[2]=8;
+  timer_prescaler_values[3]=64;
+  timer_prescaler_values[4]=256;
+  timer_prescaler_values[5]=1024;
+
+  init_leds();
+  init_buttons();
+  if(init_adc(adc_mode)==0xFF)
+    return 0xFF;
+  init_buzzer();
+  init_timer();
+  init_cm510_timer();
+
+  sei();
+
+  return 0x00;
+}
+
+// leds interface
+unsigned char init_leds(void)
+{
+  // configure the PORTC as outputs for the LED's
+  DDRC  = 0x7F;
+  // all LED's off except for the Battery LED
+  PORTC = 0x7E;
+
+  return 0x00;
+}
+
+unsigned char turn_led_on(unsigned char led_id)
+{
+  unsigned char ret_value=0x00;
+
+  switch(led_id)
+  {
+    case LED_BAT: PORTC &= ~LED_BAT;
+                  break;
+    case LED_TxD: PORTC &= ~LED_TxD;
+                  break;
+    case LED_RxD: PORTC &= ~LED_RxD;
+                  break;
+    case LED_AUX: PORTC &= ~LED_AUX;
+                  break;
+    case LED_MANAGE: PORTC &= ~LED_MANAGE;
+                     break;
+    case LED_PROGRAM: PORTC &= ~LED_PROGRAM;
+                      break;
+    case LED_PLAY: PORTC &= ~LED_PLAY;
+                   break;
+    default: ret_value=0xFF;
+             break;
+  }
+
+  return ret_value;
+}
+
+unsigned char turn_led_off(unsigned char led_id)
+{
+  unsigned char ret_value=0x00;
+
+  switch(led_id)
+  {
+    case LED_BAT: PORTC |= LED_BAT;
+                  break;
+    case LED_TxD: PORTC |= LED_TxD;
+                  break;
+    case LED_RxD: PORTC |= LED_RxD;
+                  break;
+    case LED_AUX: PORTC |= LED_AUX;
+                  break;
+    case LED_MANAGE: PORTC |= LED_MANAGE;
+                     break;
+    case LED_PROGRAM: PORTC |= LED_PROGRAM;
+                      break;
+    case LED_PLAY: PORTC |= LED_PLAY;
+                   break;
+    default: ret_value=0xFF;
+             break;
+  }
+
+  return ret_value;
+}
+
+unsigned char toggle_led(unsigned char led_id)
+{
+  unsigned char ret_value=0x00;
+
+  switch(led_id)
+  {
+    case LED_BAT: PORTC ^= LED_BAT;
+                  break;
+    case LED_TxD: PORTC ^= LED_TxD;
+                  break;
+    case LED_RxD: PORTC ^= LED_RxD;
+                  break;
+    case LED_AUX: PORTC ^= LED_AUX;
+                  break;
+    case LED_MANAGE: PORTC ^= LED_MANAGE;
+                     break;
+    case LED_PROGRAM: PORTC ^= LED_PROGRAM;
+                      break;
+    case LED_PLAY: PORTC ^= LED_PLAY;
+                   break;
+    default: ret_value=0xFF;
+             break;
+  }
+
+  return ret_value;
+}
+
+unsigned char is_led_on(unsigned char led_id)
+{
+  unsigned char value=0x00;
+
+  switch(led_id)
+  {
+    case LED_BAT: value = (PINC & LED_BAT)>>6;
+                  break;
+    case LED_TxD: value = (PINC & LED_TxD)>>5;
+                  break;
+    case LED_RxD: value = (PINC & LED_RxD)>>4;
+                  break;
+    case LED_AUX: value = (PINC & LED_AUX)>>3;
+                  break;
+    case LED_MANAGE: value = (PINC & LED_MANAGE)>>2;
+                     break;
+    case LED_PROGRAM: value = (PINC & LED_PROGRAM)>>1;
+                      break;
+    case LED_PLAY: value = (PINC & LED_PLAY);
+                   break;
+    default: value=0xFF;
+             break;
+  }
+
+  return value;  
+}
+
+// buttons interface
+unsigned char init_buttons(void)
+{
+  unsigned char i=0;
+
+  DDRD  &= 0xFE;
+  PORTD |= 0x01;
+	
+  DDRE  &= 0x0F;
+  PORTE |= 0xF0;
+
+  for(i=0;i<NUM_BUTTONS;i++)
+  {
+    button_func_ptr[i]=0x0000;
+    button_func_ptr[i]=0x0000;
+    button_old_value[i]=0x00;
+  }
+
+  return 0x00;
+}
+
+unsigned char is_button_pressed(unsigned char button_id)
+{
+  unsigned char value;
+
+  switch(button_id)
+  {
+    case BTN_UP: value=(~PINE & BTN_UP) >> 4;
+                 break;
+    case BTN_DOWN: value=(~PINE & BTN_DOWN) >> 5;
+                   break;
+    case BTN_LEFT: value=(~PINE & BTN_LEFT) >> 6;
+                   break;
+    case BTN_RIGHT: value=(~PINE & BTN_RIGHT) >> 7;
+                    break;
+    case BTN_START: value=(~PIND & BTN_START);
+                    break;
+    default: value=0xFF;
+             break;
+  }
+
+  return value;
+}
+
+unsigned char button_assign_callback(unsigned char button_id, void (*function)(void *param),void *param)
+{
+  unsigned char value=0x00;
+
+  switch(button_id)
+  {
+    case BTN_UP: button_func_ptr[0]=function;
+                 button_func_param[0]=param;
+                 break;
+    case BTN_DOWN: button_func_ptr[1]=function;
+                   button_func_param[1]=param;
+                   break;
+    case BTN_LEFT: button_func_ptr[2]=function;
+                   button_func_param[2]=param;
+                   break;
+    case BTN_RIGHT: button_func_ptr[3]=function;
+                    button_func_param[3]=param;
+                    break;
+    case BTN_START: button_func_ptr[4]=function;
+                    button_func_param[4]=param;
+                    break;
+    default: value=0xFF;
+             break;
+  }
+
+  return value;
+}
+
+// ADC interface
+ISR(ADC_vect)
+{
+  ADCSRA |= (1 << ADIF);		// AD-Conversion Interrupt Flag Clear
+  adc_values[adc_current_channel]=ADC;
+  adc_current_channel=(adc_current_channel+1)%NUM_ADC;
+  // enable next channel
+  PORTA=~(0x80>>adc_current_channel);
+  // select next channel
+  ADMUX=(adc_current_channel+1);
+  // wait for the signal to be stable
+  _delay_us(12);
+  // start conversion on first channel
+  ADCSRA|=(1 << ADSC);
+}
+
+unsigned char init_adc(unsigned char mode)
+{
+  unsigned char i=0;
+
+  ADCSRA=(1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);// enable ADC set prescaler 1/128
+                                                                  // disable auto trigger
+  ADCSRB=0x00;
+  ADMUX=ADC_PORT_1; // use external reference, right ajust the result
+                    // select first channel
+
+  // configure the GPIO to enable the sensors
+  DDRA  = 0xFC;
+  PORTA = 0xFC;// all sensors disabled
+
+  for(i=0;i<NUM_ADC;i++)
+    adc_values[i]=0x0000;
+  adc_current_channel=0;
+  adc_mode=mode;
+
+  if(mode==ASYNC)
+  {
+//    ADCSRA|=(1<<ADIE);// enable interrupts 
+    // enable first sensor
+    PORTA&= ~0x80;
+    // wait for the signal to be stable
+    _delay_us(12);
+    // start conversion on first channel
+    ADCSRA|=(1 << ADSC);
+  }
+
+  return 0x00;
+}
+
+unsigned short int get_adc_channel(unsigned char channel_id)
+{
+  if(channel_id>NUM_ADC)
+    return 0xFFFF;
+  else
+  {
+    if(adc_mode==ASYNC)
+      return adc_values[channel_id-1];
+    else
+    {
+      PORTA=~(0x80>>(channel_id-1));// enable the desired channel
+      _delay_us(12);
+      ADMUX=((ADMUX&0xF8)|channel_id);
+      ADCSRA |= (1 << ADIF);// AD-Conversion Interrupt Flag Clear
+      ADCSRA |= (1 << ADSC);// start the conversion
+      while( !(ADCSRA & (1 << ADIF)) );// Wait until AD-Conversion completes    
+      PORTA = 0xFC;// disable all sensors
+      return ADC;
+    }
+  }
+}
+
+// buzzer interface
+unsigned char init_buzzer(void)
+{
+  // configure the GPIO
+  DDRB  |= 0x20;// configure bit 5 as output
+  PORTB &= 0xDF;// clear the ouput
+
+  // initialize timer 1 to work as PWM (Fast PWM mode)
+  TCCR1B=((TCCR1B & ~0x07) | 0x00);// set the prescaler to 1024
+  TCNT1H=0x00;                   
+  TCNT1L=0x00;
+  // set PWM mode with ICR top-count
+  TCCR1A&=~(1<<WGM10);
+  TCCR1A|=(1<<WGM11);
+  TCCR1B|=(1<<WGM12);
+  TCCR1B|=(1<<WGM13);
+  // clear output compare value A
+  OCR1AH=0x00;
+  OCR1AL=0x00;
+
+  // initialize the note frequencies
+  buzzer_note_freq[0]=29;
+  buzzer_note_freq[1]=26;
+  buzzer_note_freq[2]=24;
+  buzzer_note_freq[3]=22;
+  buzzer_note_freq[4]=20;
+  buzzer_note_freq[5]=18;
+  buzzer_note_freq[6]=16;
+
+  return 0x00;
+}
+
+unsigned char buzzer_start(unsigned char note)
+{
+  if(note>=NUM_NOTES)
+    return 0xFF;
+  else
+  {
+    TCCR1B=((TCCR1B & ~0x07) | 0x05);// set the prescaler to 1024
+    // set top count value
+    ICR1H = 0x00;
+    ICR1L = buzzer_note_freq[note];
+    // Set the compare value to half the period
+    OCR1AH = 0x00;
+    OCR1AL = buzzer_note_freq[note]>>1;
+    // turn on channel A (OC1A) PWM output
+    // set OC1A as non-inverted PWM
+    TCCR1A|=(1<<COM1A1);
+    TCCR1A&=(~(1<<COM1A0));
+ 
+    return 0x00;
+  }
+}  
+
+unsigned char buzzer_change_note(unsigned char note)
+{
+  if(note>=NUM_NOTES)
+    return 0xFF;
+  else
+  {
+	cli();
+    // set top count value
+    ICR1H = 0x00;
+    ICR1L = buzzer_note_freq[note];
+    // Set the compare value to half the period
+    OCR1AH = 0x00;
+    OCR1AL = buzzer_note_freq[note]>>1;
+	sei();
+
+    return 0x00;
+  }
+}
+
+unsigned char buzzer_stop(void)
+{
+  TCCR1B=((TCCR1B & ~0x07) | 0x00);// set the prescaler to 1024
+  // turn off channel A (OC1A) PWM output
+  // set OC1A (OutputCompare action) to none
+  TCCR1A&=(~(1<<COM1A1));
+  TCCR1A&=(~(1<<COM1A0));
+
+  return 0x00;
+}
+
+// timer interface
+SIGNAL(TIMER3_OVF_vect)
+{
+  if(timer_func!=0x0000)
+    timer_func(timer_param);
+ 
+  timer_done=0x01;
+
+  TCCR3B=(TCCR3B & 0xF8);// set the prescaler to 0 (timer disabled)
+  TIMSK3&=(~(0x01<<TOIE3));// disable TCNT2 overflow
+}
+
+unsigned char init_timer(void)
+{
+  timer_func=0x0000;
+  timer_param=0x0000;
+  timer_done=0x01;
+
+  timer_prescaler = 0;
+
+  // initialize timer 2
+  TCCR3B=(TCCR3B & 0xF8);// set the prescaler to 0 (timer disabled)
+  TCNT3H=0x00;
+  TCNT3L=0x00;
+  // disable interrupts
+  TIMSK3&=(~(0x01<<TOIE3));         
+
+  return 0x00;
+}
+
+unsigned char timer_set_time(unsigned short int time_ms)
+{
+  unsigned char i;
+  unsigned long int count=0;
+
+  for(i=1;i<6;i++)
+  {
+    count=time_ms*(F_CPU/1000)/timer_prescaler_values[i];
+    if(count<0xFFFF)
+      break;
+  }
+  if(count>0xFFFF)
+    return 0xFF;
+  else
+  {
+    timer_prescaler=i;
+
+    TCNT3H=(((0xFFFF-count)>>8)&0xFF);
+    TCNT3L=((0xFFFF-count)&0xFF);
+   
+    return 0x00;  
+  }
+}
+
+unsigned char timer_start(void)
+{
+  if(timer_prescaler!=0)
+  {
+    timer_done=0x00;
+ 
+    // set the prescaler to a value != 0 to enable the timer
+    TCCR3B=((TCCR3B & 0xF8) | timer_prescaler);// set the prescaler to correct value
+    TIMSK3|=(0x01<<TOIE3);              // enable TCNT3 overflow
+
+    return 0x00;
+  }
+  else
+    return 0xFF;
+}
+
+unsigned char timer_set_callback(void (*function)(void *param),void *param)
+{
+  timer_func=function;
+  timer_param=param;
+
+  return 0x00;
+}
+
+unsigned char timer_stop(void)
+{
+  timer_done=0x01;
+
+  // set prescaler to 0
+  TCCR3B=((TCCR3B & 0xF8) | timer_prescaler_values[0]);// set the prescaler to correct value
+  // reset count value
+  TCNT3H=0x00;
+  TCNT3L=0x00;
+  // disable interrupts
+  TIMSK3&=(~(0x01<<TOIE3));         
+
+  return 0x00;
+}
+
+unsigned char is_timer_done(void)
+{
+  return timer_done;
+}
+
+unsigned char timer_wait(void)
+{
+  while(timer_done==0x00);
+
+  return 0x00;
+}
diff --git a/controllers/src/examples/Makefile b/controllers/src/examples/Makefile
deleted file mode 100644
index db83592ea8c4d3501365f3b0ed1bd8bc74b83a87..0000000000000000000000000000000000000000
--- a/controllers/src/examples/Makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-PROJECT=cm510_ex
-########################################################
-# afegir tots els fitxers que s'han de compilar aquí
-########################################################
-SOURCES=main.c
-
-OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/
-DEV_DIR=../../../dyn_devices/
-COMM_DIR=../../../communications/
-MAN_DIR=../../../motion/
-CONT_DIR=../../
-CC=avr-gcc
-OBJCOPY=avr-objcopy
-MMCU=atmega2561
-
-INCLUDE_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(MAN_DIR)include -I$(DEV_DIR)include
-
-LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
-
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
-
-LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
-
-HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
-
-.PHONY: all
-
-all: $(PROJECT).hex 
-
-$(PROJECT).hex: $(PROJECT).elf
-	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
-$(PROJECT).elf: $(OBJS)
-	$(CC) $(LDFLAGS) $(OBJS) $(LIBS) -o $(PROJECT).elf
-%.o:%.c
-	$(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $<
-
-download: $(MAIN_OUT_HEX)
-	fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new
-
-clean:
-	-rm $(PROJECT).*
-	-rm $(OBJS)
diff --git a/controllers/src/examples/main.c b/controllers/src/examples/main.c
index fa6923951b9dc0c57f42948db90fbaedee7b255d..a5be94820c12ccc1f9a080bdecd68bf191c9375a 100755
--- a/controllers/src/examples/main.c
+++ b/controllers/src/examples/main.c
@@ -15,32 +15,66 @@
  *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "cm510.h"
-#include <stdio.h>
-#include <util/delay.h>
-
-void user_init(void)
-{
-  serial_console_init(57600);
-  user_time_set_period(10);
-}
-
-void user_loop(void)
-{
-  if(user_time_is_period_done())
-  {
-    printf("Gyro X: %d\n",get_adc_avg_channel(ADC_PORT_3));
-    printf("Gyro Y: %d\n",get_adc_avg_channel(ADC_PORT_4));
-  }
-  if(is_button_pressed(BTN_START))
-    turn_led_on(LED_AUX);
-  else
-    turn_led_off(LED_AUX);
-  if(is_button_falling_edge(BTN_UP))
-    buzzer_start(NOTE_SOL,30,30);
-  else
-  {
-    if(is_button_falling_edge(BTN_DOWN))
-      buzzer_stop();
-  }
-}
+#include "cm510.h"
+#include "serial.h"
+#include <stdio.h>
+#include <util/delay.h>
+
+void toggle_aux_led(void *param)
+{
+  toggle_led(LED_AUX);
+}
+
+void play_do(void *param)
+{
+  toggle_led(LED_MANAGE);
+}
+
+void play_re(void *param)
+{
+  toggle_led(LED_PROGRAM);
+}
+
+void play_mi(void *param)
+{
+  toggle_led(LED_PLAY);
+}
+
+void play_fa(void *param)
+{
+  toggle_led(LED_TxD);
+}
+
+void play_stop(void *param)
+{
+  toggle_led(LED_RxD);	
+}
+
+int main(void)
+{
+  init_cm510(ASYNC);
+
+  serial_initialize(57600);
+
+  timer_set_time(1000);
+  timer_set_callback(toggle_aux_led,0x0000);
+  button_assign_callback(BTN_UP,play_do,0x0000);
+  button_assign_callback(BTN_DOWN,play_re,0x0000);
+  button_assign_callback(BTN_LEFT,play_mi,0x0000);
+  button_assign_callback(BTN_RIGHT,play_fa,0x0000);
+  button_assign_callback(BTN_START,play_stop,0x0000);
+  
+  timer_start();
+  timer_wait();
+  
+  while(1)
+  {
+/*    printf("%d, %d, %d, %d, %d, %d\n",get_adc_channel(ADC_PORT_1),
+	                                  get_adc_channel(ADC_PORT_2),
+									  get_adc_channel(ADC_PORT_3),
+									  get_adc_channel(ADC_PORT_4),
+									  get_adc_channel(ADC_PORT_5),
+									  get_adc_channel(ADC_PORT_6));*/
+	_delay_ms(100);
+  }
+}
diff --git a/controllers/src/gpio.c b/controllers/src/gpio.c
deleted file mode 100644
index 3e0ae9fd481933984aea8f102de344b7c5cb6343..0000000000000000000000000000000000000000
--- a/controllers/src/gpio.c
+++ /dev/null
@@ -1,357 +0,0 @@
-#include "gpio.h"
-
-typedef struct{
-  volatile uint8_t pressed;
-  volatile uint8_t rising_edge;
-  volatile uint8_t falling_edge;
-  volatile uint8_t old_state;
-}TButtonState;
-
-/* private variables */
-TButtonState button_up;
-TButtonState button_down;
-TButtonState button_left;
-TButtonState button_right;
-TButtonState button_start;
-
-/* private functions */
-void pushbuttons_loop(void)
-{
-  if((~PINE & BTN_UP)!=0x00)  
-  {
-    button_up.pressed=0x01;
-    if(button_up.old_state==0x00)
-    {
-      button_up.rising_edge=0x01; 
-      button_up.old_state=0x01;
-    }
-  }
-  else
-  {
-    if(button_up.old_state==0x01)
-    {
-      button_up.falling_edge=0x01;
-      button_up.old_state=0x00;
-    }
-  }
-  if((~PINE & BTN_DOWN)!=0x00)  
-  {
-    button_down.pressed=0x01;
-    if(button_down.old_state==0x00)
-    {
-      button_down.rising_edge=0x01; 
-      button_down.old_state=0x01;
-    }
-  }
-  else
-  {
-    if(button_down.old_state==0x01)
-    {
-      button_down.falling_edge=0x01;
-      button_down.old_state=0x00;
-    }
-  }
-  if((~PINE & BTN_LEFT)!=0x00)  
-  {
-    button_left.pressed=0x01;
-    if(button_left.old_state==0x00)
-    {
-      button_left.rising_edge=0x01; 
-      button_left.old_state=0x01;
-    }
-  }
-  else
-  {
-    if(button_left.old_state==0x01)
-    {
-      button_left.falling_edge=0x01;
-      button_left.old_state=0x00;
-    }
-  }
-  if((~PINE & BTN_RIGHT)!=0x00)  
-  {
-    button_right.pressed=0x01;
-    if(button_right.old_state==0x00)
-    {
-      button_right.rising_edge=0x01; 
-      button_left.old_state=0x01;
-    }
-  }
-  else
-  {
-    if(button_right.old_state==0x01)
-    {
-      button_right.falling_edge=0x01;
-      button_left.old_state=0x00;
-    }
-  }
-  if((~PIND & BTN_START)!=0x00)  
-  {
-    button_start.pressed=0x01;
-    if(button_start.old_state==0x00)
-    {
-      button_start.rising_edge=0x01; 
-      button_left.old_state=0x01;
-    }
-  }
-  else
-  {
-    if(button_start.old_state==0x01)
-    {
-      button_start.falling_edge=0x01;
-      button_left.old_state=0x00;
-    }
-  }
-}
-
-/* public functions */
-void init_leds(void)
-{
-  // configure the PORTC as outputs for the LED's
-  DDRC  = 0x7F;
-  // all LED's off except for the Battery LED
-  PORTC = 0x7E;
-
-  /* initialize internal variables */
-  button_up.pressed=0x00;
-  button_up.rising_edge=0x00;
-  button_up.falling_edge=0x00;
-  button_up.old_state=0x00;
-  button_down.pressed=0x00;
-  button_down.rising_edge=0x00;
-  button_down.falling_edge=0x00;
-  button_down.old_state=0x00;
-  button_left.pressed=0x00;
-  button_left.rising_edge=0x00;
-  button_left.falling_edge=0x00;
-  button_left.old_state=0x00;
-  button_right.pressed=0x00;
-  button_right.rising_edge=0x00;
-  button_right.falling_edge=0x00;
-  button_right.old_state=0x00;
-  button_start.pressed=0x00;
-  button_start.rising_edge=0x00;
-  button_start.falling_edge=0x00;
-  button_start.old_state=0x00;
-}
-
-void turn_led_on(leds_t led_id)
-{
-  switch(led_id)
-  {
-    case LED_BAT: PORTC &= ~LED_BAT;
-                  break;
-    case LED_TxD: PORTC &= ~LED_TxD;
-                  break;
-    case LED_RxD: PORTC &= ~LED_RxD;
-                  break;
-    case LED_AUX: PORTC &= ~LED_AUX;
-                  break;
-    case LED_MANAGE: PORTC &= ~LED_MANAGE;
-                     break;
-    case LED_PROGRAM: PORTC &= ~LED_PROGRAM;
-                      break;
-    case LED_PLAY: PORTC &= ~LED_PLAY;
-                   break;
-  }
-}
-
-void turn_led_off(leds_t led_id)
-{
-  switch(led_id)
-  {
-    case LED_BAT: PORTC |= LED_BAT;
-                  break;
-    case LED_TxD: PORTC |= LED_TxD;
-                  break;
-    case LED_RxD: PORTC |= LED_RxD;
-                  break;
-    case LED_AUX: PORTC |= LED_AUX;
-                  break;
-    case LED_MANAGE: PORTC |= LED_MANAGE;
-                     break;
-    case LED_PROGRAM: PORTC |= LED_PROGRAM;
-                      break;
-    case LED_PLAY: PORTC |= LED_PLAY;
-                   break;
-  }
-}
-
-void toggle_led(leds_t led_id)
-{
-  switch(led_id)
-  {
-    case LED_BAT: PORTC ^= LED_BAT;
-                  break;
-    case LED_TxD: PORTC ^= LED_TxD;
-                  break;
-    case LED_RxD: PORTC ^= LED_RxD;
-                  break;
-    case LED_AUX: PORTC ^= LED_AUX;
-                  break;
-    case LED_MANAGE: PORTC ^= LED_MANAGE;
-                     break;
-    case LED_PROGRAM: PORTC ^= LED_PROGRAM;
-                      break;
-    case LED_PLAY: PORTC ^= LED_PLAY;
-                   break;
-  }
-}
-
-uint8_t is_led_on(leds_t led_id)
-{
-  uint8_t value=0x00;
-
-  switch(led_id)
-  {
-    case LED_BAT: value = (PINC & LED_BAT)>>6;
-                  break;
-    case LED_TxD: value = (PINC & LED_TxD)>>5;
-                  break;
-    case LED_RxD: value = (PINC & LED_RxD)>>4;
-                  break;
-    case LED_AUX: value = (PINC & LED_AUX)>>3;
-                  break;
-    case LED_MANAGE: value = (PINC & LED_MANAGE)>>2;
-                     break;
-    case LED_PROGRAM: value = (PINC & LED_PROGRAM)>>1;
-                      break;
-    case LED_PLAY: value = (PINC & LED_PLAY);
-                   break;
-  }
-
-  return value;  
-}
-
-void init_buttons(void)
-{
-  DDRD  &= 0xFE;
-  PORTD |= 0x01;
-	
-  DDRE  &= 0x0F;
-  PORTE |= 0xF0;
-}
-
-uint8_t is_button_pressed(pushbuttons_t button_id)
-{
-  unsigned char value=0x00;
-
-  switch(button_id)
-  {
-    case BTN_UP: if(button_up.pressed)
-                 {
-                   value=0x01;
-                   button_up.pressed=0x00;
-                 }
-                 break;
-    case BTN_DOWN: if(button_down.pressed)
-                   {
-                     value=0x01;
-                     button_down.pressed=0x00;
-                   }
-                   break;
-    case BTN_LEFT: if(button_left.pressed)
-                   {
-                     value=0x01;
-                     button_left.pressed=0x00;
-                   }
-                   break;
-    case BTN_RIGHT: if(button_right.pressed)
-                    {
-                      value=0x01;
-                      button_right.pressed=0x00;
-                    }
-                    break;
-    case BTN_START: if(button_start.pressed)
-                    {
-                      value=0x01;
-                      button_start.pressed=0x00;
-                    }
-                    break;
-  }
-
-  return value;
-}
-
-uint8_t is_button_rising_edge(pushbuttons_t button_id)
-{
-  unsigned char value=0x00;
-
-  switch(button_id)
-  {
-    case BTN_UP: if(button_up.rising_edge)
-                 {
-                   value=0x01;
-                   button_up.rising_edge=0x00;
-                 }
-                 break;
-    case BTN_DOWN: if(button_down.rising_edge)
-                   {
-                     value=0x01;
-                     button_down.rising_edge=0x00;
-                   }
-                   break;
-    case BTN_LEFT: if(button_left.rising_edge)
-                   {
-                     value=0x01;
-                     button_left.rising_edge=0x00;
-                   }
-                   break;
-    case BTN_RIGHT: if(button_right.rising_edge)
-                    {
-                      value=0x01;
-                      button_right.rising_edge=0x00;
-                    }
-                    break;
-    case BTN_START: if(button_start.rising_edge)
-                    {
-                      value=0x01;
-                      button_start.rising_edge=0x00;
-                    }
-                    break;
-  }
-
-  return value;
-}
-
-uint8_t is_button_falling_edge(pushbuttons_t button_id)
-{
-  unsigned char value=0x00;
-
-  switch(button_id)
-  {
-    case BTN_UP: if(button_up.falling_edge)
-                 {
-                   value=0x01;
-                   button_up.falling_edge=0x00;
-                 }
-                 break;
-    case BTN_DOWN: if(button_down.falling_edge)
-                   {
-                     value=0x01;
-                     button_down.falling_edge=0x00;
-                   }
-                   break;
-    case BTN_LEFT: if(button_left.falling_edge)
-                   {
-                     value=0x01;
-                     button_left.falling_edge=0x00;
-                   }
-                   break;
-    case BTN_RIGHT: if(button_right.falling_edge)
-                    {
-                      value=0x01;
-                      button_right.falling_edge=0x00;
-                    }
-                    break;
-    case BTN_START: if(button_start.falling_edge)
-                    {
-                      value=0x01;
-                      button_start.falling_edge=0x00;
-                    }
-                    break;
-  }
-
-  return value;
-}
-
diff --git a/controllers/src/user_time.c b/controllers/src/user_time.c
deleted file mode 100644
index 90a29a408040df38c986d49bd3ea1a76e0badb42..0000000000000000000000000000000000000000
--- a/controllers/src/user_time.c
+++ /dev/null
@@ -1,126 +0,0 @@
-#include "user_time.h"
-
-/* private variables */
-uint8_t user_time_single;
-uint16_t user_time_time;
-uint16_t user_time_period;
-volatile uint8_t user_time_done;
-volatile uint8_t user_time_period_done;
-
-/* private funcitons */
-void user_time_loop(void)
-{
-  static uint16_t count=0;
-
-  if(TIFR5&(1<<OCF5A))
-  {
-    TIFR5|=(1<<OCF5A);// clear any interrupt
-    TCNT5H=0x00;                   
-    TCNT5L=0x00;
-    if(user_time_single==0x01)
-    {
-      if(count==user_time_time)
-      {
-        count=0;
-        user_time_done=0x01;
-        TCCR5B&=0xF8;// disable timer 
-      }
-      else
-        count++;
-    }
-    else
-    {
-      if(count==user_time_period)
-      {
-        count=0;
-        user_time_period_done=0x01;
-      }
-      else
-        count++;
-    }
-  }
-}
-
-/* public functions */
-void init_user_time(void)
-{
-  // initialize timer3 for buzzer time control (100ms period)
-  TIMSK5=0x00;// disable all interrupts
-  TIFR5=0x2F;// clear any pending interrupt
-  TCNT5H=0x00;                   
-  TCNT5L=0x00;
-  // set PWM mode with ICR top-count (CTC mode)
-  TCCR5A=0x00;
-  TCCR5A&=~(1<<WGM50);
-  TCCR5A&=~(1<<WGM51);
-  TCCR5B=0x00;
-  TCCR5B|=(1<<WGM52);
-  TCCR5B&=~(1<<WGM53);
-  // clear output compare value A
-  OCR5AH=0x18;
-  OCR5AL=0x6A;
-  TCCR5B&=0xF8;// disable timer by default
-
-  /* initialize internal variables */
-  user_time_single=0x00;
-  user_time_time=0x00;
-  user_time_period=0x00;
-  user_time_done=0x00;
-  user_time_period_done=0x00;
-}
-
-void user_time_set_period(uint16_t period_ms)
-{
-  user_time_single=0x00;
-  user_time_period=period_ms/100;
-  user_time_period_done=0x00;
-  TCNT5H=0x00;                   
-  TCNT5L=0x00;
-  TIFR5=0x2F;// clear any pending interrupt
-  TCCR5B&=0xF8;// disable timer by default
-  TCCR5B|=0x04;
-}
-
-void user_time_set_one_time(uint16_t time_ms)
-{
-  user_time_single=0x01;
-  user_time_time=time_ms/100;
-  user_time_done=0x00;
-  TCNT5H=0x00;                   
-  TCNT5L=0x00;
-  TIFR5=0x2F;// clear any pending interrupt
-  TCCR5B&=0xF8;// disable timer by default
-  TCCR5B|=0x04;
-}
-
-uint8_t user_time_is_period_done(void)
-{
-  if(user_time_period_done==0x01)
-  {
-    user_time_period_done=0x00;
-    return 0x01;
-  }
-  else
-    return 0x00;
-}
-
-uint8_t user_time_is_done(void)
-{
-  if(user_time_done==0x01)
-  {
-    user_time_done=0x00;
-    return 0x01;
-  }
-  else
-    return 0x00;
-}
-
-void user_time_stop(void)
-{
-  TCCR5B&=0xF8;// disable timer 
-  TIFR5|=(1<<OCF5A);// clear any interrupt
-  TCNT5H=0x00;                   
-  TCNT5L=0x00;
-  user_time_period_done=0x00;  
-  user_time_done=0x00;  
-}
diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile
index 44b6332e1d6be66947d0201478045048d607c92e..2e08fdf5ad7ea3030088d9ad85a1156eaaa3d872 100755
--- a/dyn_devices/Makefile
+++ b/dyn_devices/Makefile
@@ -1,25 +1,51 @@
+ROBOTIS_C_DIR ?=
+
+ifdef ROBOTIS_C_DIR
+    ROBOTIS_C_DIR_FOUND=true
+else
+    ROBOTIS_C_DIR_FOUND=false
+endif
+
 PROJECT=libdyn_devices
-SOURCES=src/dyn_servos/dyn_servos.c# src/exp_board/exp_board.c 
+SOURCES=src/dyn_servos/dyn_servos.c src/exp_board/exp_board.c 
 OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/ 
-BIN_DIR=./build/
-LIB_DIR=./lib/
-COMM_DIR=../communications
+SDIR=./src/ 
+IDIR=./include/
+BDIR=./build/
+LDIR=./lib/
 CC=avr-gcc
-OBJCOPY=avr-ar
+OBJCOPY=avr-ar rsc
 MMCU=atmega2561
 
-INC_DIRS=-I./include/ -I$(COMM_DIR)/include
-
-LIBS=$(COMM_DIR)/lib/libcomm.a
+ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/
+ROBOTIS_LIBRARY_DIR=$(ROBOTIS_C_DIR)/lib/
 
 CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
-ARFLAGS=rsc
+.PHONY: all check_robotis_c show_banner clean
 
-.PHONY: all show_banner clean
+all: check_robotis_c show_banner $(PROJECT).a
 
-all: show_banner communications $(PROJECT).a 
+check_robotis_c:
+	@if ! $(ROBOTIS_C_DIR_FOUND); then                                                 \
+	    echo " IMPORTANT !!! ";                                                        \
+	    echo " ROBOTIS_C_DIR variable is not set! The software won't compile";         \
+	    echo " unless you specify the path to Robotis embedded C software.";           \
+	    echo "";                                                                       \
+	    echo " You should:";                                                           \
+	    echo "   1. Edit the makefile, at the begging set up ROBOTIS_C_DIR";           \
+	    echo "   2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \
+	    echo "";                                                                       \
+	    echo " Beware of:";                                                            \
+	    echo "   1. Do not use '(' in the directory name (as it comes from robotis) "; \
+	    echo "   2. Absolute path is preferred to avoid problems with submake calls"; \
+	    exit 2;                                                                        \
+	fi
+
+	@if [ ! -d $(ROBOTIS_C_DIR) ]; then                                                  \
+	   echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it.";  \
+	   exit 2;                                                                           \
+	fi
 
 show_banner:
 	@echo "------------------------------------------------------";
@@ -34,18 +60,11 @@ show_banner:
 	@echo "------------------------------------------------------";
 
 $(PROJECT).a: ${OBJS}
-	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS)
+	$(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS)
 
 %.o: %.c 
-	$(CC) -c $(CFLAGS) $(INC_DIRS) -o $@ $<
-
-communications:
-	$(MAKE) -C $(COMM_DIR)
-
-examples:
-	$(MAKE) -C src/examples 
+	$(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -L${ROBOTIS_LIBRARY_DIR} -ldynamixel -o $@ $<
 
 clean:
-	rm -f ${LIB_DIR}$(PROJECT).a
+	rm -f ${LDIR}$(PROJECT).a
 	rm -f $(OBJS) 
-	$(MAKE) -C src/examples clean
diff --git a/dyn_devices/include/dyn_servos.h b/dyn_devices/include/dyn_servos.h
index f7b0ba738b0413ce43fe7890c203a9a701dc299a..f16bd92dfafe7b434a8bfcbbc7e6e1f351a17170 100755
--- a/dyn_devices/include/dyn_servos.h
+++ b/dyn_devices/include/dyn_servos.h
@@ -18,9 +18,6 @@
 #ifndef _DYN_SERVOS_H
 #define _DYN_SERVOS_H
 
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
 // error definitions
 #define         INSTRUCTION_ERROR         0x40
 #define         OVERLOAD_ERROR            0x20
@@ -40,6 +37,40 @@
 #define         IS_INPUT_VOLTAGE_ERROR_SET(A)    (A&INPUT_VOLTAGE_ERROR)
 
 
+// low level dynamixel functions
+/**
+ * \brief function to synchronous write to multiple servos
+ * 
+ * This function implements the synchronous write of the Dynamixel protocol. This
+ * write operation allows to write the same addresses of multiple servos with a 
+ * single operation. This function uses the broadcast identifier (0xFE) and 
+ * receives no status packet as an answer.
+ *
+ * This function does nothing is any of the parameters are incorrect.
+ *
+ * \param num_servos An integer with the number of servos that are accessed by
+ *                   this operation. This value is used to determine the size
+ *                   of the other parameters.
+ * \param servo_ids a vector with the identifier of all the servos that have to
+ *                  be accessed. The length of this vector must be equal to the 
+ *                  num_servos value.
+ * \param start_addr the address from which to start writing the data on all 
+ *                   servos. This value, together with the length parameter must
+ *                   be compatible with the memory map of each servo to avoid
+ *                   unecpected errors.
+ * \param data a vector with the data, for each servo, to be written. This vector
+ *             must have a length equal to num_servos*length, and the information
+ *             must be organized as follows:
+ *    
+ *               servo 1 data | servo 2 data | ··· | servo n data
+ *
+ * \param length the total length, in bytes, of the imformation to be written to 
+ *               each servo.
+ *
+ * \return 0 if the function completes correctly, -1 otherwise
+ */
+unsigned char dxl_sync_tx_packet(unsigned char num_servos,unsigned char *servo_ids,unsigned char start_addr, unsigned char *data, int length);
+
 // servo interface functions EEPROM memory
 /**
  * \brief function to get the model number of a given servo
@@ -68,7 +99,7 @@
  *         is returned in case of any error.
  *
  */
-uint16_t get_model_number(uint8_t servo_id);
+unsigned short int get_model_number(unsigned char servo_id);
 /**
  * \brief function to get the firmware version of a given servo
  *
@@ -81,7 +112,7 @@ uint16_t get_model_number(uint8_t servo_id);
  * \return the current firmware version of the servo or 0xFFFF in case of any 
  *         error.
  */
-uint8_t get_firmware_version(uint8_t servo_id);
+unsigned char get_firmware_version(unsigned char servo_id);
 /**
  * \brief function to change the current identifier of a given servo
  *
@@ -98,7 +129,7 @@ uint8_t get_firmware_version(uint8_t servo_id);
  * 
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_servo_id(uint8_t old_id, uint8_t new_id);
+unsigned char set_servo_id(unsigned char old_id, unsigned char new_id);
 /**
  * \brief Function to get the current baudrate of the bus
  *
@@ -116,7 +147,7 @@ uint8_t set_servo_id(uint8_t old_id, uint8_t new_id);
  *
  * \return the baudrate code of the given servo or 0xFF in case of any error.
  */
-uint8_t get_baudrate(uint8_t servo_id);
+unsigned char get_baudrate(unsigned char servo_id);
 /**
  * \brief Function to change the current baudrate of the given servo.
  *
@@ -134,7 +165,7 @@ uint8_t get_baudrate(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate);
+unsigned char set_baudrate(unsigned char servo_id, unsigned char baudrate);
 /**
  * \brief Function to get the current angle limits of a given servo.
  * 
@@ -157,7 +188,7 @@ uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw);
+unsigned char get_angle_limits(unsigned char servo_id, unsigned short int *cw, unsigned short int *ccw);
 /**
  * \brief Function to set the angle limits of a given servo
  * 
@@ -179,7 +210,7 @@ uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw);
+unsigned char set_angle_limits(unsigned char servo_id, unsigned short int cw, unsigned short int ccw);
 /**
  * \brief Function to get the temperature limit of a given servo
  * 
@@ -193,7 +224,7 @@ uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw);
  *
  * \return the current temperature limit or 0xFF in case of any error.
  */
-uint8_t get_temperature_limit(uint8_t servo_id);
+unsigned char get_temperature_limit(unsigned char servo_id);
 /**
  * \brief Function to change the temperature limit of a given servo
  * 
@@ -210,7 +241,7 @@ uint8_t get_temperature_limit(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_temperature_limit(uint8_t servo_id, uint8_t temp);
+unsigned char set_temperature_limit(unsigned char servo_id, unsigned char temp);
 /**
  * \brief Function to get the operating voltage range of a given servo
  * 
@@ -230,7 +261,7 @@ uint8_t set_temperature_limit(uint8_t servo_id, uint8_t temp);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high);
+unsigned char get_voltage_limits(unsigned char servo_id, unsigned char *low, unsigned char *high);
 /**
  * \brief Function to change the operating voltage range of a given servo
  *
@@ -249,7 +280,7 @@ uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high);
+unsigned char set_voltage_limits(unsigned char servo_id, unsigned char low, unsigned char high);
 /**
  * \brief Function to get the maximum servo torque
  *
@@ -267,7 +298,7 @@ uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high);
  *
  * \return the current maximum torque allowed or 0xFFFF in case of any error.
  */
-uint16_t get_max_torque(uint8_t servo_id);
+unsigned short int get_max_torque(unsigned char servo_id);
 /**
  * \brief Function to change the maximum allowed torque
  *
@@ -288,7 +319,7 @@ uint16_t get_max_torque(uint8_t servo_id);
  *               
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_max_torque(uint8_t servo_id, uint16_t torque);
+unsigned char set_max_torque(unsigned char servo_id, unsigned short int torque);
 /**
  * \brief function to get the current led alarm configuration
  *
@@ -312,7 +343,7 @@ uint8_t set_max_torque(uint8_t servo_id, uint16_t torque);
  * 
  * \return the current led alarm mask or 0xFF in case of any error
  */
-uint8_t get_led_alarm(uint8_t servo_id);
+unsigned char get_led_alarm(unsigned char servo_id);
 /**
  * \brief Function to set the led alarm configuration
  *
@@ -339,7 +370,7 @@ uint8_t get_led_alarm(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_led_alarm(uint8_t servo_id, uint8_t alarms);
+unsigned char set_led_alarm(unsigned char servo_id, unsigned char alarms);
 /**
  * \brief function to get the current shutdown alarm configuration
  *
@@ -363,7 +394,7 @@ uint8_t set_led_alarm(uint8_t servo_id, uint8_t alarms);
  * 
  * \return the current led alarm mask or 0xFF in case of any error
  */
-uint8_t get_shd_alarm(uint8_t servo_id);
+unsigned char get_shd_alarm(unsigned char servo_id);
 /**
  * \brief Function to set the led alarm configuration
  *
@@ -389,7 +420,7 @@ uint8_t get_shd_alarm(uint8_t servo_id);
  * \return 0 if the function completes correctly, -1 otherwise
 
  */
-uint8_t set_shd_alarm(uint8_t servo_id, uint8_t alarms);
+unsigned char set_shd_alarm(unsigned char servo_id, unsigned char alarms);
 
 // servo interface functions RAM memory
 /**
@@ -406,7 +437,7 @@ uint8_t set_shd_alarm(uint8_t servo_id, uint8_t alarms);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_wheel_mode(uint8_t servo_id);
+unsigned char set_wheel_mode(unsigned char servo_id);
 /**
  * \brief Function to set the joint mode
  *
@@ -421,7 +452,7 @@ uint8_t set_wheel_mode(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_joint_mode(uint8_t servo_id);
+unsigned char set_joint_mode(unsigned char servo_id);
 /**
  * \brief Function to check the current servo mode of a given servo
  *
@@ -435,7 +466,7 @@ uint8_t set_joint_mode(uint8_t servo_id);
  * \return 0 if the servo is in joint mode, 0 if the servo is in wheel mode and
  *         0xFF in case of any error.
  */
-uint8_t is_joint_mode(uint8_t servo_id); 
+unsigned char is_joint_mode(unsigned char servo_id); 
 /**
  * \brief Function to enable the given servo
  *
@@ -448,7 +479,7 @@ uint8_t is_joint_mode(uint8_t servo_id);
  * 
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t enable_servo(uint8_t servo_id);
+unsigned char enable_servo(unsigned char servo_id);
 /**
  * \brief function to disable the given servo
  *
@@ -461,7 +492,7 @@ uint8_t enable_servo(uint8_t servo_id);
  * 
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t disable_servo(uint8_t servo_id);
+unsigned char disable_servo(unsigned char servo_id);
 /**
  * \brief function to check wether the servo is enabled or not
  *
@@ -474,7 +505,7 @@ uint8_t disable_servo(uint8_t servo_id);
  * \return 0 if the servo is disabled, 1 if the servo is enabled and 0xFF in
  *         case of any error.
  */
-uint8_t is_servo_enabled(uint8_t servo_id);
+unsigned char is_servo_enabled(unsigned char servo_id);
 /**
  * \brief Function to turn the servo led on
  * 
@@ -487,7 +518,7 @@ uint8_t is_servo_enabled(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t turn_servo_led_on(uint8_t servo_id);
+unsigned char turn_servo_led_on(unsigned char servo_id);
 /**
  * \brief Function to turn the servo led off
  *
@@ -500,7 +531,7 @@ uint8_t turn_servo_led_on(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t turn_servo_led_off(uint8_t servo_id);
+unsigned char turn_servo_led_off(unsigned char servo_id);
 /**
  * \brief Function to check wether the servo led is on or not
  *
@@ -513,7 +544,7 @@ uint8_t turn_servo_led_off(uint8_t servo_id);
  * \return 0 if the LED is turnwed off, 1 if the led is turned on and 0xFF in case
  *         of any error.
  */
-uint8_t is_servo_led_on(uint8_t servo_id);
+unsigned char is_servo_led_on(unsigned char servo_id);
 /**
  * \brief Function to get the current compliance margin of a given servo
  *
@@ -535,7 +566,7 @@ uint8_t is_servo_led_on(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw);
+unsigned char get_compliance_margin(unsigned char servo_id, unsigned char *cw, unsigned char *ccw);
 /**
  * \brief Function to set the desired compliance margin of a given servo
  *
@@ -557,7 +588,7 @@ uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw);
+unsigned char set_compliance_margin(unsigned char servo_id, unsigned char cw, unsigned char ccw);
 /**
  * \brief Function to get the current compliance slope of a given servo
  *
@@ -589,7 +620,7 @@ uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw);
  * 
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw);
+unsigned char get_compliance_slope(unsigned char servo_id, unsigned char *cw, unsigned char *ccw);
 /**
  * \brief Function to set the desired compliance slope of a given servo
  *
@@ -621,19 +652,19 @@ uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw);
  * 
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_compliance_slope(uint8_t servo_id, uint8_t cw, uint8_t ccw);
+unsigned char set_compliance_slope(unsigned char servo_id, unsigned char cw, unsigned char ccw);
 /**
  * \brief Function to get the PID controller parameters of a given servo
  *
  * Not yet implemented
  */
-uint8_t get_pid_params(uint8_t servo_id,uint8_t *P,uint8_t *I, uint8_t *D);
+unsigned char get_pid_params(unsigned char servo_id,unsigned char *P,unsigned char *I, unsigned char *D);
 /**
  * \brief Function to set the PID controller parameters of a given servo
  *
  * Not yet implemented
  */
-uint8_t set_pid_params(uint8_t servo_id,uint8_t P,uint8_t I, uint8_t D);
+unsigned char set_pid_params(unsigned char servo_id,unsigned char P,unsigned char I, unsigned char D);
 /**
  * \brief Function to get the current target position
  *
@@ -652,7 +683,7 @@ uint8_t set_pid_params(uint8_t servo_id,uint8_t P,uint8_t I, uint8_t D);
  * \return the current target position of the servo between 0 and the maximum encoder 
  *         resolution or 0xFFFF in case of any error.
  */
-uint16_t get_target_position(uint8_t servo_id);
+unsigned short int get_target_position(unsigned char servo_id);
 /**
  * \brief Function to set the desired target position
  * 
@@ -673,7 +704,7 @@ uint16_t get_target_position(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_target_position(uint8_t serov_id, uint16_t position);
+unsigned char set_target_position(unsigned char serov_id, unsigned short int position);
 /**
  * \brief Function to get the current target speed
  *
@@ -691,7 +722,7 @@ uint8_t set_target_position(uint8_t serov_id, uint16_t position);
  *
  * \return the current speed command and 0xFFFF in cas of any error. 
  */
-uint16_t get_target_speed(uint8_t servo_id);
+unsigned short int get_target_speed(unsigned char servo_id);
 /**
  * \brief Function to set teh desired target speed
  *
@@ -712,7 +743,7 @@ uint16_t get_target_speed(uint8_t servo_id);
  * 
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_target_speed(uint8_t servo_id, uint16_t speed);
+unsigned char set_target_speed(unsigned char servo_id, unsigned short int speed);
 /**
  * \brief Function to set the maximum torque for a given servo
  * 
@@ -738,7 +769,7 @@ uint8_t set_target_speed(uint8_t servo_id, uint16_t speed);
  *
  * \return the maximum torque allowed or 0xFFFF in case of any error.
  */
-uint16_t get_torque_limit(uint8_t servo_id);
+unsigned short int get_torque_limit(unsigned char servo_id);
 /**
  * \brief Function to set the maximum torque for a given servo
  *
@@ -760,7 +791,7 @@ uint16_t get_torque_limit(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_torque_limit(uint8_t servo_id, uint16_t torque);
+unsigned char set_torque_limit(unsigned char servo_id, unsigned short int torque);
 /**
  * \brief Function to return the current position of the given servo
  *
@@ -774,7 +805,7 @@ uint8_t set_torque_limit(uint8_t servo_id, uint16_t torque);
  * 
  * \return the current servo position and 0xFFFF in case of any error.
  */
-uint16_t get_current_position(uint8_t servo_id);
+unsigned short int get_current_position(unsigned char servo_id);
 /**
  * \brief Function to return the current speed of the given servo
  *
@@ -792,7 +823,7 @@ uint16_t get_current_position(uint8_t servo_id);
  * 
  * \return the current servo speed and 0xFFFF in case of any error.
  */
-uint16_t get_current_speed(uint8_t servo_id);
+unsigned short int get_current_speed(unsigned char servo_id);
 /**
  * \brief Function to return the current load of the given servo
  * 
@@ -806,7 +837,7 @@ uint16_t get_current_speed(uint8_t servo_id);
  * 
  * \return the current torque ratio and 0xFFFF in case of any error.
  */
-uint16_t get_current_load(uint8_t servo_id);
+unsigned short int get_current_load(unsigned char servo_id);
 /**
  * \brief Function to get the current supply voltage of a given servo
  * 
@@ -824,7 +855,7 @@ uint16_t get_current_load(uint8_t servo_id);
  * 
  * \return the current supply voltage and 0xFF in case of any error.
  */
-uint8_t get_current_voltage(uint8_t servo_id);
+unsigned char get_current_voltage(unsigned char servo_id);
 /**
  * \brief Function to get the current temperature of a given servo
  *
@@ -842,7 +873,7 @@ uint8_t get_current_voltage(uint8_t servo_id);
  * 
  * \return the current temperature and 0xFF in case of any error.
  */
-uint8_t get_current_temperature(uint8_t servo_id);
+unsigned char get_current_temperature(unsigned char servo_id);
 /**
  * \brief Function to check whether the servo is moving or not
  *
@@ -856,7 +887,7 @@ uint8_t get_current_temperature(uint8_t servo_id);
  * \return 0 if the servo is stopped, 1 if the servo is moving and 0xFF in 
  *         case of any error.
  */
-uint8_t is_moving(uint8_t servo_id);
+unsigned char is_moving(unsigned char servo_id);
 /**
  * \brief Function to check whether the servo is locked or not
  *
@@ -871,7 +902,7 @@ uint8_t is_moving(uint8_t servo_id);
  * \return 0 if the servo is unlocked, 1 if the servo is locked and 0xFF in 
  *         case of any error.
  */
-uint8_t is_locked(uint8_t servo_id);
+unsigned char is_locked(unsigned char servo_id);
 /**
  * \brief Function to lock the servo
  *
@@ -885,7 +916,7 @@ uint8_t is_locked(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t lock(uint8_t servo_id);
+unsigned char lock(unsigned char servo_id);
 /**
  * \brief Function to get the current punch value of a given servo.
  *
@@ -903,7 +934,7 @@ uint8_t lock(uint8_t servo_id);
  *
  * \return the current punch value and 0xFFFF in case of any error.
  */
-uint16_t get_punch(uint8_t servo_id);
+unsigned short int get_punch(unsigned char servo_id);
 /**
  * \brief Function to set the desired punch value of a given servo.
  *
@@ -923,6 +954,6 @@ uint16_t get_punch(uint8_t servo_id);
  *
  * \return 0 if the function completes correctly, -1 otherwise
  */
-uint8_t set_punch(uint8_t servo_id, uint16_t punch);
+unsigned char set_punch(unsigned char servo_id, unsigned short int punch);
 
 #endif
diff --git a/dyn_devices/src/dyn_servos/dyn_servos.c b/dyn_devices/src/dyn_servos/dyn_servos.c
index abb37e94e2b2f867c60b9d1a58d7a713c430524c..6c368002e222f7cf9a8544462706286c4f75d7b2 100755
--- a/dyn_devices/src/dyn_servos/dyn_servos.c
+++ b/dyn_devices/src/dyn_servos/dyn_servos.c
@@ -15,63 +15,92 @@
  *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <avr/io.h>
-#include <avr/interrupt.h>
 #include "dyn_servos_reg.h"
 #include "dyn_servos.h"
-#include "dynamixel_master.h"
+#include "dynamixel.h"
 
-uint16_t get_model_number(uint8_t servo_id)
+  unsigned char dxl_sync_tx_packet(unsigned char num_servos,unsigned char *servo_ids,unsigned char start_addr, unsigned char *data, int length)
+  {
+    unsigned char i,j,data_ptr=0;
+  
+    dxl_set_txpacket_id(BROADCAST_ID);
+    dxl_set_txpacket_instruction(INST_SYNC_WRITE);
+    dxl_set_txpacket_parameter(0, start_addr);
+    dxl_set_txpacket_parameter(1, length);
+    for( i=0; i<num_servos; i++ )
+    {
+      dxl_set_txpacket_parameter(2+(length+1)*i, servo_ids[i]);
+      for( j=0; j<length ;j++)
+      {
+        dxl_set_txpacket_parameter(2+(length+1)*i+1+j,data[data_ptr]);
+        data_ptr++;
+      }
+    }
+    dxl_set_txpacket_length((length+1)*num_servos+4);
+  
+    dxl_txrx_packet();
+  
+    return dxl_get_result();
+  }
+
+unsigned short int get_model_number(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,MODEL_NUM,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,MODEL_NUM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t get_firmware_version(uint8_t servo_id)
+unsigned char get_firmware_version(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,FIRMWARE_VER,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,FIRMWARE_VER);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t set_servo_id(uint8_t old_id, uint8_t new_id)
+unsigned char set_servo_id(unsigned char old_id, unsigned char new_id)
 {
-  if(dyn_master_write_byte(old_id,ID,new_id)==DYN_SUCCESS)
+  dxl_write_byte(old_id,ID,new_id);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_baudrate(uint8_t servo_id)
+unsigned char get_baudrate(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,BAUD_RATE,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,BAUD_RATE);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate)
+unsigned char set_baudrate(unsigned char servo_id, unsigned char baudrate)
 {
-  if(dyn_master_write_byte(servo_id,BAUD_RATE,baudrate)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,BAUD_RATE,baudrate);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw)
+unsigned char get_angle_limits(unsigned char servo_id, unsigned short int *cw, unsigned short int *ccw)
 {
-  if(dyn_master_read_word(servo_id,CW_ANGLE_LIM,cw)==DYN_SUCCESS)
+  *cw=dxl_read_word(servo_id,CW_ANGLE_LIM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_read_word(servo_id,CCW_ANGLE_LIM,ccw)==DYN_SUCCESS)
+    *ccw=dxl_read_word(servo_id,CCW_ANGLE_LIM);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -80,11 +109,13 @@ uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw)
     return 0xFF;//error
 }
 
-uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw)
+unsigned char set_angle_limits(unsigned char servo_id, unsigned short int cw, unsigned short int ccw)
 {
-  if(dyn_master_write_word(servo_id,CW_ANGLE_LIM,cw)==DYN_SUCCESS)
+  dxl_write_word(servo_id,CW_ANGLE_LIM,cw);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_write_word(servo_id,CCW_ANGLE_LIM,ccw)==DYN_SUCCESS)
+    dxl_write_word(servo_id,CCW_ANGLE_LIM,ccw);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -93,29 +124,33 @@ uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw)
     return 0xFF;//error
 }
 
-uint8_t get_temperature_limit(uint8_t servo_id)
+unsigned char get_temperature_limit(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,TEMP_LIM,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,TEMP_LIM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t set_temperature_limit(uint8_t servo_id, uint8_t temp)
+unsigned char set_temperature_limit(unsigned char servo_id, unsigned char temp)
 {
-  if(dyn_master_write_byte(servo_id,TEMP_LIM,temp)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,TEMP_LIM,temp);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high)
+unsigned char get_voltage_limits(unsigned char servo_id, unsigned char *low, unsigned char *high)
 {
-  if(dyn_master_read_byte(servo_id,LOW_VOLTAGE_LIM,low)==DYN_SUCCESS)
+  *low=dxl_read_byte(servo_id,LOW_VOLTAGE_LIM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_read_byte(servo_id,HIGH_VOLTAGE_LIM,high)==DYN_SUCCESS)
+    *high=dxl_read_byte(servo_id,HIGH_VOLTAGE_LIM);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -124,11 +159,13 @@ uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high)
     return 0xFF;//error
 }
 
-uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high)
+unsigned char set_voltage_limits(unsigned char servo_id, unsigned char low, unsigned char high)
 {
-  if(dyn_master_write_byte(servo_id,LOW_VOLTAGE_LIM,low)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,LOW_VOLTAGE_LIM,low);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_write_byte(servo_id,HIGH_VOLTAGE_LIM,high)==DYN_SUCCESS)
+    dxl_write_word(servo_id,HIGH_VOLTAGE_LIM,high);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -137,73 +174,79 @@ uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high)
     return 0xFF;//error
 }
 
-uint16_t get_max_torque(uint8_t servo_id)
+unsigned short int get_max_torque(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,MAX_TORQUE,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,MAX_TORQUE);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t set_max_torque(uint8_t servo_id, uint16_t torque)
+unsigned char set_max_torque(unsigned char servo_id, unsigned short int torque)
 {
-  if(dyn_master_write_word(servo_id,MAX_TORQUE,torque)==DYN_SUCCESS)
+  dxl_write_word(servo_id,MAX_TORQUE,torque);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_led_alarm(uint8_t servo_id)
+unsigned char get_led_alarm(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,LED_ALARM,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,LED_ALARM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t set_led_alarm(uint8_t servo_id, uint8_t alarms)
+unsigned char set_led_alarm(unsigned char servo_id, unsigned char alarms)
 {
-  if(dyn_master_write_byte(servo_id,LED_ALARM,alarms)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,LED_ALARM,alarms);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_shd_alarm(uint8_t servo_id)
+unsigned char get_shd_alarm(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,SHD_ALARM,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,SHD_ALARM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t set_shd_alarm(uint8_t servo_id, uint8_t alarms)
+unsigned char set_shd_alarm(unsigned char servo_id, unsigned char alarms)
 {
-  if(dyn_master_write_byte(servo_id,SHD_ALARM,alarms)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,SHD_ALARM,alarms);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t set_wheel_mode(uint8_t servo_id)
+unsigned char set_wheel_mode(unsigned char servo_id)
 {
   return set_angle_limits(servo_id,0,0);
 }
 
-uint8_t set_joint_mode(uint8_t servo_id)
+unsigned char set_joint_mode(unsigned char servo_id)
 {
   return set_angle_limits(servo_id,1024,0);
 }
 
-uint8_t is_joint_mode(uint8_t servo_id)
+unsigned char is_joint_mode(unsigned char servo_id)
 {
-  uint16_t cw,ccw;
+  unsigned short int cw,ccw;
 
   if(get_angle_limits(servo_id,&cw,&ccw)==0xFF)
     return 0xFF;
@@ -216,63 +259,71 @@ uint8_t is_joint_mode(uint8_t servo_id)
   }
 }
 
-uint8_t enable_servo(uint8_t servo_id)
+unsigned char enable_servo(unsigned char servo_id)
 {
-  if(dyn_master_write_byte(servo_id,TORQUE_EN,0x01)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,TORQUE_EN,0x01);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t disable_servo(uint8_t servo_id)
+unsigned char disable_servo(unsigned char servo_id)
 {
-  if(dyn_master_write_byte(servo_id,TORQUE_EN,0x00)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,TORQUE_EN,0x00);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t is_servo_enabled(uint8_t servo_id)
+unsigned char is_servo_enabled(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,TORQUE_EN,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,TORQUE_EN);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t turn_servo_led_on(uint8_t servo_id)
+unsigned char turn_servo_led_on(unsigned char servo_id)
 {
-  if(dyn_master_write_byte(servo_id,LED,0x01)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,LED,0x01);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t turn_servo_led_off(uint8_t servo_id)
+unsigned char turn_servo_led_off(unsigned char servo_id)
 {
-  if(dyn_master_write_byte(servo_id,LED,0x00)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,LED,0x00);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t is_servo_led_on(uint8_t servo_id)
+unsigned char is_servo_led_on(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,LED,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,LED);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw)
+unsigned char get_compliance_margin(unsigned char servo_id, unsigned char *cw, unsigned char *ccw)
 {
-  if(dyn_master_read_byte(servo_id,CW_COMP_MARGIN,cw)==DYN_SUCCESS)
+  *cw=dxl_read_byte(servo_id,CW_COMP_MARGIN);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_read_byte(servo_id,CCW_COMP_MARGIN,ccw)==DYN_SUCCESS)
+    *ccw=dxl_read_byte(servo_id,CCW_COMP_MARGIN);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -281,11 +332,13 @@ uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw)
     return 0xFF;//error
 }
 
-uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw)
+unsigned char set_compliance_margin(unsigned char servo_id, unsigned char cw, unsigned char ccw)
 {
-  if(dyn_master_write_byte(servo_id,CW_COMP_MARGIN,cw)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,CW_COMP_MARGIN,cw);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_write_byte(servo_id,CCW_COMP_MARGIN,ccw)==DYN_SUCCESS)
+    dxl_write_word(servo_id,CCW_COMP_MARGIN,ccw);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -294,11 +347,13 @@ uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw)
     return 0xFF;//error
 }
 
-uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw)
+unsigned char get_compliance_slope(unsigned char servo_id, unsigned char *cw, unsigned char *ccw)
 {
-  if(dyn_master_read_byte(servo_id,CW_COMP_SLOPE,cw)==DYN_SUCCESS)
+  *cw=dxl_read_byte(servo_id,CW_COMP_SLOPE);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_read_byte(servo_id,CCW_COMP_SLOPE,ccw)==DYN_SUCCESS)
+    *ccw=dxl_read_byte(servo_id,CCW_COMP_SLOPE);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -307,11 +362,13 @@ uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw)
     return 0xFF;//error
 }
 
-uint8_t set_compliance_slope(uint8_t servo_id, uint8_t cw, uint8_t ccw)
+unsigned char set_compliance_slope(unsigned char servo_id, unsigned char cw, unsigned char ccw)
 {
-  if(dyn_master_write_byte(servo_id,CW_COMP_SLOPE,cw)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,CW_COMP_SLOPE,cw);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
   {
-    if(dyn_master_write_byte(servo_id,CCW_COMP_SLOPE,ccw)==DYN_SUCCESS)
+    dxl_write_word(servo_id,CCW_COMP_SLOPE,ccw);
+    if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
       return 0x00;// the actual value
     else
       return 0xFF;
@@ -320,161 +377,177 @@ uint8_t set_compliance_slope(uint8_t servo_id, uint8_t cw, uint8_t ccw)
     return 0xFF;//error
 }
 
-uint8_t get_pid_params(uint8_t servo_id,uint8_t *P,uint8_t *I, uint8_t *D)
+unsigned char get_pid_params(unsigned char servo_id,unsigned char *P,unsigned char *I, unsigned char *D)
 {
   return 0xFF;
 }
 
-uint8_t set_pid_params(uint8_t servo_id,uint8_t P,uint8_t I, uint8_t D)
+unsigned char set_pid_params(unsigned char servo_id,unsigned char P,unsigned char I, unsigned char D)
 {
   return 0xFF;
 }
 
-uint16_t get_target_position(uint8_t servo_id)
+unsigned short int get_target_position(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,GOAL_POS,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,GOAL_POS);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t set_target_position(uint8_t servo_id, uint16_t position)
+unsigned char set_target_position(unsigned char servo_id, unsigned short int position)
 {
-  if(dyn_master_write_word(servo_id,GOAL_POS,position)==DYN_SUCCESS)
+  dxl_write_word(servo_id,GOAL_POS,position);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint16_t get_target_speed(uint8_t servo_id)
+unsigned short int get_target_speed(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,GOAL_SPEED,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,GOAL_SPEED);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t set_target_speed(uint8_t servo_id, uint16_t speed)
+unsigned char set_target_speed(unsigned char servo_id, unsigned short int speed)
 {
-  if(dyn_master_write_word(servo_id,GOAL_SPEED,speed)==DYN_SUCCESS)
+  dxl_write_word(servo_id,GOAL_SPEED,speed);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint16_t get_torque_limit(uint8_t servo_id)
+unsigned short int get_torque_limit(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,TORQUE_LIM,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,TORQUE_LIM);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t set_torque_limit(uint8_t servo_id, uint16_t torque)
+unsigned char set_torque_limit(unsigned char servo_id, unsigned short int torque)
 {
-  if(dyn_master_write_word(servo_id,TORQUE_LIM,torque)==DYN_SUCCESS)
+  dxl_write_word(servo_id,TORQUE_LIM,torque);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint16_t get_current_position(uint8_t servo_id)
+unsigned short int get_current_position(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,CURRENT_POS,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,CURRENT_POS);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint16_t get_current_speed(uint8_t servo_id)
+unsigned short int get_current_speed(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,CURRENT_SPEED,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,CURRENT_SPEED);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint16_t get_current_load(uint8_t servo_id)
+unsigned short int get_current_load(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,CURRENT_LOAD,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,CURRENT_LOAD);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t get_current_voltage(uint8_t servo_id)
+unsigned char get_current_voltage(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,CURRENT_VOLTAGE,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,CURRENT_VOLTAGE);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t get_current_temperature(uint8_t servo_id)
+unsigned char get_current_temperature(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,CURRENT_TEMP,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,CURRENT_TEMP);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t is_moving(uint8_t servo_id)
+unsigned char is_moving(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,MOVING,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,MOVING);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t is_locked(uint8_t servo_id)
+unsigned char is_locked(unsigned char servo_id)
 {
-  uint8_t value;
+  unsigned char value;
 
-  if(dyn_master_read_byte(servo_id,LOCK,&value)==DYN_SUCCESS)
+  value=dxl_read_byte(servo_id,LOCK);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFF;//error
 }
 
-uint8_t lock(uint8_t servo_id)
+unsigned char lock(unsigned char servo_id)
 {
-  if(dyn_master_write_byte(servo_id,LOCK,0x01)==DYN_SUCCESS)
+  dxl_write_byte(servo_id,LOCK,0x01);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
 }
 
-uint16_t get_punch(uint8_t servo_id)
+unsigned short int get_punch(unsigned char servo_id)
 {
-  uint16_t value;
+  unsigned short int value;
 
-  if(dyn_master_read_word(servo_id,PUNCH,&value)==DYN_SUCCESS)
+  value=dxl_read_word(servo_id,PUNCH);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return value;// the actual value
   else
     return 0xFFFF;//error
 }
 
-uint8_t set_punch(uint8_t servo_id, uint16_t punch)
+unsigned char set_punch(unsigned char servo_id, unsigned short int punch)
 {
-  if(dyn_master_write_word(servo_id,PUNCH,punch)==DYN_SUCCESS)
+  dxl_write_word(servo_id,PUNCH,punch);
+  if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS)
     return 0x00;// the actual value
   else
     return 0xFF;//error
diff --git a/dyn_devices/src/examples/Makefile b/dyn_devices/src/examples/Makefile
deleted file mode 100755
index e47489236aa81efc8ff2c3bd7ba6d599ae7006d5..0000000000000000000000000000000000000000
--- a/dyn_devices/src/examples/Makefile
+++ /dev/null
@@ -1,47 +0,0 @@
-PROJECT=dyn_servos_ex
-########################################################
-# afegir tots els fitxers que s'han de compilar aquí
-########################################################
-SOURCES=main.c
-
-OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/
-DEV_DIR=../../
-COMM_DIR=../../../communications/
-CC=avr-gcc
-OBJCOPY=avr-objcopy
-MMCU=atmega2561
-
-LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
-
-INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include
-
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
-
-LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
-
-HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
-
-.PHONY: all
-
-all: communications dyn_devices $(PROJECT).hex
-
-$(PROJECT).hex: $(PROJECT).elf
-	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
-$(PROJECT).elf: $(OBJS) $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
-	$(CC) $(LDFLAGS) $(OBJS) $(LIB_DIRS) $(LIBS) -o $(PROJECT).elf
-%.o:%.c
-	$(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $<
-
-communications:
-	$(MAKE) -C $(COMM_DIR)
-
-dyn_devices:
-	$(MAKE) -C $(DEV_DIR)
-
-download: $(MAIN_OUT_HEX)
-	fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new
-
-clean:
-	-rm $(PROJECT).*
-	-rm $(OBJS)
diff --git a/dyn_devices/src/examples/main.c b/dyn_devices/src/examples/main.c
index f9ad49f2833c93b6444ef382640f07026af36b93..71c5f61d8d6e5c3b53c01e5e1ca8edaccad61ef3 100755
--- a/dyn_devices/src/examples/main.c
+++ b/dyn_devices/src/examples/main.c
@@ -15,42 +15,22 @@
  *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "dyn_servos.h"
-#include "dynamixel_master.h"
-#include "serial_console.h"
-#include <stdio.h>
-#include <util/delay.h>
-
-int main(void)
-{
-  unsigned char num;
-  unsigned char ids[32];
-  unsigned short int model,position;
-
-  serial_console_init(57600);
-  dyn_master_init();
-  sei();
-
-  dyn_master_scan(&num,ids);
-  if(num>0)
-  {
-    model=get_model_number(ids[0]);
-    printf("%d\n",model);
-    set_target_speed(ids[0],100);
-    for(;;)
-    {
-      set_target_position(ids[0],0);
-      do{
-        position=get_current_position(ids[0]);
-      }while(position>24);
-      set_target_position(ids[0],1023);
-      do{
-        position=get_current_position(ids[0]);
-      }while(position<1000);
-    }
-  }
-  else
-    printf("No device found");
-
-  while(1);
-}
+#include "dyn_servos.h"
+#include "dynamixel.h"
+#include "serial.h"
+#include <stdio.h>
+
+int main(void)
+{
+  unsigned short int model;
+
+  serial_initialize(57600);
+
+  dxl_initialize(0,1);
+
+  model=get_model_number(1);
+
+  printf("%d\n",model);
+
+  while(1);
+}
diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index af73baf40bd2f1baf60df456187de07af573fbb6..d11525e49666758c5ec893b10c389c9c2d1d3d6f 100755
--- a/dyn_devices/src/exp_board/exp_board.c
+++ b/dyn_devices/src/exp_board/exp_board.c
@@ -19,16 +19,8 @@
 #include "exp_board.h"
 #include "dynamixel.h"
 
-/* private variables */
 unsigned char exp_board_id;
 
-/* private functions */
-void exp_board_loop(void)
-{
-  
-}
-
-// public functions
 // expansion board initialize
 void exp_board_init(unsigned char exp_board)
 {
diff --git a/motion/Makefile b/motion/Makefile
index a6372baf5b0addcfbf95247cdd79507d8e64509a..4aca4c28a131916f4846509f591471e03b54b161 100755
--- a/motion/Makefile
+++ b/motion/Makefile
@@ -1,27 +1,54 @@
+ROBOTIS_C_DIR = ../robotis_lib
+
+ifdef ROBOTIS_C_DIR
+    ROBOTIS_C_DIR_FOUND=true
+else
+    ROBOTIS_C_DIR_FOUND=false
+endif
+
 PROJECT=libmotion_manager
-SOURCES=src/motion_manager.c src/action.c src/motion_pages.c src/balance.c
+SOURCES=src/motion_manager.c src/action.c src/motion_pages.c
 OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/ 
-BIN_DIR=./build/
-LIB_DIR=./lib/
-COMM_DIR=../communications/
-DEV_DIR=../dyn_devices/
-CONT_DIR=../controllers/
+SDIR=./src/ 
+IDIR=./include/
+BDIR=./build/
+LDIR=./lib/
 CC=avr-gcc
-OBJCOPY=avr-ar
+OBJCOPY=avr-ar rsc
 MMCU=atmega2561
 
-INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/ -I$(CONT_DIR)include/
+ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/
 
-LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
+SERVOS_INCLUDE_DIR=../dyn_devices/include/
+
+CONTROLLER_INCLUDE_DIR=../controllers/include/
 
 CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
-ARFLAGS= rsc
+.PHONY: all check_robotis_c show_banner clean
+
+all: check_robotis_c show_banner $(PROJECT).a
 
-.PHONY: all show_banner clean
+check_robotis_c:
+	@if ! $(ROBOTIS_C_DIR_FOUND); then                                                 \
+	    echo " IMPORTANT !!! ";                                                        \
+	    echo " ROBOTIS_C_DIR variable is not set! The software won't compile";         \
+	    echo " unless you specify the path to Robotis embedded C software.";           \
+	    echo "";                                                                       \
+	    echo " You should:";                                                           \
+	    echo "   1. Edit the makefile, at the begging set up ROBOTIS_C_DIR";           \
+	    echo "   2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \
+	    echo "";                                                                       \
+	    echo " Beware of:";                                                            \
+	    echo "   1. Do not use '(' in the directory name (as it comes from robotis) "; \
+	    echo "   2. Absolute path is preferred to avoid problems with submake calls"; \
+	    exit 2;                                                                        \
+	fi
 
-all: show_banner communications dyn_devices $(PROJECT).a
+	@if [ ! -d $(ROBOTIS_C_DIR) ]; then                                                  \
+	   echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it.";  \
+	   exit 2;                                                                           \
+	fi
 
 show_banner:
 	@echo "------------------------------------------------------";
@@ -37,21 +64,11 @@ show_banner:
 
 
 $(PROJECT).a: ${OBJS}
-	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS)
-
-communications:
-	$(MAKE) -C $(COMM_DIR)
-
-dyn_devices:
-	$(MAKE) -C $(DEV_DIR)
-
-examples:
-	$(MAKE) -C src/examples
+	$(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS)
 
 %.o: %.c 
-	$(CC) -c $(CFLAGS) ${INC_DIRS} -o $@ $<
+	$(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -I${SERVOS_INCLUDE_DIR} -I${CONTROLLER_INCLUDE_DIR} -o $@ $<
 
 clean:
-	rm -f ${LIB_DIR}$(PROJECT).a
+	rm -f ${LDIR}$(PROJECT).a
 	rm -f $(OBJS) 
-	$(MAKE) -C src/examples clean
diff --git a/motion/include/action.h b/motion/include/action.h
index 929ad3fa569e85a042af698bd77a7e1e168f24ec..549728d6745d48b1e7eaeb32fab03d9045a59000 100644
--- a/motion/include/action.h
+++ b/motion/include/action.h
@@ -1,10 +1,18 @@
 #ifndef _ACTION_H
 #define _ACTION_H
 
+#include <inttypes.h>
+
+#define NUM_SERVOS 	18
+
 // basic motion interface
+void action_init(void);
 uint8_t action_set_page(uint8_t page_id);
 void action_start_page(void);
 void action_stop_page(void);
 uint8_t is_action_running(void);
 
+// motion manager interface
+void action_process(void);
+
 #endif
diff --git a/motion/include/balance.h b/motion/include/balance.h
deleted file mode 100644
index 5c227eb00a2be42b38b8fcdd38541ed20935aced..0000000000000000000000000000000000000000
--- a/motion/include/balance.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef _BALANCE_H
-#define _BALANCE_H
-
-typedef enum {robot_standing,robot_face_down,robot_face_up} fallen_t;
-
-// public functions
-void balance_init(void);
-/**
- * \brief Function to check if the robot has fallen to the ground
- * 
- * This function returns:
- *   0x00 : if the robot is standing.
- *   0x01 : if the robot has fallen face-to-ground 
- *   0x02 : if the robot has fallen back-to-ground
- * After reading the value, the internal state is automatically set
- * back to 0x00
- */
-fallen_t balance_robot_has_fallen(void);
-uint8_t balance_calibrate_gyro(void);
-/**
- * \brief Function to enable the internal gyros
- * 
- * If the gyroscopes have been detected and calibrated in the init_motion() function,
- * this function enbales them to be used. When enabled they perform two tasks: first
- * balance the robot while moving to make it more stable and also detect when the 
- * robot falls.
- */
-void balance_enable_gyro(void);
-/**
- * \brief Function to disable the internal gyros
- *
- * If the gyroscopes have been detected and calibrated in the init_motion() function,
- * this function disales them. When disabled it is no longer possible to detect when 
- * robot falls. gyro
- *
- * This function may be used to avoid cancelling the standing up action due to fast 
- * motions associated with this motion. After the robot is standing up, the gyros may
- * be enabled again.
- */
-void balance_disable_gyro(void);
-/**
- * \brief Function to check whether the gyros are enabled or not
- *
- * This function checks wether the gyros are currently enabled or not.
- *
- * \return 0x01 if the gyros are enabled and 0x00 otherwise.
- */
-uint8_t balance_is_gyro_enabled(void);
-
-#endif
diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h
deleted file mode 100644
index f96994c4cfcd0624ab272636e478ffeceefb09dd..0000000000000000000000000000000000000000
--- a/motion/include/motion_cfg.h
+++ /dev/null
@@ -1,74 +0,0 @@
-#ifndef _MOTION_CFG_H
-#define _MOTION_CFG_H
-
-#include "buzzer.h"
-
-#ifndef MANAGER_MAX_NUM_SERVOS
-  #define MANAGER_MAX_NUM_SERVOS                  18
-#endif
-
-#ifndef MANAGER_MISSING_SERVOS_ALARM_NOTE
-  #define MANAGER_MISSING_SERVOS_ALARM_NOTE       NOTE_SOL
-#endif
-
-#ifndef MANAGER_MISSING_SERVOS_ALARM_TIME_ON
-  #define MANAGER_MISSING_SERVOS_ALARM_TIME_ON    10
-#endif
-
-#ifndef MANAGER_MISSING_SERVOS_ALARM_TIME_OFF
-  #define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF   100
-#endif
-
-#ifndef BALANCE_GYRO_CAL_NUM_SAMPLES
-  #define BALANCE_GYRO_CAL_NUM_SAMPLES            10
-#endif 
-
-#ifndef BALANCE_GYRO_X_CHANNHEL
-  #define BALANCE_GYRO_X_CHANNEL                  3
-#endif 
-
-#ifndef BALANCE_GYRO_Y_CHANNEL
-  #define BALANCE_GYRO_Y_CHANNEL                  4
-#endif
-
-#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_NOTE
-  #define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE      NOTE_SOL
-#endif
-
-#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON
-  #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON   10
-#endif
-
-#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF
-  #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF  100
-#endif
-
-#ifndef BALANCE_MAX_CAL_GYRO_ERROR
-  #define BALANCE_MAX_CAL_GYRO_ERROR              20.0
-#endif
-
-#ifndef BALANCE_FORWARD_FALL_THD_VALUE
-  #define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
-#endif
-
-#ifndef BALANCE_BACKWARD_FALL_THD_VALUE
-  #define BALANCE_BACKWARD_FALL_THD_VALUE         200
-#endif
-
-#ifndef BALANCE_KNEE_GAIN
-  #define BALANCE_KNEE_GAIN                       (4.0/54.0)
-#endif
-
-#ifndef BALANCE_ANKLE_ROLL_GAIN
-  #define BALANCE_ANKLE_ROLL_GAIN                 (4.0/18.0)
-#endif
-
-#ifndef BALANCE_HIP_PITCH_GAIN
-  #define BALANCE_HIP_PITCH_GAIN                  (4.0/20.0)
-#endif
-
-#ifndef BALANCE_ANKLE_PITCH_GAIN
-  #define BALANCE_ANKLE_PITCH_GAIN                (4.0/40.0)
-#endif
-
-#endif
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 29b0ee61b9c8905828b823f6c75135ba69be8682..1745d0855f444a847a2219c003100890aee2ed97 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -2,19 +2,109 @@
 #define _MOTION_MANAGER_H
 
 #include <inttypes.h>
+#include "motion_pages.h"
+
+#define       MANAGER_MAX_NUM_SERVOS         20
+#define       GYRO_CAL_NUM_SAMPLES           10
+#define       X_GYRO_CHANNEL                  3
+#define       Y_GYRO_CHANNEL                  4
+
+extern TPage action_next_page;
+extern TPage action_current_page;
 
 // servo information structure
 typedef struct{
   uint8_t id;
+  uint16_t encoder_resolution;
+  uint8_t gear_ratio;
   uint16_t max_value;
   uint16_t min_value;
   uint16_t center_value;
+  uint16_t center_angle;
+  uint16_t max_speed;
   uint16_t current_value;
+  int16_t max_angle;
+  int16_t min_angle;
+  int16_t current_angle;
+  uint8_t module;
   uint8_t cc_slope;
   uint8_t ccw_slope;
 }TServoInfo;
 
+int16_t servo_offsets[MANAGER_MAX_NUM_SERVOS];
+
 // public functions
-uint8_t manager_init(uint8_t num_servos);
+void manager_init(unsigned char num_servos);
+void manager_set_index_value(uint8_t servo_id, uint16_t value);
+void manager_set_servo_value(uint8_t servo_id, uint16_t value);
+uint16_t manager_get_index_value(uint8_t servo_id);
+uint16_t manager_get_servo_value(uint8_t servo_id);
+
+void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope);
+
+void manager_set_servo_value(uint8_t servo_id, uint16_t value);
+void manager_set_index_value(uint8_t servo_id, uint16_t value);
+uint16_t manager_get_servo_value(uint8_t servo_id);
+uint16_t manager_get_index_value(uint8_t servo_id);
+
+/**
+ * \brief Function to check if the robot has fallen to the ground
+ * 
+ * This function returns:
+ *   0x00 : if the robot is standing.
+ *   0x01 : if the robot has fallen face-to-ground 
+ *   0x02 : if the robot has fallen back-to-ground
+ * After reading the value, the internal state is automatically set
+ * back to 0x00
+ */
+unsigned char manager_check_robot_fallen(void);
 
+// gyro interface
+unsigned char manager_calibrate_gyro(void);
+/**
+ * \brief Function to enable the internal gyros
+ * 
+ * If the gyroscopes have been detected and calibrated in the init_motion() function,
+ * this function enbales them to be used. When enabled they perform two tasks: first
+ * balance the robot while moving to make it more stable and also detect when the 
+ * robot falls.
+ */
+void manager_enable_gyro(void);
+/**
+ * \brief Function to disable the internal gyros
+ *
+ * If the gyroscopes have been detected and calibrated in the init_motion() function,
+ * this function disales them. When disabled it is no longer possible to detect when 
+ * robot falls. gyro
+ *
+ * This function may be used to avoid cancelling the standing up action due to fast 
+ * motions associated with this motion. After the robot is standing up, the gyros may
+ * be enabled again.
+ */
+void manager_disable_gyro(void);
+/**
+ * \brief Function to check whether the gyros are enabled or not
+ *
+ * This function checks wether the gyros are currently enabled or not.
+ *
+ * \return 0x01 if the gyros are enabled and 0x00 otherwise.
+ */
+unsigned char manager_is_gyro_enabled(void);
+/**
+ * \brief Function to assign a used defined callback function
+ *
+ * By default, a function is used to balance the robot motion given the values of 
+ * gyroscopes. This balance function can be chnaged by the user by means of this
+ * function call.
+ * 
+ * \param function the new function to be used to balance the robot motion. The 
+ *                 prototype of this function must be as follows:
+ *
+ *       void <func_name>(int16_t x_gyro,int16_t y_gyro, int16_t *offsets)
+ *                 
+ *                 where x_gyro and y_gyro are the offset values of the gyroscopes
+ *                 and the output offsets to be applied to all the servos. This 
+ *                 last vector must have the same size as the number of servos
+ *                 controlled by the motion module.
+ */
 #endif
diff --git a/motion/src/action.c b/motion/src/action.c
index 300aa966c768d52e739d604dd6e8fdf6d2af55b0..4cd742e9d92ddf8dd892d85b7dff595a37388967 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -1,11 +1,12 @@
+#include "action.h"
+#include "dyn_servos.h"
+#include "motion_pages.h"
+#include "motion_manager.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
-#include "motion_cfg.h"
-#include "action.h"
-#include "motion_pages.h"
-#include "serial_console.h"
-#include <stdio.h>
+#include "cm510.h"
+
 
 /**************************************
  * Section             /----\
@@ -22,12 +23,6 @@ typedef enum{ ZERO_FINISH, NONE_ZERO_FINISH} TStopMode;
 #define       TIME_BASE_SCHEDULE       0x0a
 #define       INVALID_BIT_MASK         0x4000
 
-// external functions
-extern uint16_t manager_get_index_value(uint8_t servo_id);
-extern uint16_t manager_get_servo_value(uint8_t servo_id);
-extern void manager_set_servo_value(uint8_t servo_id, uint16_t value);
-extern void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope);
-
 // private variables
 uint8_t action_finished;
 uint8_t action_stop;
@@ -76,7 +71,10 @@ uint8_t action_set_page(uint8_t page_id)
 
   error=load_page_info(page_id,&action_current_page);
   if(error==0)
+  {
+    //ram_write_byte(CM730_ACTION_NUMBER,page_id);
     action_current_index=page_id;
+  }
   return error;
 }
 
@@ -92,20 +90,26 @@ void action_start_page(void)
   action_step_count = 0;
   bPlayRepeatCount = action_current_page.header.repeat;
   action_next_index = 0x00;
+  
 
-  for( i=1; i<=MANAGER_MAX_NUM_SERVOS; i++ )
+  for( i=1; i<=NUM_SERVOS; i++ )
   {
-    wpTargetAngle1024[i] = manager_get_index_value(i);
-    ipLastOutSpeed1024[i] = 0;
-    ipMovingAngle1024[i] = 0;
-    ipGoalSpeed1024[i] = 0;
+      wpTargetAngle1024[i] = manager_get_index_value(i);
+      ipLastOutSpeed1024[i] = 0;
+      ipMovingAngle1024[i] = 0;
+      ipGoalSpeed1024[i] = 0;
   }
   action_is_running=0x01;
+  //ram_write_byte(CM730_ACTION_STATUS,0x01);
+  //ram_set_bit(CM730_ACTION_CONTROL,0x00);
+  //ram_clear_bit(CM730_ACTION_CONTROL,0x01);
 }
 
 void action_stop_page(void)
 {
   action_stop=0x01;
+  //ram_clear_bit(CM730_ACTION_CONTROL,0x00);
+  //ram_set_bit(CM730_ACTION_CONTROL,0x01);
 }
 
 uint8_t is_action_running(void)
@@ -128,6 +132,7 @@ void action_process(void)
   int64_t lStartSpeed1024_PreTime_256T;
   int64_t lMovingAngle_Speed1024Scale_256T_2T;
   int64_t lDivider1,lDivider2;
+  uint16_t wMaxAngle1024;
   uint16_t wMaxSpeed256;
   uint16_t wPrevTargetAngle; // Start position
   uint16_t wCurrentTargetAngle; // Target position
@@ -142,7 +147,7 @@ void action_process(void)
   {
     wUnitTimeCount = 0;
 
-    for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+    for(bID=1;bID<=NUM_SERVOS;bID++)
     {
       wpStartAngle1024[bID] = manager_get_servo_value(bID);
       ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
@@ -153,9 +158,9 @@ void action_process(void)
     {
       // MAIN Section Áغñ
       bSection = MAIN_SECTION;
-//      turn_led_off(LED_TxD);
+      turn_led_off(LED_TxD);
       wUnitTimeNum =  wUnitTimeTotalNum - (wAccelStep << 1);
-      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+      for(bID=1;bID<=NUM_SERVOS;bID++)
       {
         if( bpFinishType[bID] == NONE_ZERO_FINISH )
         {
@@ -172,10 +177,10 @@ void action_process(void)
     {
       // POST Section Áغñ
       bSection = POST_SECTION;
-//      turn_led_off(LED_TxD);
+      turn_led_off(LED_TxD);
       wUnitTimeNum = wAccelStep;
 
-      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+      for(bID=1;bID<=NUM_SERVOS;bID++)
       {
         ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
       }
@@ -186,22 +191,22 @@ void action_process(void)
       if( wPauseTime )
       {
         bSection = PAUSE_SECTION;
-//        turn_led_off(LED_TxD);
+        turn_led_off(LED_TxD);
         wUnitTimeNum = wPauseTime;
       }
       else
       {
         bSection = PRE_SECTION;
-//        turn_led_on(LED_TxD);
+        turn_led_on(LED_TxD);
       }
     }
     else if( bSection == PAUSE_SECTION )
     {
       // PRE Section Áغñ
       bSection = PRE_SECTION;
-//      turn_led_on(LED_TxD);
+      turn_led_on(LED_TxD);
 
-      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+      for(bID=1;bID<=NUM_SERVOS;bID++)
       {
         ipLastOutSpeed1024[bID] = 0;
       }
@@ -268,7 +273,7 @@ void action_process(void)
         //  wMaxAngle1024 = 0;
 
       ////////// Jointº° ÆÄ¶ó¹ÌÅÍ °è»ê
-      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+      for(bID=1;bID<=NUM_SERVOS;bID++)
       {
           // ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÁÀ¸·Î ±ËÀûÀ» °è»ê
           ipAccelAngle1024[bID] = 0;
@@ -361,7 +366,7 @@ void action_process(void)
 
       if(lDivider2 == 0)
         lDivider2 = 1;
-      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+      for(bID=1;bID<=NUM_SERVOS;bID++)
       {
           lStartSpeed1024_PreTime_256T = (long)ipLastOutSpeed1024[bID] * ulPreSectionTime256T; //  *300/1024 * 1024/720 * 256 * 2
           lMovingAngle_Speed1024Scale_256T_2T = (((long)ipMovingAngle1024[bID]) * 2560L) / 12;
@@ -384,11 +389,11 @@ void action_process(void)
     wUnitTimeCount++;
     if( bSection == PAUSE_SECTION )
     {
-//      toggle_led(LED_AUX);
+      toggle_led(LED_AUX);
     }
     else
     {
-      for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++)
+      for(bID=1;bID<=NUM_SERVOS;bID++)
       {
         if( ipMovingAngle1024[bID] == 0 )
           manager_set_servo_value(bID,wpStartAngle1024[bID]);
diff --git a/motion/src/balance.c b/motion/src/balance.c
deleted file mode 100644
index d7461ef0fca2bfb56269050ca5a558a5045ec857..0000000000000000000000000000000000000000
--- a/motion/src/balance.c
+++ /dev/null
@@ -1,141 +0,0 @@
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <util/delay.h>
-#include "motion_cfg.h"
-#include "balance.h"
-#include "adc.h"
-#include "buzzer.h"
-
-// external functions
-extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
-extern void buzzer_stop_alarm(void);
-
-// private variables
-uint16_t balance_x_gyro_center;
-uint16_t balance_y_gyro_center;
-uint8_t balance_gyro_calibrated;
-uint8_t balance_enabled;
-uint8_t balance_robot_fallen_state;
-int16_t balance_offsets[MANAGER_MAX_NUM_SERVOS];
-
-// private functions
-void balance_loop(void)
-{
-  int16_t gyro_x=0;
-  int16_t gyro_y=0;
-
-  if(balance_enabled == 0x01 && balance_gyro_calibrated == 0x01)
-  {
-    // get x,y gyroscope
-    gyro_x=get_adc_channel(BALANCE_GYRO_X_CHANNEL)-balance_x_gyro_center;
-    gyro_y=get_adc_channel(BALANCE_GYRO_Y_CHANNEL)-balance_y_gyro_center;
-
-    if(gyro_x > BALANCE_BACKWARD_FALL_THD_VALUE)
-      balance_robot_fallen_state = robot_face_up;
-    else if(gyro_x < BALANCE_FORWARD_FALL_THD_VALUE)
-      balance_robot_fallen_state = robot_face_down;
-
-    float x_error1,x_error2,y_error1,y_error2;
-    x_error1=gyro_x*BALANCE_KNEE_GAIN;//4.0/54.0;
-    x_error2=gyro_x*BALANCE_ANKLE_ROLL_GAIN;//4.0/18.0;
-    y_error1=gyro_y*BALANCE_HIP_PITCH_GAIN;//4.0/20.0;
-    y_error2=gyro_y*BALANCE_ANKLE_PITCH_GAIN;//4.0/40.0;
-
-    balance_offsets[8] = (uint16_t)y_error1;
-    balance_offsets[9] = (uint16_t)y_error1;
-    balance_offsets[12] = (uint16_t)x_error1;//-1.0;
-    balance_offsets[13] = (uint16_t)-x_error1;//+1.0;
-    balance_offsets[14] = (uint16_t)+x_error2;
-    balance_offsets[15] = (uint16_t)-x_error2;
-    balance_offsets[16] = (uint16_t)-y_error2;
-    balance_offsets[17] = (uint16_t)-y_error2;
-  }
-}
-
-int16_t balance_get_offset(uint8_t index)
-{
-  return balance_offsets[index];
-}
-
-void balance_get_all_offsets(int16_t **offsets)
-{
-  *offsets=&balance_offsets[0];
-}
-
-// public functions
-void balance_init(void)
-{
-  uint8_t i;
-
-  balance_x_gyro_center=0;
-  balance_y_gyro_center=0;
-  balance_gyro_calibrated=0x00;
-  balance_enabled=0x00;
-  balance_robot_fallen_state=robot_standing;
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
-    balance_offsets[i]=0;
-}
-
-uint8_t balance_robot_has_fallen(void)
-{
-  uint8_t state = balance_robot_fallen_state;
-  balance_robot_fallen_state = robot_standing;
-  return state;
-}
-
-uint8_t balance_calibrate_gyro(void)
-{
-  uint16_t x_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES];
-  uint16_t y_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES];
-  float x_gyro_average=0.0,y_gyro_average=0.0,std_x=0.0,std_y=0.0;
-  uint8_t i=0;
-
-  balance_x_gyro_center=0;
-  balance_y_gyro_center=0;
-  for(i=0;i<BALANCE_GYRO_CAL_NUM_SAMPLES;i++)
-  {
-    _delay_ms(50);
-    x_gyro_values[i]=get_adc_channel(BALANCE_GYRO_X_CHANNEL); 
-    y_gyro_values[i]=get_adc_channel(BALANCE_GYRO_Y_CHANNEL); 
-    x_gyro_average+=x_gyro_values[i];
-    y_gyro_average+=y_gyro_values[i];
-  }  
-  x_gyro_average/=(float)BALANCE_GYRO_CAL_NUM_SAMPLES;
-  y_gyro_average/=(float)BALANCE_GYRO_CAL_NUM_SAMPLES;
-  // compute the standard deviation
-  for(i=0;i<BALANCE_GYRO_CAL_NUM_SAMPLES;i++)
-  {
-    std_x+=(x_gyro_values[i]-x_gyro_average)*(x_gyro_values[i]-x_gyro_average);
-    std_y+=(y_gyro_values[i]-y_gyro_average)*(y_gyro_values[i]-y_gyro_average);
-  }
-  std_x=sqrt(std_x/BALANCE_GYRO_CAL_NUM_SAMPLES);
-  std_y=sqrt(std_y/BALANCE_GYRO_CAL_NUM_SAMPLES);
-  if(std_x<BALANCE_MAX_CAL_GYRO_ERROR && std_y<BALANCE_MAX_CAL_GYRO_ERROR)
-  {
-    balance_gyro_calibrated=0x01;
-    balance_x_gyro_center=(uint16_t)x_gyro_average;
-    balance_y_gyro_center=(uint16_t)y_gyro_average;
-  }
-  else
-  {
-    buzzer_start_alarm(BALANCE_GYRO_CAL_FAILED_ALARM_NOTE,BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON,BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF);
-    return 0x00;
-  }
-  buzzer_stop_alarm();
-  return 0x01;
-}
-
-void balance_enable(void)
-{
-  balance_enabled=0x01;
-}
-
-void balance_disable(void)
-{
-  balance_enabled=0x00;
-}
-
-uint8_t balance_is_enabled(void)
-{
-  return balance_enabled;
-}
diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile
index 6b50b49effa4336102f6b86be4556fea9f5fa6af..63e7191ae722f8bba643d837c64e842202220a4c 100644
--- a/motion/src/examples/Makefile
+++ b/motion/src/examples/Makefile
@@ -1,55 +1,80 @@
-PROJECT=manager_ex
-########################################################
-# afegir tots els fitxers que s'han de compilar aquí
-########################################################
-SOURCES=main.c
+ROBOTIS_C_DIR =../../../robotis_lib
 
+ifdef ROBOTIS_C_DIR
+    ROBOTIS_C_DIR_FOUND=true
+else
+    ROBOTIS_C_DIR_FOUND=false
+endif
+
+PROJECT=example
+SOURCES=main.c 
 OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/
-DEV_DIR=../../../dyn_devices/
-COMM_DIR=../../../communications/
-CONT_DIR=../../../controllers/
-MAN_DIR=../../
+SDIR=../../src/ 
+IDIR=../../include/
+BDIR=../../build/
+LDIR=../../lib/
 CC=avr-gcc
-OBJCOPY=avr-objcopy
+OBJCOPY=avr-ar rsc
 MMCU=atmega2561
+CONTLIB=../../../controllers/lib/
+SERVLIB=../../../dyn_devices/lib/
+ROBOTISLIB=$(ROBOTIS_C_DIR)/lib/
 
-LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
+ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/
 
-INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
+SERVOS_INCLUDE_DIR=../../../dyn_devices/include/
 
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CONTROLLER_INCLUDE_DIR=../../../controllers/include/
 
-LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
+CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
-HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
+.PHONY: all check_robotis_c show_banner clean
 
-.PHONY: all
+all: check_robotis_c show_banner $(PROJECT).elf $(PROJECT).hex
 
-all: communications dyn_devices controllers motion_manager $(PROJECT).hex
+check_robotis_c:
+	@if ! $(ROBOTIS_C_DIR_FOUND); then                                                 \
+	    echo " IMPORTANT !!! ";                                                        \
+	    echo " ROBOTIS_C_DIR variable is not set! The software won't compile";         \
+	    echo " unless you specify the path to Robotis embedded C software.";           \
+	    echo "";                                                                       \
+	    echo " You should:";                                                           \
+	    echo "   1. Edit the makefile, at the begging set up ROBOTIS_C_DIR";           \
+	    echo "   2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \
+	    echo "";                                                                       \
+	    echo " Beware of:";                                                            \
+	    echo "   1. Do not use '(' in the directory name (as it comes from robotis) "; \
+	    echo "   2. Absolute path is preferred to avoid problems with submake calls"; \
+	    exit 2;                                                                        \
+	fi
 
-$(PROJECT).hex: $(PROJECT).elf
-	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
-$(PROJECT).elf: $(OBJS)
-	$(CC) $(LDFLAGS) $(OBJS) $(LIBS) -o $(PROJECT).elf
-%.o:%.c
-	$(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $<
+	@if [ ! -d $(ROBOTIS_C_DIR) ]; then                                                  \
+	   echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it.";  \
+	   exit 2;                                                                           \
+	fi
 
-communications:
-	$(MAKE) -C $(COMM_DIR)
+show_banner:
+	@echo "------------------------------------------------------";
+	@echo "       _____                                          ";
+	@echo "      /  _  \                                         ";
+	@echo "      | |_| |               The Humanoid Lab          ";
+	@echo "  ____\_____/____                                     ";
+	@echo " /               \    http://apollo.upc.es/humanoide/ ";
+	@echo "/   _         _   \                                   ";
+	@echo "|  | |       | |  |            $(PROJECT)             ";
+	@echo "|  | |       | |  |                                   ";
+	@echo "------------------------------------------------------";
 
-dyn_devices:
-	$(MAKE) -C $(DEV_DIR)
 
-controllers:
-	$(MAKE) -C $(CONT_DIR)
+$(PROJECT).elf: ${OBJS}
+	$(CC) -g -mmcu=$(MMCU) -o ${LDIR}$(PROJECT).elf $(OBJS) -L $(LDIR) -l motion_manager -L $(CONTLIB) -l controllers -L $(SERVLIB) -l dyn_devices -L $(ROBOTISLIB) -l dynamixel -L $(ROBOTISLIB) -l serial
 
-motion_manager:
-	$(MAKE) -C $(MAN_DIR)
+$(PROJECT).hex: $(PROJECT).elf
+	avr-objcopy -O ihex $(LDIR)$(PROJECT).elf $(PROJECT).hex 
 
-download: $(MAIN_OUT_HEX)
-	fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new
+%.o: %.c 
+	$(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -I${SERVOS_INCLUDE_DIR} -I${CONTROLLER_INCLUDE_DIR} -o $@ $<
 
 clean:
-	-rm $(PROJECT).*
-	-rm $(OBJS)
+	rm -f ${LDIR}$(PROJECT).a
+	rm -f $(OBJS) 
diff --git a/motion/src/examples/main.c b/motion/src/examples/main.c
index 93e0ce421b3f7aa7d16e5c3292f48b1b75be7497..3a50003e54f89fc451dccd8f42ae6746c1a085a7 100644
--- a/motion/src/examples/main.c
+++ b/motion/src/examples/main.c
@@ -1,35 +1,65 @@
+#include "motion_manager.h"
+#include "action.h"
+#include "cm510.h"
 #include <util/delay.h>
 #include <stdio.h>
-#include "cm510.h"
+#include "exp_board.h"
+#include "exp_board_reg.h"
+#include "serial.h"
 
-void user_init(void)
-{
-  serial_console_init(57600);
-}
 
-void user_loop(void)
+int main(void)
 {
-  if(is_button_falling_edge(BTN_UP))
-  {
-    if(is_action_running()==0x00)
-    {
-      printf("start action\n");
-      if(action_set_page(3)==0)
-        action_start_page();
-      else
-        printf("Error loading page\n");
-    }
+	int n_servos = 18;
+	init_cm510(ASYNC);
+
+  manager_init(n_servos);
+
+  int exp_board_ad_4, exp_board_ad_5, gyro_x, gyro_y;
+
+  //serial_initialize(57600);
+  exp_board_init(192);
+
+
+  action_init();
+	action_set_page(1);
+	action_start_page();
+	_delay_ms(2000);
+
+	if(manager_calibrate_gyro())
+	{
+		manager_enable_gyro();
+	  action_set_page(3);
+	  action_start_page();	
   }
-  else
+  
+  exp_board_ad_4 = get_adc_avg_channel(ADC4);
+  while(exp_board_ad_4 < 400)
   {
-    if(is_button_falling_edge(BTN_DOWN))
-    {
-      if(is_action_running())
-      {
-        printf("stop action\n");
-        action_stop_page();
-      }
-    }
+  	exp_board_ad_4 = get_adc_avg_channel(ADC4);
+  	//printf("exp_board_ad_4: %d\n",exp_board_ad_4);		
+  	_delay_ms(50);
   }
-}
+  //printf("stop");	
+	action_stop_page();
 
+	//while(is_action_running())	
+	action_set_page(1);
+	action_start_page();
+
+	//_delay_ms(1000);
+	//action_set_page(32);
+	//action_start_page();
+	
+	while(1) {
+    //exp_board_ad_4 = get_adc_avg_channel(ADC4);
+    //printf("exp_board_ad_4: %d\n",exp_board_ad_4);		
+
+    //exp_board_ad_5 = get_adc_avg_channel(ADC5);
+    //printf("exp_board_ad_5: %d\n\n",exp_board_ad_5);		
+		//turn_led_on(LED_PROGRAM);
+		//action_process();
+		//turn_led_off(LED_PROGRAM);
+		//_delay_ms(500);
+	}
+}
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 71232414b946f5d6c18a1da303dcc6710a015733..e70eae49157e2130aa21efe1e8738f1b7481c761 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -1,106 +1,120 @@
+#include "motion_manager.h"
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
-#include "motion_cfg.h"
-#include "motion_manager.h"
-#include "motion_pages.h"
-#include "dynamixel_master.h"
+#include "cm510.h"
+#include "dynamixel.h"
 #include "dyn_servos.h"
 #include "dyn_servos_reg.h"
 #include "action.h"
-#include "balance.h"
-#include "buzzer.h"
-
-// external functions
-extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
-extern void buzzer_stop_alarm(void);
-extern void balance_loop(void);
-extern void balance_get_all_offsets(int16_t **offsets);
-extern void action_init(void);
-extern void action_process(void);
+#include "cm510.h"
 
 // private variables
 uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
-extern TPage action_next_page;
-extern TPage action_current_page;
+volatile unsigned long timer0_overflow_count;
+
+// gyroscope private variables
+unsigned short int x_gyro_center;
+unsigned short int y_gyro_center;
+unsigned char gyro_enabled;
+unsigned char robot_fallen_state;
 
 typedef struct
 {
   uint8_t cc_slope;
   uint8_t ccw_slope;
   uint16_t position;
-}dyn_send_data;
+}dxl_send_struct;
 
-uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
-dyn_send_data data[MANAGER_MAX_NUM_SERVOS];
-TWriteData packets[MANAGER_MAX_NUM_SERVOS];
 
 // private functions
-void manager_timer_init(void)
-{
-  // initialize timer4 for buzzer time control (7.8ms period)
-  TIMSK4=0x00;// disable all interrupts
-  TIFR4=0x2F;// clear any pending interrupt
-  TCNT4H=0x00;                   
-  TCNT4L=0x00;
-  // set PWM mode with ICR top-count (CTC mode)
-  TCCR4A=0x00;
-  TCCR4A&=~(1<<WGM40);
-  TCCR4A&=~(1<<WGM41);
-  TCCR4B=0x00;
-  TCCR4B|=(1<<WGM42);
-  TCCR4B&=~(1<<WGM43);
-  // clear output compare value A
-  OCR4AH=0x3D;
-  OCR4AL=0x08;
-  TCCR4B&=0xF8;// set the prescaler to 8
-  TCCR4B|=0x02;
-}
-
-uint8_t manager_period_done(void)
-{
-  if(TIFR4&(1<<OCF4A))
-  {
-    TIFR4|=(1<<OCF4A);// clear any interrupt
-    TCNT4H=0x00;                   
-    TCNT4L=0x00;
-    return 0x01;
-  }
-  else
-    return 0x00;
-}
-
-void manager_send_motion_command(void) 
-{
-  uint8_t *pdata;
+void manager_send_motion_command(void) {
+  static uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+  static dxl_send_struct packet[MANAGER_MAX_NUM_SERVOS];
+  uint8_t * ppacket;
   uint8_t i;
   uint16_t target_pos;
-  int16_t *offsets;
+  uint8_t ccslope, ccwslope;
 
-  pdata = (uint8_t *)data;
+  ppacket = (uint8_t *  )packet;
   
-  balance_get_all_offsets(&offsets);
-  for(i=0;i<manager_num_servos;i++) 
-  {
-    target_pos = manager_servos[i].current_value + offsets[i];
-    pdata[i*4+2] = target_pos&0xFF;
-    pdata[i*4+3] = (target_pos>>8)&0xFF;
+  for(i=0;i<manager_num_servos;i++) {
+    servo_ids[i]=manager_servos[i].id;
+    target_pos = manager_servos[i].current_value + servo_offsets[i];
 
     if(manager_servos[i].cc_slope == 0) 
-      pdata[i*4] = 32;
-    else 
-      pdata[i*4] = 1<<(0x0F&manager_servos[i].cc_slope);
+      ccslope = 32;
+    else ccslope = 1<<(0x0F&manager_servos[i].cc_slope);
 
     if(manager_servos[i].ccw_slope == 0)
-      pdata[i*4+1] = 32;
-    else 
-      pdata[i*4+1] = 1<<(0x0F&manager_servos[i].ccw_slope);
+      ccwslope = 32;
+    else ccslope = 1<<(0x0F&manager_servos[i].ccw_slope);
+
+    ppacket[i*4] = ccslope;
+    ppacket[i*4+1] = ccwslope;
+    ppacket[i*4+2] = target_pos&0xFF;
+    ppacket[i*4+3] = (target_pos>>8)&0xFF;
+  }
+  
+  dxl_sync_tx_packet(manager_num_servos,(unsigned char *)servo_ids,CW_COMP_SLOPE,(unsigned char *)packet,4);
+}
+
+void manager_balance(void)
+{
+  int16_t gyro_x=0;
+  int16_t gyro_y=0;
+
+
+  if(gyro_enabled == 0x01)
+  {
+    // get x,y gyroscope
+    gyro_x=get_adc_channel(X_GYRO_CHANNEL)-x_gyro_center;
+    gyro_y=get_adc_channel(Y_GYRO_CHANNEL)-y_gyro_center;
+
+
+    if(gyro_x > 200)
+      robot_fallen_state = 0x02;
+    else if(gyro_x < -200)
+      robot_fallen_state = 0x01;
+
+    float x_error1,x_error2,y_error1,y_error2;
+    x_error1=gyro_x*4.0/54.0;
+    x_error2=gyro_x*4.0/18.0;
+    y_error1=gyro_y*4.0/20.0;
+    y_error2=gyro_y*4.0/40.0;
 
-    packets[i].data_addr=(uint8_t *)&data[i];
+    servo_offsets[8] = (uint16_t)y_error1;
+    servo_offsets[9] = (uint16_t)y_error1;
+    servo_offsets[12] = (uint16_t)x_error1;//-1.0;
+    servo_offsets[13] = (uint16_t)-x_error1;//+1.0;
+    servo_offsets[14] = (uint16_t)+x_error2;
+    servo_offsets[15] = (uint16_t)-x_error2;
+    servo_offsets[16] = (uint16_t)-y_error2;
+    servo_offsets[17] = (uint16_t)-y_error2;
   }
-  if(manager_num_servos>0)
-    dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets); 
+}
+
+inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
+{
+  return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_angle;
+}
+
+inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
+{
+  return (((int16_t)((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution))-manager_servos[servo_id].center_angle);
+}
+
+inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
+{
+  if(speed>manager_servos[servo_id].max_speed)
+    speed=manager_servos[servo_id].max_speed;
+  return (speed*3)>>1;
+}
+
+inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
+{
+  return (value*2)/3;
 }
 
 void manager_get_current_position(void)
@@ -108,7 +122,10 @@ void manager_get_current_position(void)
   uint8_t i;
 
   for(i=0;i<manager_num_servos;i++)
+  {
     manager_servos[i].current_value=get_current_position(i+1);
+//    manager_servos[i].current_angle=manager_value_to_angle(i+1,manager_servos[i].current_value);
+  }
 }
 
 void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope)
@@ -126,11 +143,6 @@ void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope)
   }
 }
 
-void manager_set_index_value(uint8_t servo_id, uint16_t value)
-{
-  manager_servos[servo_id].current_value=value;
-}
-
 void manager_set_servo_value(uint8_t servo_id, uint16_t value)
 {
   uint8_t i;
@@ -145,6 +157,13 @@ void manager_set_servo_value(uint8_t servo_id, uint16_t value)
   }
 }
 
+void manager_set_index_value(uint8_t servo_id, uint16_t value)
+{
+  manager_servos[servo_id].current_value=value;
+  manager_servos[servo_id].current_angle=((value-manager_servos[servo_id].center_value)*(manager_servos[servo_id].max_angle-manager_servos[servo_id].min_angle))/manager_servos[servo_id].max_value;
+  //ram_write_word(CM730_SERVO_1_POS_L+servo_id*2,manager_servos[servo_id].current_angle);
+}
+
 uint16_t manager_get_index_value(uint8_t servo_id)
 {
   return manager_servos[servo_id-1].current_value;
@@ -161,53 +180,176 @@ uint16_t manager_get_servo_value(uint8_t servo_id)
   return 2048;
 }
 
-void manager_loop(void)
+uint8_t manager_scan_servos(uint8_t n_servos)
 {
-  if(manager_period_done()==0x01)
+  uint8_t i;
+  uint8_t num = 0;
+  uint8_t error;
+
+  for(i=1;i<=n_servos;i++)
   {
-    // call the action process
-    action_process(); //action_ready
-    // balance the robot
-    balance_loop();
-    // send the motion commands to the servos
-    manager_send_motion_command();
+    error = dxl_ping(i);
+    if(error==0)
+    {
+      num++;      
+    }
   }
+  return num;
+}
+
+void timer_init(void)
+{
+  timer0_overflow_count=0;
+  // on the ATmega168, timer 0 is also used for fast hardware pwm
+  // (using phase-correct PWM would mean that timer 0 overflowed half as often
+  // resulting in different millis() behavior on the ATmega8 and ATmega168)
+  TCCR0A|=(1<<WGM01);
+  TCCR0A|=(1<<WGM00);
+  TCCR0B|=(1<<WGM02);
+
+  // set timer 0 prescale factor to 1024
+  TCCR0B|=(1<<CS02);
+  TCCR0B|=(1<<CS00);
+  // Write the value 122 on TOP
+  OCR0A=0x7A;
+  // enable timer 0 overflow interrupt
+  TIMSK0|=(1<<TOIE0);
+}
+
+// interrupt handlers
+SIGNAL(TIMER0_OVF_vect)
+{
+  turn_led_on(LED_PROGRAM); //debug
+  timer0_overflow_count++;
+  OCR0A=0x7A;
+
+  // call the action process
+  action_process(); //action_ready
+
+  // balance the robot
+  manager_balance();
+
+  // send the motion commands to the servos
+  manager_send_motion_command();
+
+  turn_led_off(LED_PROGRAM); //debug
 }
 
 // public functions
-uint8_t manager_init(uint8_t num_servos)
+void manager_init(unsigned char num_servos)
 {
   uint8_t i;
+  uint8_t found_servos;
+  robot_fallen_state = 0x00;
 
   // enable power to the servos
-  dyn_master_init();
+  dxl_initialize(0,1);
   _delay_ms(500);
 
-  /* scan the bus for all available servos */
-  dyn_master_scan(&manager_num_servos,servo_ids);
-  if(num_servos != manager_num_servos)
-    buzzer_start_alarm(MANAGER_MISSING_SERVOS_ALARM_NOTE,MANAGER_MISSING_SERVOS_ALARM_TIME_ON,MANAGER_MISSING_SERVOS_ALARM_TIME_OFF);
-  else
-    buzzer_stop_alarm();
+  if(num_servos>MANAGER_MAX_NUM_SERVOS)
+    return;
+
+  manager_num_servos=num_servos;
+  found_servos = manager_scan_servos(num_servos);
+  if(num_servos != found_servos)
+    turn_led_on(LED_MANAGE);
 
   for(i=0;i<manager_num_servos;i++)
   {
     //set_target_speed(i+1,0);
-    enable_servo(servo_ids[i]);
-    manager_servos[i].id=servo_ids[i];
+    servo_offsets[i] = 0;
+
+    enable_servo(i+1);
+    manager_servos[i].id=i+1;
+    manager_servos[i].encoder_resolution=1023;
+    manager_servos[i].gear_ratio=254;
+    manager_servos[i].max_angle=300<<7;
+    manager_servos[i].center_angle=150<<7;
+    manager_servos[i].max_speed=354;
     manager_servos[i].max_value=1023;
     manager_servos[i].min_value=0;
     manager_servos[i].center_value=512;
-    manager_servos[i].current_value=get_current_position(servo_ids[i]);
+    manager_servos[i].max_angle=1500;
+    manager_servos[i].min_angle=-1500;
+    manager_servos[i].current_value=get_current_position(i+1);
+ //   manager_servos[i].current_angle=manager_value_to_angle(i+1,manager_servos[i].current_value);
   } 
 
-  /* initialize the period timer */
-  manager_timer_init();
-  /* initialize the action module */
-  action_init();
+  timer_init();
+  // initialize the controller
+  //init_cm510(ASYNC);
+  // initialize the timer interrupts
+  sei();
+  //action_init();
+  
+}
+
+unsigned char manager_check_robot_fallen(void)
+{
+  unsigned char state = robot_fallen_state;
+  robot_fallen_state = 0x00;
+  return state;
+}
+
+unsigned char manager_calibrate_gyro(void)
+{
+  unsigned short int x_gyro_values[GYRO_CAL_NUM_SAMPLES];
+  unsigned short int y_gyro_values[GYRO_CAL_NUM_SAMPLES];
+  float x_gyro_average=0.0,y_gyro_average=0.0,std_x=0.0,std_y=0.0;
+  unsigned char calibrated=0x00;
+  unsigned char i=0,count=0;
 
+  x_gyro_center=0;
+  y_gyro_center=0;
+  while(!calibrated)
+  {
+    for(i=0;i<GYRO_CAL_NUM_SAMPLES;i++)
+    {
+      _delay_ms(50);
+      x_gyro_values[i]=get_adc_channel(X_GYRO_CHANNEL); 
+      y_gyro_values[i]=get_adc_channel(Y_GYRO_CHANNEL); 
+      x_gyro_average+=x_gyro_values[i];
+      y_gyro_average+=y_gyro_values[i];
+    }  
+    x_gyro_average/=(float)GYRO_CAL_NUM_SAMPLES;
+    y_gyro_average/=(float)GYRO_CAL_NUM_SAMPLES;
+    // compute the standard deviation
+    for(i=0;i<GYRO_CAL_NUM_SAMPLES;i++)
+    {
+      std_x+=(x_gyro_values[i]-x_gyro_average)*(x_gyro_values[i]-x_gyro_average);
+      std_y+=(y_gyro_values[i]-y_gyro_average)*(y_gyro_values[i]-y_gyro_average);
+    }
+    std_x=sqrt(std_x/GYRO_CAL_NUM_SAMPLES);
+    std_y=sqrt(std_y/GYRO_CAL_NUM_SAMPLES);
+    if(std_x<20.0 && std_y<20.0)
+    {
+      calibrated=0x01;
+      x_gyro_center=(unsigned short int)x_gyro_average;
+      y_gyro_center=(unsigned short int)y_gyro_average;
+    }
+    else
+    {
+      count++;
+      if(count==5)
+        return 0x00;
+    }
+  }
   return 0x01;
 }
 
+void manager_enable_gyro(void)
+{
+  gyro_enabled=0x01;
+}
+
+void manager_disable_gyro(void)
+{
+  gyro_enabled=0x00;
+}
+
+unsigned char manager_is_gyro_enabled(void)
+{
+  return gyro_enabled;
+}
 
 
diff --git a/motion/src/motion_pages.c b/motion/src/motion_pages.c
index ef28d68ea22f1f88f867559c985f1ed40e310420..c8641f9e46af6e56dca137118ca738206b6f0e7c 100644
--- a/motion/src/motion_pages.c
+++ b/motion/src/motion_pages.c
@@ -1,14 +1,14 @@
 #include "motion_pages.h"
+#include "cm510.h"
 
-void pages_get_page(uint8_t page_id,TPage *page) 
-{
-  uint8_t i=0;
-  uint8_t *ppage = (uint8_t *)page;
-  TPage *base_page = &((TPage *)PAGE_BASE_ADDR)[page_id];
-
+void pages_get_page(uint8_t page_id,TPage *page) {
+	uint8_t i=0;
+	uint8_t *ppage = (uint8_t *)page;
+	TPage *base_page = &((TPage *)PAGE_BASE_ADDR)[page_id];
+  
   for(i=0;i<sizeof(TPage);i++) {
-    ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i);
-  }
+		ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i);
+	}
 }
 
 uint8_t pages_check_checksum(TPage *page)
@@ -40,18 +40,24 @@ void pages_copy_page(TPage *src,TPage *dst)
     ((uint8_t *)dst)[i]=((volatile uint8_t *)src)[i];
 }
 
-uint8_t load_page_info(uint8_t page_id,TPage *page) 
-{
+
+uint8_t load_page_info(uint8_t page_id,TPage *page) {
+//	uint8_t *start_address;
+//	uint16_t i;
+
+//	start_address=(uint8_t *)POSE_BASE_ADR+(0x200*page_id);
+
+//	for(i=0;i<0x200;i++) (((uint8_t *)page)[i])=pgm_read_byte(start_address+i);
   uint32_t i=0;
   uint8_t *ppage = (uint8_t *)page;
+//  TPage *base_page = (TPage *)PAGE_BASE_ADDR;
 
-  for(i=0;i<sizeof(TPage);i++) 
-    ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+((uint32_t)sizeof(TPage)*(uint32_t)page_id)+i);
-  
-  if(pages_check_checksum(page)==0) 
-    return 0;
-  else 
-    return -1;
+  for(i=0;i<sizeof(TPage);i++) {
+    ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+(uint32_t)(sizeof(TPage)*page_id)+i);
+//    ppage[i]= pgm_read_byte_far( &(base_page[page_id])+i);
+  }
+	if(pages_check_checksum(page)==0) return 0;
+	else return -1;
 }
 
 
diff --git a/robotis_lib/src/Dynamixel/dxl_hal.c b/robotis_lib/src/Dynamixel/dxl_hal.c
index 3c39f70bc45bf3eec167102affdd74d45b96486f..6a4817982ab91bc905d3c626164768140b4399f8 100755
--- a/robotis_lib/src/Dynamixel/dxl_hal.c
+++ b/robotis_lib/src/Dynamixel/dxl_hal.c
@@ -206,4 +206,4 @@ unsigned char dxl_hal_get_queue(void)
 SIGNAL(USART0_RX_vect)
 {
 	dxl_hal_put_queue( UDR0 );
-}
+}
\ No newline at end of file