diff --git a/communications/Makefile b/communications/Makefile
index 17a0a643714bc0e6bb803b19868e0fcc25a251ae..31e357108472b6ee57388dcf06edc7cf1ffe2aff 100644
--- a/communications/Makefile
+++ b/communications/Makefile
@@ -15,7 +15,7 @@ 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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 ARFLAGS=rsc
 
diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h
index c1f76dfcd2a5e550c5a2d2a476dd22a8d59dce74..01f3d947065aa3a7c5b8dba78c94dc049154a6a7 100644
--- a/communications/include/dynamixel_master.h
+++ b/communications/include/dynamixel_master.h
@@ -9,6 +9,7 @@ extern "C" {
 
 /* public functions */
 void dyn_master_init(void);
+uint8_t dyn_master_is_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);
diff --git a/communications/include/serial_console.h b/communications/include/serial_console.h
index c2c1d0f98afe511c8367d8a7f0de89b63837cf3f..e21d13fd147c8ee3d2a863894cf37a00d1fd9810 100644
--- a/communications/include/serial_console.h
+++ b/communications/include/serial_console.h
@@ -9,6 +9,9 @@ extern "C" {
 #include <avr/interrupt.h>
 
 void serial_console_init(uint32_t baudrate);
+uint8_t serial_console_get_num_data(void);
+int cm510_printf(const char *fmt, ...);
+int cm510_scanf(const char *fmt, ... );
 
 #ifdef __cplusplus
 }
diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c
index 08b286db9711db97eb84de81445848cba36041cb..95865d9704c57df97e4ceb7d210ddbd7b6d4469f 100644
--- a/communications/src/dynamixel_master.c
+++ b/communications/src/dynamixel_master.c
@@ -13,6 +13,7 @@ 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;
+uint8_t dyn_master_initialized=0x00;
 /* private timeout variables */
 uint16_t dyn_master_rx_timeout_us;
 
@@ -213,6 +214,12 @@ void dyn_master_init(void)
   TCNT0=0x00;
 
   dyn_master_set_rx_mode();
+  dyn_master_initialized=0x01;
+}
+
+uint8_t dyn_master_is_init(void)
+{
+  return dyn_master_initialized;
 }
 
 void dyn_master_set_rx_timeout(uint16_t timeout_us)
diff --git a/communications/src/examples/Makefile b/communications/src/examples/Makefile
index fd3b35d327a4feadee4160ce1945fbf9222837d0..1c56a7a50e7a050aa5797da2170182f0604cfde0 100644
--- a/communications/src/examples/Makefile
+++ b/communications/src/examples/Makefile
@@ -14,7 +14,7 @@ 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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/communications/src/examples/main.c b/communications/src/examples/main.c
index 6188a35168e74d4c7ab8c2dc24e99b65db3eeac3..a3d6baa646d72b0c620743726f7ee7d0a3f96702 100755
--- a/communications/src/examples/main.c
+++ b/communications/src/examples/main.c
@@ -19,12 +19,12 @@ int16_t main(void)
     dyn_master_scan(&num,ids);
     if(num==1) 
     {
-      printf("Found one device\n");
+      cm510_printf("Found one device\n");
       PORTC=0x3F;
     }
     else
     {
-      printf("No device found\n");
+      cm510_printf("No device found\n");
       PORTC=0x7F;
     }
   }
diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
index 64fa7f4ecc58828bdbcadd8d31eb4787ac52d085..97dd04850a3c42e81b638f7a8440df4887923f10 100644
--- a/communications/src/serial_console.c
+++ b/communications/src/serial_console.c
@@ -1,6 +1,7 @@
 #include "comm_cfg.h"
 #include "serial_console.h"
 #include <stdio.h>
+#include <stdarg.h>
 
 /* private variables */
 volatile uint8_t serial_console_rx_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
@@ -138,3 +139,35 @@ void serial_console_init(uint32_t baudrate)
   serial_console_set_baudrate(baudrate);
   device=fdevopen(serial_console_putchar,serial_console_getchar);
 }
+
+uint8_t serial_console_get_num_data(void)
+{  
+  return serial_console_rx_num_data; 
+}
+
+int cm510_printf(const char *fmt, ...)
+{
+  #ifdef __REAL__
+  return 0;
+  #else
+  va_list ap;
+  int i;
+
+  va_start(ap,fmt);
+  i = vfprintf(stdout, fmt, ap);
+  va_end(ap);
+  return i;
+  #endif
+}
+
+int cm510_scanf(const char *fmt, ...)
+{
+  va_list arg;
+  int done;
+ 
+  va_start (arg, fmt);
+  done = vfscanf (stdin, fmt, arg);
+  va_end (arg);
+  
+  return done;
+}
diff --git a/controllers/Makefile b/controllers/Makefile
index 06de89ab4c2f0b0213c68472f70a022962d96a9f..9315f6f6f7ffacf49480ee4af33cf66e2f2c2cca 100755
--- a/controllers/Makefile
+++ b/controllers/Makefile
@@ -18,7 +18,7 @@ INC_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(DEV_DIR)include -I$(MAN_
 
 LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a $(MAN_DIR)lib/libmotion_manager.a
 
-CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 ARFLAGS=rsc
 
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index 0b2b730ac12bbc37fba3455238aed9fe85a1d636..54775760f91b5cbe79d46f74d6f9c4660d71a6be 100644
--- a/controllers/include/cm510_cfg.h
+++ b/controllers/include/cm510_cfg.h
@@ -20,7 +20,7 @@
 #define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
 
 // motion configuration parameters
-#define MANAGER_MAX_NUM_SERVOS                  18 
+#define MANAGER_MAX_NUM_SERVOS                  32 
 #define MANAGER_MISSING_SERVOS_ALARM_NOTE       NOTE_SOL
 #define MANAGER_MISSING_SERVOS_ALARM_TIME_ON    10
 #define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF   100
diff --git a/controllers/src/cm510.c b/controllers/src/cm510.c
index 5e8a96e1cddbbe97876efc43b561972712a8931a..16612ed2edf70c225e86a7808a6e29daf7121158 100755
--- a/controllers/src/cm510.c
+++ b/controllers/src/cm510.c
@@ -20,6 +20,7 @@
 #include <inttypes.h>
 #include <util/delay.h>
 #include "cm510_cfg.h"
+#include "dynamixel_master.h"
 #include "cm510.h"
 #include "exp_board.h"
 
@@ -46,12 +47,17 @@ void init_cm510(void)
 
 int16_t main(void)
 {
+  uint8_t num_servos;
+
   init_cm510();
+  turn_led_off(LED_BAT);
   sei();
-  /* call the user initialization function */
-  manager_init(18);
   _delay_ms(2000);
+  /* call the user initialization function */
+  dyn_master_init();
   exp_board_init(EXP_BOARD_ID);
+  num_servos=18+exp_pwm_num_servos();
+  manager_init(num_servos);
   user_init();
   // turn BAT_LED on to indicate the initialization is done
   turn_led_on(LED_BAT);
diff --git a/controllers/src/examples/Makefile b/controllers/src/examples/Makefile
index 7c7088841ea4c814e6c9be041459741a658ea512..7a53003c279ef951de88b4425b1804bc6c7f08cf 100644
--- a/controllers/src/examples/Makefile
+++ b/controllers/src/examples/Makefile
@@ -18,7 +18,7 @@ INCLUDE_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(MAN_DIR)include -I$(
 
 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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/controllers/src/examples/main.c b/controllers/src/examples/main.c
index a1d62161825f6ddd3735b991735363977c2d59ac..73a75bd5eebc800697165e7aa663b37501fe044e 100755
--- a/controllers/src/examples/main.c
+++ b/controllers/src/examples/main.c
@@ -29,8 +29,8 @@ void user_loop(void)
 {
   if(user_time_is_period_done())
   {
-    printf("Gyro X: %d\n",get_adc_channel(ADC_PORT_3));
-    printf("Gyro Y: %d\n",get_adc_channel(ADC_PORT_4));
+    cm510_printf("Gyro X: %d\n",get_adc_channel(ADC_PORT_3));
+    cm510_printf("Gyro Y: %d\n",get_adc_channel(ADC_PORT_4));
   }
   if(is_button_pressed(BTN_START))
     turn_led_on(LED_AUX);
diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile
index c0bcc5b33104e21947c6a0c2281013734ca8498d..0536f19dad92634c408815b9c66b05eaa38844c8 100755
--- a/dyn_devices/Makefile
+++ b/dyn_devices/Makefile
@@ -14,7 +14,7 @@ INC_DIRS=-I./include/ -I$(COMM_DIR)/include -I$(CONT_DIR)/include
 
 LIBS=$(COMM_DIR)/lib/libcomm.a
 
-CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 ARFLAGS=rsc
 
diff --git a/dyn_devices/include/exp_board.h b/dyn_devices/include/exp_board.h
index 9fae29e5a1773f53d55c9bfb4b63a0c29304b0e6..9d6e8b32d88b6fa0f36108a080ded477b08b7c21 100755
--- a/dyn_devices/include/exp_board.h
+++ b/dyn_devices/include/exp_board.h
@@ -70,6 +70,7 @@ uint8_t exp_pwm_set_frequency(uint16_t freq_hz);
 uint16_t exp_pwm_get_frequency(void);
 uint8_t exp_pwm_set_duty_cycle(pwm_id_t pwm_id, uint8_t duty);
 uint8_t exp_pwm_get_duty_cycle(pwm_id_t pwm_id);
+uint8_t exp_pwm_num_servos(void);
 /* DAC interface */
 uint8_t exp_dac_start(void);
 uint8_t exp_dac_stop(void);
diff --git a/dyn_devices/include/exp_board_reg.h b/dyn_devices/include/exp_board_reg.h
index d8e30c7b234502c2077dabde6232edc1061f353f..28da3da76a47f37855041c3cdec16dddbed2a5a0 100755
--- a/dyn_devices/include/exp_board_reg.h
+++ b/dyn_devices/include/exp_board_reg.h
@@ -239,7 +239,8 @@
 #define COMPASS_HEADING_VAL_H_block     0xCA
 #define COMPASS_AVG_HEADING_VAL_L_block 0xCB
 #define COMPASS_AVG_HEADING_VAL_H_block 0xCC
+#define NUM_PWM_SERVOS                  0xCD
 
-#define RAM_SIZE                  205
+#define RAM_SIZE                  206
 
 #endif
diff --git a/dyn_devices/src/examples/Makefile b/dyn_devices/src/examples/Makefile
index e7b85a0f0848d840f98261c705815b49cb50d5bd..05e3b278186f1d327781061eafe01ba763228476 100755
--- a/dyn_devices/src/examples/Makefile
+++ b/dyn_devices/src/examples/Makefile
@@ -19,7 +19,7 @@ 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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/dyn_devices/src/examples/exp_board_ex.c b/dyn_devices/src/examples/exp_board_ex.c
index bff254aef9ec74d57a7f01c2aecf49b065711ee9..2d757002167cf563361ec6e998a717e96069cf1f 100755
--- a/dyn_devices/src/examples/exp_board_ex.c
+++ b/dyn_devices/src/examples/exp_board_ex.c
@@ -32,41 +32,41 @@ int main(void)
   _delay_ms(4000);
   if(exp_board_init(0xC0)==0x00)
   {
-    printf("expansion baord found\n");
+    cm510_printf("expansion baord found\n");
     /* GPIO test */
 /*    if(exp_gpio_config(GPIO0,GPIO_OUT))
-      printf("Error configuring the GPIO0 pin\n");
+      cm510_printf("Error configuring the GPIO0 pin\n");
     if(exp_gpio_config(GPIO1,GPIO_IN))
-      printf("Error configuring the GPIO1 pin\n");
+      cm510_printf("Error configuring the GPIO1 pin\n");
     for(i=0;i<10;i++)
     {
       if(exp_gpio_set_value(0,1))
-        printf("Error Error setting the value of the GPIO0 pin\n");
+        cm510_printf("Error Error setting the value of the GPIO0 pin\n");
       if(exp_gpio_get_value(GPIO1)==0x01)
-        printf("GPIO pin is HIGH\n");
+        cm510_printf("GPIO pin is HIGH\n");
       else if(exp_gpio_get_value(GPIO1)==0x00)
-        printf("GPIO pin is LOW\n");
+        cm510_printf("GPIO pin is LOW\n");
       else
-        printf("Error getting the GPIO1 value\n");
+        cm510_printf("Error getting the GPIO1 value\n");
       _delay_ms(1000);
       if(exp_gpio_set_value(0,0))
-        printf("Error Error setting the value of the GPIO1 pin\n");
+        cm510_printf("Error Error setting the value of the GPIO1 pin\n");
       if(exp_gpio_get_value(GPIO1)==0x00)
-        printf("GPIO pin is LOW\n");
+        cm510_printf("GPIO pin is LOW\n");
       else if(exp_gpio_get_value(GPIO1)==0x01)
-        printf("GPIO pin is HIGH\n");
+        cm510_printf("GPIO pin is HIGH\n");
       else
-        printf("Error getting the GPIO1 value\n");
+        cm510_printf("Error getting the GPIO1 value\n");
       _delay_ms(1000);
     }*/
     /* LED and switches test */
 /*    for(i=0;i<10;i++)
     {
       exp_gpio_set_led();
-      printf("Switches value: %d\n",exp_gpio_get_switches());
+      cm510_printf("Switches value: %d\n",exp_gpio_get_switches());
       _delay_ms(1000);
       exp_gpio_clear_led();
-      printf("Switches value: %d\n",exp_gpio_get_switches());
+      cm510_printf("Switches value: %d\n",exp_gpio_get_switches());
       _delay_ms(1000);
     }*/
     /* PWM test */
@@ -89,58 +89,58 @@ int main(void)
     for(i=0;i<10;i++)
     {
       exp_dac_set_voltage(DAC0,1000); 
-      printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+      cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
       _delay_ms(1000);
       exp_dac_set_voltage(DAC0,2000); 
-      printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+      cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
       _delay_ms(1000);
       exp_dac_set_voltage(DAC0,3000); 
-      printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+      cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
       _delay_ms(1000);
       exp_dac_set_voltage(DAC0,4000); 
-      printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+      cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
       _delay_ms(1000);
     }
     dac_stop();*/
     /* compass test */
     exp_compass_start();
-    printf("Number of samples to average: %d\n",exp_compass_get_avg_samples());
+    cm510_printf("Number of samples to average: %d\n",exp_compass_get_avg_samples());
     for(i=0;i<100;i++)
     {
-      printf("Current heading: %d\n",exp_compass_get_heading());
-      printf("Current averaged heading: %d\n",exp_compass_get_avg_heading());
+      cm510_printf("Current heading: %d\n",exp_compass_get_heading());
+      cm510_printf("Current averaged heading: %d\n",exp_compass_get_avg_heading());
       _delay_ms(1000);
     }
     exp_compass_stop();
     /* ADC test */
 /*    exp_adc_start();
-    printf("Number of samples to average: %d\n",exp_adc_get_average_samples());
-    printf("Sampling period: %d ms\n",exp_adc_get_sample_period());
+    cm510_printf("Number of samples to average: %d\n",exp_adc_get_average_samples());
+    cm510_printf("Sampling period: %d ms\n",exp_adc_get_sample_period());
     for(i=0;i<100;i++)
     {
-      printf("Current voltage at ADC0: %d mV\n",exp_adc_get_channel(ADC0));
-      printf("Current averaged voltage at ADC0: %d mV\n",exp_adc_get_avg_channel(ADC0));
+      cm510_printf("Current voltage at ADC0: %d mV\n",exp_adc_get_channel(ADC0));
+      cm510_printf("Current averaged voltage at ADC0: %d mV\n",exp_adc_get_avg_channel(ADC0));
       _delay_ms(1000);
     }
     exp_adc_stop();*/
     /* UART USB test */
 /*    exp_uart_usb_start();
-    printf("current baudrate: %d\n",exp_uart_usb_get_baudrate());
+    cm510_printf("current baudrate: %d\n",exp_uart_usb_get_baudrate());
     for(i=0;i<10;i++)
     {
       while(!exp_uart_usb_is_data_available())
         _delay_ms(100);
       data=exp_uart_usb_receive_data();
-      printf("Received data: %d\n",data);
+      cm510_printf("Received data: %d\n",data);
       exp_uart_usb_send_byte(data);
       while(exp_uart_usb_is_sending())
         _delay_ms(100);
-      printf("data sent\n");
+      cm510_printf("data sent\n");
     }
     exp_uart_usb_stop();*/
   }
   else
-    printf("expansion board not found\n"); 
+    cm510_printf("expansion board not found\n"); 
 
   while(1);
 }
diff --git a/dyn_devices/src/examples/servos_ex.c b/dyn_devices/src/examples/servos_ex.c
index f9ad49f2833c93b6444ef382640f07026af36b93..3dbb1a2adc27597f007951a979b9026114b5296f 100755
--- a/dyn_devices/src/examples/servos_ex.c
+++ b/dyn_devices/src/examples/servos_ex.c
@@ -35,7 +35,7 @@ int main(void)
   if(num>0)
   {
     model=get_model_number(ids[0]);
-    printf("%d\n",model);
+    cm510_printf("%d\n",model);
     set_target_speed(ids[0],100);
     for(;;)
     {
@@ -50,7 +50,7 @@ int main(void)
     }
   }
   else
-    printf("No device found");
+    cm510_printf("No device found");
 
   while(1);
 }
diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index 7f18d59d7177c63dbb0accf8c5fc66ed31138f5f..f4df041c21ed4b96d523966776fa3c6a79c795b0 100755
--- a/dyn_devices/src/exp_board/exp_board.c
+++ b/dyn_devices/src/exp_board/exp_board.c
@@ -35,6 +35,7 @@ uint8_t exp_board_dac_present;
 uint8_t exp_board_uart_ttl_present;
 uint8_t exp_board_pwm_present;
 uint8_t exp_board_uart_usb_present;
+uint8_t exp_board_num_servos;
 
 uint8_t exp_board_int_data[BLOCK_LENGTH];
 
@@ -80,6 +81,8 @@ uint8_t exp_board_init(uint8_t board_id)
         exp_board_dac_present=0x00;
         exp_board_uart_usb_present=0x01;
       }
+      // read the number pf PWM servos
+      dyn_master_read_byte(exp_board_id,NUM_PWM_SERVOS,&exp_board_num_servos);
       return 0x00;
     } 
     else
@@ -88,6 +91,8 @@ uint8_t exp_board_init(uint8_t board_id)
       exp_board_uart_ttl_present=0x00;
       exp_board_pwm_present=0x00;
       exp_board_uart_usb_present=0x00;
+      // read the number pf PWM servos
+      dyn_master_read_byte(exp_board_id,NUM_PWM_SERVOS,&exp_board_num_servos);
       return 0xFF;
     }
   }
@@ -99,6 +104,7 @@ uint8_t exp_board_init(uint8_t board_id)
     exp_board_pwm_present=0x00;
     exp_board_uart_usb_present=0x00;
     exp_board_id=0xFF;
+    exp_board_num_servos=0;
     return 0xFF;
   }
 }
@@ -426,6 +432,11 @@ uint8_t exp_pwm_get_duty_cycle(pwm_id_t pwm_id)
     return 0xFF;
 }
 
+uint8_t exp_pwm_num_servos(void)
+{
+  return exp_board_num_servos;
+}
+
 /* DAC interface */
 uint8_t exp_dac_start(void)
 {
diff --git a/examples/dexter/dexter_robot b/examples/dexter/dexter_robot
new file mode 160000
index 0000000000000000000000000000000000000000..ef1cfb9a00e25730991a7e1f9a27f7eb5aed48fd
--- /dev/null
+++ b/examples/dexter/dexter_robot
@@ -0,0 +1 @@
+Subproject commit ef1cfb9a00e25730991a7e1f9a27f7eb5aed48fd
diff --git a/examples/get_up/Makefile b/examples/get_up/Makefile
index 0a5a21b65ccf72645bf2f034f5ef30fdd1be8ecd..3dcbe475cca9d01699e00e22dd92ee27906fcac5 100644
--- a/examples/get_up/Makefile
+++ b/examples/get_up/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
 
 INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/examples/movements/Makefile b/examples/movements/Makefile
index 6d668f0a8c55d56e746b3cdaa141a2087125f436..84f98f106644726863bcbd568b0d45bd91e566d5 100644
--- a/examples/movements/Makefile
+++ b/examples/movements/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
 
 INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/examples/pan_tilt/Makefile b/examples/pan_tilt/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..f3d952011102b9a70676a0a29afff2f7be73f10c
--- /dev/null
+++ b/examples/pan_tilt/Makefile
@@ -0,0 +1,57 @@
+PROJECT=pan_tilt
+########################################################
+# afegir tots els fitxers que s'han de compilar aquí
+########################################################
+SOURCES=pan_tilt.c 
+
+OBJS=$(SOURCES:.c=.o)
+SRC_DIR=./
+DEV_DIR=../../dyn_devices/
+COMM_DIR=../../communications/
+CONT_DIR=../../controllers/
+MAN_DIR=../../motion/
+CC=avr-gcc
+OBJCOPY=avr-objcopy
+MMCU=atmega2561
+
+LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
+
+INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include -I../movements
+
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -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 controllers motion_manager $(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 $@ $<
+
+communications:
+	$(MAKE) -C $(COMM_DIR)
+
+dyn_devices:
+	$(MAKE) -C $(DEV_DIR)
+
+controllers:
+	$(MAKE) -C $(CONT_DIR)
+
+motion_manager:
+	$(MAKE) -C $(MAN_DIR)
+
+download: $(MAIN_OUT_HEX)
+	fw_downloader -d /dev/ttyUSB0 -f ./$(PROJECT).hex -p cm510
+
+clean:
+	-rm $(PROJECT).map
+	-rm $(PROJECT).hex
+	-rm $(PROJECT).elf
+	-rm $(OBJS)
diff --git a/examples/pan_tilt/pan_tilt.c b/examples/pan_tilt/pan_tilt.c
new file mode 100644
index 0000000000000000000000000000000000000000..8445616d9b66924257b7193322990fce3010ea42
--- /dev/null
+++ b/examples/pan_tilt/pan_tilt.c
@@ -0,0 +1,88 @@
+#include <util/delay.h>
+#include "cm510.h"
+#include "balance.h"
+#include "exp_board.h"
+#include "pan_tilt.h"
+
+typedef enum {wait_start,wait_cmd,wait_pan_left,wait_pan_right,wait_tilt_up,wait_tilt_down} main_states;
+
+#define PAN_SERVO_ID    19
+#define TILT_SERVO_ID   20
+
+void user_init(void)
+{
+  serial_console_init(57600);
+  balance_init();
+  balance_calibrate_gyro();
+  balance_enable_gyro();
+  user_time_set_period(100);
+  pan_tilt_init(PAN_SERVO_ID,TILT_SERVO_ID);
+}
+
+void user_loop(void)
+{
+  static main_states state=wait_start;
+
+  if(user_time_is_period_done())
+  {
+    switch(state)
+    {
+      case wait_start: if(is_button_rising_edge(BTN_START))
+                       {
+                         action_set_page(31);
+                         action_start_page();
+                         state=wait_cmd;
+                       }
+                       else
+                         state=wait_start;
+                       cm510_printf("wait_start %d\n",(int)state);
+                       break;
+      case wait_cmd: if(is_button_rising_edge(BTN_LEFT))
+                     {
+                       pan_set_speed(10);
+                       pan_move_angle(70);
+                       state=wait_pan_left;
+                     }
+                     else if(is_button_rising_edge(BTN_RIGHT))
+                     {
+                       pan_set_speed(200);
+                       pan_move_angle(-70);
+                       state=wait_pan_right;
+                     } 
+                     else if(is_button_rising_edge(BTN_UP))
+                     {
+                       tilt_set_speed(10);
+                       tilt_move_angle(70);
+                       state=wait_tilt_up;
+                     } 
+                     else if(is_button_rising_edge(BTN_DOWN))
+                     {
+                       tilt_set_speed(200);
+                       tilt_move_angle(-70);
+                       state=wait_tilt_down;
+                     } 
+                     break;
+      case wait_pan_left: if(pan_is_moving())
+                            state=wait_pan_left;
+                          else
+                            state=wait_cmd;
+                          break; 
+      case wait_pan_right: if(pan_is_moving())
+                             state=wait_pan_right;
+                           else
+                             state=wait_cmd;
+                           break;
+      case wait_tilt_up: if(tilt_is_moving())
+                           state=wait_tilt_up;
+                         else
+                           state=wait_cmd;
+                         break;
+      case wait_tilt_down: if(tilt_is_moving())
+                             state=wait_tilt_down;
+                           else
+                             state=wait_cmd;
+                           break;
+    }
+  }
+}
+
diff --git a/examples/sensors/Makefile b/examples/sensors/Makefile
index df7669117ab8fe61c5732222dbd76914b8693f31..dc31ef4e263a2e5a7ced250c329d8f49f463acb6 100644
--- a/examples/sensors/Makefile
+++ b/examples/sensors/Makefile
@@ -14,11 +14,11 @@ CC=avr-gcc
 OBJCOPY=avr-objcopy
 MMCU=atmega2561
 
-LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
+LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(DEV_DIR)lib/libdyn_devices.a $(COMM_DIR)lib/libcomm.a
 
 INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/examples/sensors/sensors.c b/examples/sensors/sensors.c
index a983364fd8a22a685263c61eff9c65af62814bba..44011f34c0fe42aa02d6208a3a58e2acf960ed15 100644
--- a/examples/sensors/sensors.c
+++ b/examples/sensors/sensors.c
@@ -37,9 +37,9 @@ void user_loop(void)
                      else
                        state=read_sensors;
                      break;
-    case read_sensors: printf("CM510 ADC port 1: %d\n",get_adc_avg_channel(ADC_PORT_2));
-                       printf("Exp. Board compass: %d\n",exp_compass_get_avg_heading());
-                       printf("Exp. Board ADC port 7: %d\n",exp_adc_get_avg_channel(ADC7));
+    case read_sensors: cm510_printf("CM510 ADC port 1: %d\n",get_adc_avg_channel(ADC_PORT_2));
+                       cm510_printf("Exp. Board compass: %d\n",exp_compass_get_avg_heading());
+                       cm510_printf("Exp. Board ADC port 7: %d\n",exp_adc_get_avg_channel(ADC7));
                        break;
   }
 }
diff --git a/examples/stairs/Makefile b/examples/stairs/Makefile
index 746c9e6374b09e2557d4e4a07341418e7b4620b4..09812e87e450c8a46963b900c89f66dffa151365 100644
--- a/examples/stairs/Makefile
+++ b/examples/stairs/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
 
 INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include -I../movements
 
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/examples/vision/Makefile b/examples/vision/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..2181ffc707c9b67b403be35a16c3073cc88c523b
--- /dev/null
+++ b/examples/vision/Makefile
@@ -0,0 +1,63 @@
+# computer code
+INPUT_FILE  = vision.cpp
+OUTPUT_FILE = vision
+LIBS        = -lpthread -L/usr/lib -lopencv_calib3d -lopencv_contrib -lopencv_core -lopencv_features2d -lopencv_flann -lopencv_gpu -lopencv_highgui -lopencv_imgproc -lopencv_legacy -lopencv_ml -lopencv_objdetect -lopencv_ocl -lopencv_photo -lopencv_stitching -lopencv_superres -lopencv_ts -lopencv_video -lopencv_videostab -L/usr/local/lib/iridrivers -ldetectqrcode -lcomm -liriutils
+INCLUDES    = -I. -I/usr/include/opencv -I/usr/local/include/iridrivers
+
+# microcontroller code
+CM510_PROJECT = cm510_vision
+CM510_SOURCES = cm510_vision.c
+
+CM510_OBJS=$(CM510_SOURCES:.c=.o)
+CM510_SRC_DIR=./
+CM510_DEV_DIR=../../dyn_devices/
+CM510_COMM_DIR=../../communications/
+CM510_CONT_DIR=../../controllers/
+CM510_MAN_DIR=../../motion/
+CC=avr-gcc
+OBJCOPY=avr-objcopy
+MMCU=atmega2561
+
+CM510_LIBS=$(CM510_MAN_DIR)lib/libmotion_manager.a $(CM510_CONT_DIR)lib/libcontrollers.a $(CM510_COMM_DIR)lib/libcomm.a $(CM510_DEV_DIR)lib/libdyn_devices.a
+
+CM510_INCLUDE_DIRS=-I$(CM510_DEV_DIR)include -I$(CM510_COMM_DIR)include -I$(CM510_CONT_DIR)include -I$(CM510_MAN_DIR)include -I../movements
+
+CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+
+LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(CM510_PROJECT).map -DF_CPU=16000000UL
+
+HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
+
+all: communications dyn_devices controllers motion_manager $(CM510_PROJECT).hex
+	$(CXX) $(INPUT_FILE) $(LIBS) $(INCLUDES) -o $(OUTPUT_FILE)
+
+$(CM510_PROJECT).hex: $(CM510_PROJECT).elf
+	$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS)  $< $@
+$(CM510_PROJECT).elf: $(CM510_OBJS)
+	$(CC) $(LDFLAGS) $(CM510_OBJS) $(CM510_LIBS) -o $(CM510_PROJECT).elf
+%.o:%.c
+	$(CC) -c $(CFLAGS) $(CM510_INCLUDE_DIRS) -o $@ $<
+
+communications:
+	$(MAKE) -C $(CM510_COMM_DIR)
+
+dyn_devices:
+	$(MAKE) -C $(CM510_DEV_DIR)
+
+controllers:
+	$(MAKE) -C $(CM510_CONT_DIR)
+
+motion_manager:
+	$(MAKE) -C $(CM510_MAN_DIR)
+
+download: $(MAIN_OUT_HEX)
+	fw_downloader -d /dev/ttyUSB0 -f ./$(CM510_PROJECT).hex -p cm510
+
+clean:
+	-rm $(CM510_PROJECT).map
+	-rm $(CM510_PROJECT).hex
+	-rm $(CM510_PROJECT).elf
+	-rm $(CM510_OBJS)
+	$(RM) $(OUTPUT_FILE)
+
+.PHONY: all clean
diff --git a/examples/vision/cm510_vision.c b/examples/vision/cm510_vision.c
new file mode 100644
index 0000000000000000000000000000000000000000..9c86ccaede8ce9d3f43b552e4cd3a008ee55b3b5
--- /dev/null
+++ b/examples/vision/cm510_vision.c
@@ -0,0 +1,77 @@
+#include <util/delay.h>
+#include "cm510.h"
+#include "balance.h"
+#include "exp_board.h"
+#include "pan_tilt.h"
+#include <stdio.h>
+
+typedef enum {wait_start,wait_cmd,wait_pan_left,wait_pan_right} main_states;
+
+#define PAN_SERVO_ID    19
+#define TILT_SERVO_ID   20
+
+void user_init(void)
+{
+  serial_console_init(57600);
+  balance_init();
+  balance_calibrate_gyro();
+  balance_enable_gyro();
+  user_time_set_period(100);
+  pan_tilt_init(PAN_SERVO_ID,TILT_SERVO_ID);
+}
+
+void user_loop(void)
+{
+  static main_states state=wait_start;
+  uint8_t cmd;
+
+  if(user_time_is_period_done())
+  {
+    switch(state)
+    {
+      case wait_start: if(is_button_rising_edge(BTN_START))
+                       {
+                         action_set_page(31);
+                         action_start_page();
+                         state=wait_cmd;
+                       }
+                       else
+                         state=wait_start;
+                       break;
+      case wait_cmd: if(serial_console_get_num_data()>0)
+                     {
+                       printf("New data received\n");
+                       cm510_scanf("%c",&cmd);
+                       switch(cmd)
+                       {
+                         case 'l': pan_set_speed(200);
+                                   pan_move_angle(-70);
+                                   state=wait_pan_right;
+                                   printf("move left\n");
+                                   break;
+                         case 'r': pan_set_speed(200);
+                                   pan_move_angle(70);
+                                   state=wait_pan_left;
+                                   printf("move right\n");
+                                   break;
+                         default: state=wait_cmd;
+                                  break;
+                       }
+                     }
+                     else
+                       state=wait_cmd;
+                     break;
+      case wait_pan_left: if(pan_is_moving())
+                            state=wait_pan_left;
+                          else
+                            state=wait_cmd;
+                          break;
+      case wait_pan_right: if(pan_is_moving())
+                             state=wait_pan_right;
+                           else
+                             state=wait_cmd;
+                           break;
+    }
+  }
+}
+
diff --git a/examples/vision/vision.cpp b/examples/vision/vision.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dbf2bab9bfc44637499ed790b62c4b5a8617a52a
--- /dev/null
+++ b/examples/vision/vision.cpp
@@ -0,0 +1,78 @@
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <iostream>
+#include "detectqrcode.h"
+#include "rs232.h"
+#include "exceptions.h"
+
+using namespace cv;
+using namespace std;
+
+int main() 
+{
+  cv::Mat gray;
+  cv::Mat frame;
+  CDetectqrcode detector;
+  std::vector<std::vector<cv::Point> > squares;
+  CRS232 serial_console("bioloid_serial_port");
+  std::string serial_console_device="/dev/pts/27";
+  TRS232_config serial_console_config;
+  std::string cmd;
+
+  cv::VideoCapture cap(0);   //0 is the id of video device.0 if you have only one camera.
+
+  if (!cap.isOpened()) 
+  { //check if video device has been initialised
+    cout << "cannot open camera";
+    return 0;
+  }
+
+  detector.init(721.977446f,717.128980f,334.800583f,242.691968f,0.0475f,0.0475f);
+
+  try{
+    serial_console_config.baud=57600;
+    serial_console_config.num_bits=8;
+    serial_console_config.parity=none;
+    serial_console_config.stop_bits=1;
+    serial_console.open((void *)&serial_console_device);
+    serial_console.config(&serial_console_config);
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+    return 0;
+  }
+
+  cv::namedWindow("QRcode",1);
+  //unconditional loop
+
+  for(unsigned int i=0;i<2;i++) 
+  { 
+    std::cout << "move left" << std::endl;
+    cmd="l\n";
+    serial_console.write((unsigned char *)cmd.c_str(),2);
+    sleep(2);
+    std::cout << "move right" << std::endl;
+    cmd="r\n";
+    serial_console.write((unsigned char *)cmd.c_str(),2);
+    sleep(2);
+  }
+
+  while (true) 
+  {
+    cap.read(frame);
+    std::vector<TQRInfo> tags;
+    cv::cvtColor(frame,gray,CV_BGR2GRAY);
+    detector.findSquares(gray, squares);
+    detector.findQR(gray,tags);
+    for(unsigned int i=0;i<tags.size();i++)
+    {
+      std::cout << "Tag ID: " << tags[i].tag_id << std::endl;
+      std::cout << "Tag position: X:" << tags[i].tvec[0] << " Y: " << tags[i].tvec[1] << " Z: " << tags[i].tvec[2] << std::endl;
+    }
+    detector.drawSquares(frame, squares);
+    cv::imshow("QRcode",frame);
+    if (waitKey(30) >= 0)
+      break;
+  }
+
+  return 0;
+}
diff --git a/examples/walk_straight/Makefile b/examples/walk_straight/Makefile
index 428ca8178e98dac817e2a54b24e1f7dca2a31ea1..bf76cb5971018c69cdb33aa6581510f1c6c19447 100644
--- a/examples/walk_straight/Makefile
+++ b/examples/walk_straight/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
 
 INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/motion/Makefile b/motion/Makefile
deleted file mode 100755
index 9071ac87ceff3a579600b258670f0f076252dfd6..0000000000000000000000000000000000000000
--- a/motion/Makefile
+++ /dev/null
@@ -1,58 +0,0 @@
-PROJECT=libmotion_manager
-SOURCES=src/motion_manager.c src/action.c src/motion_pages.c src/balance.c src/mtn_library.c
-OBJS=$(SOURCES:.c=.o)
-SRC_DIR=./src/ 
-BIN_DIR=./build/
-LIB_DIR=./lib/
-COMM_DIR=../communications/
-DEV_DIR=../dyn_devices/
-CONT_DIR=../controllers/
-CC=avr-gcc
-OBJCOPY=avr-ar
-MMCU=atmega2561
-
-INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/ -I$(CONT_DIR)include/
-
-LIBS=$(CONT_DIR)lib/libcontrollers.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
-
-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 $(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}
-	mkdir -p lib
-	$(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS)
-
-communications:
-	$(MAKE) -C $(COMM_DIR)
-
-dyn_devices:
-	$(MAKE) -C $(DEV_DIR)
-
-examples:
-	$(MAKE) -C src/examples
-
-%.o: %.c 
-	$(CC) -c $(CFLAGS) ${INC_DIRS} -o $@ $<
-
-clean:
-	rm -f ${LIB_DIR}$(PROJECT).a
-	rm -f $(OBJS) 
-	$(MAKE) -C src/examples clean
diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h
index 399c1cfc097bb9e6204769b0af55e9dc6930495a..0440e64c64301544b029bb5b779867790189d5ce 100644
--- a/motion/include/motion_cfg.h
+++ b/motion/include/motion_cfg.h
@@ -4,7 +4,7 @@
 #include "buzzer.h"
 
 #ifndef MANAGER_MAX_NUM_SERVOS
-  #define MANAGER_MAX_NUM_SERVOS                  18
+  #define MANAGER_MAX_NUM_SERVOS                  32
 #endif
 
 #ifndef MANAGER_MISSING_SERVOS_ALARM_NOTE
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 3a61d63312d0bbb7b252442d7897c74ef981ed19..5f4e43172e70a64fa0995dd77ce72dc0cd17c454 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -7,15 +7,19 @@ extern "C" {
 
 #include <inttypes.h>
 
+typedef enum {MM_NONE=0,MM_ACTION=1,MM_JOINTS=2} mm_module_t;
+
 // servo information structure
 typedef struct{
-  uint8_t id;
+  uint16_t angle;
   uint16_t max_value;
   uint16_t min_value;
   uint16_t center_value;
   uint16_t current_value;
-  uint8_t cc_slope;
-  uint8_t ccw_slope;
+  uint16_t cw_limit;
+  uint16_t ccw_limit;
+  uint8_t slope;
+  mm_module_t module;
 }TServoInfo;
 
 // public functions
diff --git a/motion/include/pan_tilt.h b/motion/include/pan_tilt.h
new file mode 100644
index 0000000000000000000000000000000000000000..8623da4d24d35ca6a290daa144e9e6465cbb492c
--- /dev/null
+++ b/motion/include/pan_tilt.h
@@ -0,0 +1,28 @@
+#ifndef PAN_TILT_H
+#define PAN_TILT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <avr/io.h>
+
+// public functions
+void pan_tilt_init(uint8_t pan_id,uint8_t tilt_id);
+void pan_tilt_move_angles(int8_t pan_angle,uint8_t titl_angle);
+void pan_move_angle(int8_t pan_angle);
+void pan_set_speed(uint8_t pan_speed);
+void pan_stop(void);
+uint8_t pan_is_moving(void);
+void tilt_move_angle(int8_t tilt_angle);
+void tilt_set_speed(uint8_t tilt_speed);
+void tilt_stop(void);
+uint8_t tilt_is_moving(void);
+
+void pan_tilt_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/motion/src/action.c b/motion/src/action.c
index 622be3d8ea9f194856ddc391ac87e66a164d8288..6029311220afbc1930d940bfbbb1ad8b71d54a0e 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -4,6 +4,7 @@
 #include "motion_cfg.h"
 #include "action.h"
 #include "motion_pages.h"
+#include "motion_manager.h"
 #include "serial_console.h"
 
 /**************************************
@@ -21,12 +22,9 @@ 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);
+// external variables
 extern uint8_t manager_num_servos;
+extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
 
 // private variables
 uint8_t action_finished;
@@ -61,7 +59,6 @@ uint8_t action_is_running;
 // public functions
 void action_init(void)
 {
-  //ram_write_byte(CM730_ACTION_STATUS,0x00);
   action_finished=0x00;
   action_stop=0x00;
   action_next_index=0xFF;
@@ -93,12 +90,15 @@ void action_start_page(void)
   bPlayRepeatCount = action_current_page.header.repeat;
   action_next_index = 0x00;
 
-  for( i=1; i<=manager_num_servos; i++ )
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    wpTargetAngle1024[i] = manager_get_servo_value(i);
-    ipLastOutSpeed1024[i] = 0;
-    ipMovingAngle1024[i] = 0;
-    ipGoalSpeed1024[i] = 0;
+    if(manager_servos[i].module==MM_ACTION)
+    {
+      wpTargetAngle1024[i] = manager_servos[i].current_value;
+      ipLastOutSpeed1024[i] = 0;
+      ipMovingAngle1024[i] = 0;
+      ipGoalSpeed1024[i] = 0;
+    }
   }
   action_is_running=0x01;
 }
@@ -142,10 +142,13 @@ void action_process(void)
   {
     wUnitTimeCount = 0;
 
-    for(bID=1;bID<=manager_num_servos;bID++)
+    for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
     {
-      wpStartAngle1024[bID] = manager_get_servo_value(bID);
-      ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
+      if(manager_servos[bID].module==MM_ACTION)
+      {
+        wpStartAngle1024[bID] = manager_servos[bID].current_value;
+        ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
+      }
     }
 
     // Section ¾÷µ¥ÀÌÆ® ( PRE -> MAIN -> POST -> (PAUSE or PRE) ... )
@@ -154,17 +157,20 @@ void action_process(void)
       // MAIN Section Áغñ
       bSection = MAIN_SECTION;
       wUnitTimeNum =  wUnitTimeTotalNum - (wAccelStep << 1);
-      for(bID=1;bID<=manager_num_servos;bID++)
+      for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
       {
-        if( bpFinishType[bID] == NONE_ZERO_FINISH )
+        if(manager_servos[bID].module==MM_ACTION)
         {
-          if( (wUnitTimeTotalNum - wAccelStep) == 0 ) // µî¼Ó ±¸°£ÀÌ ÀüÇô ¾ø´Ù¸é
-            ipMainAngle1024[bID] = 0;
-          else
-            ipMainAngle1024[bID] = (short)((((long)(ipMovingAngle1024[bID] - ipAccelAngle1024[bID])) * wUnitTimeNum) / (wUnitTimeTotalNum - wAccelStep));
+          if( bpFinishType[bID] == NONE_ZERO_FINISH )
+          {
+            if( (wUnitTimeTotalNum - wAccelStep) == 0 ) // µî¼Ó ±¸°£ÀÌ ÀüÇô ¾ø´Ù¸é
+              ipMainAngle1024[bID] = 0;
+            else
+              ipMainAngle1024[bID] = (short)((((long)(ipMovingAngle1024[bID] - ipAccelAngle1024[bID])) * wUnitTimeNum) / (wUnitTimeTotalNum - wAccelStep));
+          }
+          else // ZERO_FINISH
+            ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipAccelAngle1024[bID] - (short int)((((long)ipMainSpeed1024[bID] * wAccelStep * 12) / 5) >> 8);
         }
-        else // ZERO_FINISH
-          ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipAccelAngle1024[bID] - (short int)((((long)ipMainSpeed1024[bID] * wAccelStep * 12) / 5) >> 8);
       }
     }
     else if( bSection == MAIN_SECTION )
@@ -173,10 +179,9 @@ void action_process(void)
       bSection = POST_SECTION;
       wUnitTimeNum = wAccelStep;
 
-      for(bID=1;bID<=manager_num_servos;bID++)
-      {
-        ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
-      }
+      for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
+        if(manager_servos[bID].module==MM_ACTION)
+          ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
     }
     else if( bSection == POST_SECTION )
     {
@@ -196,10 +201,9 @@ void action_process(void)
       // PRE Section Áغñ
       bSection = PRE_SECTION;
 
-      for(bID=1;bID<=manager_num_servos;bID++)
-      {
-        ipLastOutSpeed1024[bID] = 0;
-      }
+      for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
+        if(manager_servos[bID].module==MM_ACTION)
+          ipLastOutSpeed1024[bID] = 0;
     }
 
     // PRE Section½Ã¿¡ ¸ðµç Áغñ¸¦ ÇÑ´Ù.
@@ -261,8 +265,10 @@ void action_process(void)
         //  wMaxAngle1024 = 0;
 
       ////////// Jointº° ÆĶó¹ÌÅÍ °è»ê
-      for(bID=1;bID<=manager_num_servos;bID++)
+      for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
       {
+        if(manager_servos[bID].module==MM_ACTION)
+        {
           // ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÁÀ¸·Î ±ËÀûÀ» °è»ê
           ipAccelAngle1024[bID] = 0;
 
@@ -322,6 +328,7 @@ void action_process(void)
           {
             bpFinishType[bID] = NONE_ZERO_FINISH;
           }
+        }
       }
       //½Ã°£À» °è»êÇؼ­ ´Ù½Ã 7.8msec·Î ³ª´©´Ù(<<7)-±×½Ã°£µ¿¾È¿¡ 7.8msec°¡ ¸î°³µé¾î°¡´ÂÁö °è»êÇÑ °Í
       //´ÜÀ§ º¯È¯µÚ¿¡ °¢/¼Óµµ¸¦ ±¸ÇÏ°í(½Ã°£)±× ½Ã°£¿¡ ´Ù½Ã 7.8msec°¡ ¸î°³µé¾î°¡ÀÖ´ÂÁö °è»ê
@@ -354,8 +361,10 @@ void action_process(void)
 
       if(lDivider2 == 0)
         lDivider2 = 1;
-      for(bID=1;bID<=manager_num_servos;bID++)
+      for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
       {
+        if(manager_servos[bID].module==MM_ACTION)
+        {
           lStartSpeed1024_PreTime_256T = (long)ipLastOutSpeed1024[bID] * ulPreSectionTime256T; //  *300/1024 * 1024/720 * 256 * 2
           lMovingAngle_Speed1024Scale_256T_2T = (((long)ipMovingAngle1024[bID]) * 2560L) / 12;
 
@@ -368,6 +377,7 @@ void action_process(void)
             ipMainSpeed1024[bID] = 1023;
           if( ipMainSpeed1024[bID] < -1023 )
             ipMainSpeed1024[bID] = -1023;
+        }
       }
       wUnitTimeNum = wAccelStep; //PreSection
     }
@@ -380,53 +390,55 @@ void action_process(void)
     }
     else
     {
-      for(bID=1;bID<=manager_num_servos;bID++)
+      for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
       {
-        if( ipMovingAngle1024[bID] == 0 )
-          manager_set_servo_value(bID,wpStartAngle1024[bID]);
-        else
+        if(manager_servos[bID].module==MM_ACTION)
         {
-          if( bSection == PRE_SECTION )
-          {
-            iSpeedN = (short)(((long)(ipMainSpeed1024[bID] - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
-            ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
-            ipAccelAngle1024[bID] =  (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN >> 1)) * wUnitTimeCount * 144) / 15) >> 9);
-            manager_set_servo_value(bID,wpStartAngle1024[bID] + ipAccelAngle1024[bID]);
-          }
-          else if( bSection == MAIN_SECTION )
-          {
-            manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID])*wUnitTimeCount) / wUnitTimeNum));
-            ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
-          }
-          else // POST_SECTION
+          if( ipMovingAngle1024[bID] == 0 )
+            manager_servos[bID].current_value=wpStartAngle1024[bID];
+          else
           {
-            if( wUnitTimeCount == (wUnitTimeNum-1) )
+            if( bSection == PRE_SECTION )
             {
-              // ½ºÅÜ ¸¶Áö¸· ¿ÀÂ÷¸¦ ÁÙÀ̱âÀ§ÇØ ±×³É ¸ñÇ¥ À§Ä¡ °ªÀ» »ç¿ë
-              manager_set_servo_value(bID,wpTargetAngle1024[bID]);
+              iSpeedN = (short)(((long)(ipMainSpeed1024[bID] - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
+              ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
+              ipAccelAngle1024[bID] =  (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN >> 1)) * wUnitTimeCount * 144) / 15) >> 9);
+              manager_servos[bID].current_value=wpStartAngle1024[bID] + ipAccelAngle1024[bID];
             }
-            else
+            else if( bSection == MAIN_SECTION )
+            {
+              manager_servos[bID].current_value=wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID])*wUnitTimeCount) / wUnitTimeNum);
+              ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
+            }
+            else // POST_SECTION
             {
-              if( bpFinishType[bID] == ZERO_FINISH )
+              if( wUnitTimeCount == (wUnitTimeNum-1) )
               {
-                iSpeedN = (short int)(((long)(0 - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
-                ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
-                manager_set_servo_value(bID,wpStartAngle1024[bID] +  (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN>>1)) * wUnitTimeCount * 144) / 15) >> 9));
+                // ½ºÅÜ ¸¶Áö¸· ¿ÀÂ÷¸¦ ÁÙÀ̱âÀ§ÇØ ±×³É ¸ñÇ¥ À§Ä¡ °ªÀ» »ç¿ë
+                manager_servos[bID].current_value=wpTargetAngle1024[bID];
               }
-              else // NONE_ZERO_FINISH
+              else
               {
-                // MAIN Section°ú µ¿ÀÏÇÏ°Ô ÀÛµ¿-µ¿ÀÏ
-                // step¿¡¼­ ¾î¶²¼­º¸´Â °¡°í ¾î¶² ¼­º¸´Â ¼­¾ßÇÏ´Â »óȲÀÌ ¹ß»ýÇÒ ¼ö ÀÖÀ¸¹Ç·Î ÀÌ·¸°Ô ÇÒ ¼ö¹Û¿¡ ¾øÀ½
-                manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID]) * wUnitTimeCount) / wUnitTimeNum));
-                ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
+                if( bpFinishType[bID] == ZERO_FINISH )
+                {
+                  iSpeedN = (short int)(((long)(0 - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
+                  ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
+                  manager_servos[bID].current_value=wpStartAngle1024[bID] +  (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN>>1)) * wUnitTimeCount * 144) / 15) >> 9);
+                }
+                else // NONE_ZERO_FINISH
+                {
+                  // MAIN Section°ú µ¿ÀÏÇÏ°Ô ÀÛµ¿-µ¿ÀÏ
+                  // step¿¡¼­ ¾î¶²¼­º¸´Â °¡°í ¾î¶² ¼­º¸´Â ¼­¾ßÇÏ´Â »óȲÀÌ ¹ß»ýÇÒ ¼ö ÀÖÀ¸¹Ç·Î ÀÌ·¸°Ô ÇÒ ¼ö¹Û¿¡ ¾øÀ½
+                  manager_servos[bID].current_value=wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
+                  ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
+                }
               }
             }
           }
+          //set slopes
+          manager_servos[bID].slope=action_current_page.header.slope[bID];
         }
-        //set slopes
-        manager_set_servo_slopes(bID, action_current_page.header.slope[bID]);
       }
     }
   }
-
 }
diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile
index b773bdda297cbb8fd4c54af7f4652636a4ec696a..d88b37124f3c8d0d9cdc6a5d45618bf5cfdc2f7b 100644
--- a/motion/src/examples/Makefile
+++ b/motion/src/examples/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
 
 INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_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
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
 
 LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
 
diff --git a/motion/src/examples/main.c b/motion/src/examples/main.c
index fc603d7ccea8ec41e33a4b040ec3e2c12d754f47..eeab8f887cd049197119a8607ee6fc3ded923918 100644
--- a/motion/src/examples/main.c
+++ b/motion/src/examples/main.c
@@ -13,22 +13,22 @@ void user_loop(void)
   {
     if(is_action_running()==0x00)
     {
-      printf("Walk ready\n");
+      cm510_printf("Walk ready\n");
       if(action_set_page(25)==0)
         action_start_page();
       else
-        printf("Error loading page\n");
+        cm510_printf("Error loading page\n");
     }
   }
   else if(is_button_falling_edge(BTN_RIGHT))
   {
     if(is_action_running()==0x00)
     {
-      printf("start walking\n");
+      cm510_printf("start walking\n");
       if(action_set_page(26)==0)
         action_start_page();
       else
-        printf("Error loading page\n");
+        cm510_printf("Error loading page\n");
     }
   }
   else
@@ -37,7 +37,7 @@ void user_loop(void)
     {
       if(is_action_running())
       {
-        printf("stop action\n");
+        cm510_printf("stop action\n");
         action_stop_page();
       }
     }
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index ea71d59629ea9cefa857d5db77a607826f5be3dc..53084cb906f797585bd28fe9863443c096252cac 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -12,6 +12,7 @@
 #include "balance.h"
 #include "buzzer.h"
 #include "exp_board.h"
+#include "pan_tilt.h"
 
 // external functions
 extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
@@ -30,7 +31,7 @@ extern TActionPage action_current_page;
 
 typedef struct
 {
-  uint8_t cc_slope;
+  uint8_t cw_slope;
   uint8_t ccw_slope;
   uint16_t position;
 }dyn_send_data;
@@ -77,93 +78,74 @@ uint8_t manager_period_done(void)
 void manager_send_motion_command(void) 
 {
   uint8_t *pdata;
-  uint8_t i;
+  uint8_t i,num=0;
   uint16_t target_pos;
   int16_t *offsets;
 
   pdata = (uint8_t *)data;
   
   balance_get_all_offsets(&offsets);
-  for(i=0;i<manager_num_servos;i++) 
+  for(i=0;i<MANAGER_MAX_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;
-
-    if(manager_servos[i].cc_slope == 0) 
-      pdata[i*4] = 32;
-    else 
-      pdata[i*4] = 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);
-
-    packets[i].data_addr=(uint8_t *)&data[i];
+    if(manager_servos[i].module!=MM_NONE)
+    {
+      target_pos = manager_servos[i].current_value + offsets[i];
+      pdata[num*4+2] = target_pos&0xFF;
+      pdata[num*4+3] = (target_pos>>8)&0xFF;
+
+      if(manager_servos[i].slope == 0) 
+      {
+        pdata[num*4] = 32;
+        pdata[num*4+1] = 32;
+      }
+      else 
+      {
+        pdata[num*4] = 1<<(0x0F&manager_servos[i].slope);
+        pdata[num*4+1] = 1<<(0x0F&manager_servos[i].slope);
+      }
+
+      packets[num].data_addr=(uint8_t *)&data[num];
+      num++;
+    }
   }
-  if(manager_num_servos>0)
+  if(num>0)
     dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets); 
 }
 
-void manager_get_current_position(void)
+void manager_set_servo_slope(uint8_t servo_id, uint8_t slope)
 {
-  uint8_t i;
-
-  for(i=0;i<manager_num_servos;i++)
-    manager_servos[i].current_value=get_current_position(manager_servos[i].id);
+  manager_servos[servo_id].slope=slope;
 }
 
-void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope)
-{
-  uint8_t i;
- 
-  for(i=0;i<manager_num_servos;i++)
-  {
-    if(manager_servos[i].id==servo_id)
-    {
-      manager_servos[i].cc_slope=slope;
-      manager_servos[i].ccw_slope=slope;
-      break;
-    }  
-  }
+uint8_t manager_get_servo_slope(uint8_t servo_id)
+{ 
+  return manager_servos[servo_id].slope;
 }
 
-void manager_set_index_value(uint8_t servo_id, uint16_t value)
+void manager_set_servo_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)
+uint16_t manager_get_servo_value(uint8_t servo_id)
 {
-  uint8_t i;
- 
-  for(i=0;i<manager_num_servos;i++)
-  {
-    if(manager_servos[i].id==servo_id)
-    {
-      manager_set_index_value(i,value);
-      break;
-    }  
-  }
+  return manager_servos[servo_id].current_value;
 }
 
-uint16_t manager_get_index_value(uint8_t servo_id)
+mm_module_t manager_get_servo_module(uint8_t servo_id)
 {
-  return manager_servos[servo_id].current_value;
+  return manager_servos[servo_id].module;
 }
 
-uint16_t manager_get_servo_value(uint8_t servo_id)
+uint8_t manager_get_cw_servo_limit(uint8_t servo_id)
 {
-  uint8_t i;
-
-  for(i=0;i<manager_num_servos;i++)
-    if(manager_servos[i].id==servo_id)
-      return manager_get_index_value(i);
-
-  return 2048;
+  return manager_servos[servo_id].cw_limit;
 }
 
+uint8_t manager_get_ccw_servo_limit(uint8_t servo_id)
+{
+  return manager_servos[servo_id].ccw_limit;
+}
 void manager_loop(void)
 {
   if(manager_period_done()==0x01)
@@ -176,6 +158,8 @@ void manager_loop(void)
     action_process(); //action_ready
     // balance the robot
     balance_loop();
+    // update the pan&tilt angles
+    pan_tilt_process();
     // send the motion commands to the servos
     manager_send_motion_command();
   }
@@ -184,31 +168,60 @@ void manager_loop(void)
 // public functions
 uint8_t manager_init(uint8_t num_servos)
 {
-  uint8_t i,num=0;
+  uint8_t i,num=0,id;
   uint16_t model;
-  uint8_t servos[PAGE_MAX_NUM_SERVOS];
 
   // enable power to the servos
-  dyn_master_init();
+  if(dyn_master_is_init()==0x00)
+    dyn_master_init();
   _delay_ms(500);
 
+  /* initialize by default the information for all servos */
+  for(i=0;i<num;i++)
+  {
+    manager_servos[i].angle=0;
+    manager_servos[i].max_value=0;
+    manager_servos[i].min_value=0;
+    manager_servos[i].center_value=0;
+    manager_servos[i].current_value=0;
+    manager_servos[i].module=MM_NONE;
+    manager_servos[i].slope=32;
+    manager_servos[i].cw_limit=0;
+    manager_servos[i].ccw_limit=0;
+  }
+
   /* scan the bus for all available servos */
-  dyn_master_scan(&num,servos);
+  dyn_master_scan(&num,servo_ids);
 
   manager_num_servos=0;
   for(i=0;i<num;i++)
   {
-    model=get_model_number(servos[i]);
-    if(model==0x000C || model==0x012C)
+    id=servo_ids[i];
+    model=get_model_number(id);
+    if(model==0x000C || model==0x012C)// standard AX-12 or AX12W
+    {
+      enable_servo(id);
+      manager_servos[id].angle=300;
+      manager_servos[id].max_value=1023;
+      manager_servos[id].min_value=0;
+      manager_servos[id].center_value=512;
+      manager_servos[id].current_value=get_current_position(id);
+      get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
+      manager_servos[id].module=MM_ACTION;
+      manager_servos[id].slope=32;
+      manager_num_servos++;
+    }
+    else if(model==0x0C00)// Emmulated servos from the expansion board pan&tilt servos
     {
-      //set_target_speed(i+1,0);
-      enable_servo(servos[i]);
-      servo_ids[manager_num_servos]=servos[i];
-      manager_servos[manager_num_servos].id=servos[i];
-      manager_servos[manager_num_servos].max_value=1023;
-      manager_servos[manager_num_servos].min_value=0;
-      manager_servos[manager_num_servos].center_value=512;
-      manager_servos[manager_num_servos].current_value=get_current_position(servos[i]);
+      enable_servo(id);
+      manager_servos[id].angle=300;
+      manager_servos[id].max_value=1023;
+      manager_servos[id].min_value=0;
+      manager_servos[id].center_value=512;
+      manager_servos[id].current_value=get_current_position(id);
+      get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
+      manager_servos[id].module=MM_JOINTS;
+      manager_servos[id].slope=32;
       manager_num_servos++;
     }
   } 
diff --git a/motion/src/pan_tilt.c b/motion/src/pan_tilt.c
new file mode 100644
index 0000000000000000000000000000000000000000..66f1ab55340aaec90197d597d212c5e1403d2075
--- /dev/null
+++ b/motion/src/pan_tilt.c
@@ -0,0 +1,209 @@
+#include "pan_tilt.h"
+#include "motion_cfg.h"
+#include "motion_manager.h"
+
+// external variables
+extern uint8_t manager_num_servos;
+extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
+
+// private variables
+// pan variables
+uint32_t pan_target_angle;
+uint32_t pan_target_speed;
+uint32_t pan_current_angle;
+uint8_t pan_stoping;
+uint8_t pan_moving;
+uint8_t pan_servo_id;
+// tilt variables
+uint32_t tilt_target_angle;
+uint32_t tilt_target_speed;
+uint32_t tilt_current_angle;
+uint8_t tilt_stoping;
+uint8_t tilt_moving;
+uint8_t tilt_servo_id;
+
+// public functions
+void pan_tilt_init(uint8_t pan_id,uint8_t tilt_id)
+{
+  // set serov ID's
+  pan_servo_id=pan_id;
+  tilt_servo_id=tilt_id;
+  /* initialize pan variables */
+  pan_target_angle=manager_servos[pan_servo_id].current_value<<7;
+  pan_target_speed=0;
+  pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+  pan_stoping=0x00;
+  pan_moving=0x00;
+  /* initialize tilt variables */
+  tilt_target_angle=manager_servos[tilt_servo_id].current_value<<7;
+  tilt_target_speed=0;
+  tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
+  tilt_stoping=0x00;
+  tilt_moving=0x00;
+}
+
+void pan_tilt_move_angles(int8_t pan_angle,uint8_t tilt_angle)
+{
+  pan_move_angle(pan_angle);
+  tilt_move_angle(tilt_angle);
+}
+
+void pan_move_angle(int8_t pan_angle)
+{
+  if(pan_servo_id!=0xFF)
+  {
+    if(pan_target_speed==0)
+    {
+      pan_target_angle=manager_servos[pan_servo_id].current_value<<7;
+      pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+      pan_moving=0x00;
+    }
+    else
+    {
+      pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+      pan_target_angle=((uint32_t)(((int32_t)pan_angle*(int32_t)manager_servos[pan_servo_id].max_value)/((int32_t)manager_servos[pan_servo_id].angle))+(uint32_t)manager_servos[pan_servo_id].center_value);
+      if(pan_target_angle>manager_servos[pan_servo_id].ccw_limit)
+        pan_target_angle=manager_servos[pan_servo_id].ccw_limit;
+      else if(pan_target_angle<manager_servos[pan_servo_id].cw_limit)
+        pan_target_angle=manager_servos[pan_servo_id].cw_limit;
+      pan_target_angle=pan_target_angle<<7;
+      pan_moving=0x01;
+    }
+  }
+}
+
+void pan_set_speed(uint8_t pan_speed)
+{
+  pan_target_speed=(((uint32_t)pan_speed*(uint32_t)manager_servos[pan_servo_id].max_value)/((uint32_t)manager_servos[pan_servo_id].angle));
+}
+
+void pan_stop(void)
+{
+  pan_stoping=0x01;
+}
+
+uint8_t pan_is_moving(void)
+{
+  return pan_moving;
+}
+
+void tilt_move_angle(int8_t tilt_angle)
+{
+  if(tilt_servo_id!=0xFF)
+  {
+    if(tilt_target_speed==0)
+    {
+      tilt_target_angle=manager_servos[tilt_servo_id].current_value<<7;
+      tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
+      tilt_moving=0x00;
+    }
+    else
+    {
+      tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
+      tilt_target_angle=(((int32_t)tilt_angle*(int32_t)manager_servos[tilt_servo_id].max_value)/((int32_t)manager_servos[tilt_servo_id].angle))+manager_servos[tilt_servo_id].center_value;
+      if(tilt_target_angle>manager_servos[tilt_servo_id].ccw_limit)
+        tilt_target_angle=manager_servos[tilt_servo_id].ccw_limit;
+      else if(tilt_target_angle<manager_servos[tilt_servo_id].cw_limit)
+        tilt_target_angle=manager_servos[tilt_servo_id].cw_limit;
+      tilt_target_angle=tilt_target_angle<<7;
+      tilt_moving=0x01;
+    }
+  }
+}
+
+void tilt_set_speed(uint8_t tilt_speed)
+{
+  tilt_target_speed=(((uint32_t)tilt_speed*(uint32_t)manager_servos[tilt_servo_id].max_value)/((uint32_t)manager_servos[tilt_servo_id].angle));
+}
+
+void tilt_stop(void)
+{
+  tilt_stoping=0x01;
+}
+
+uint8_t tilt_is_moving(void)
+{
+  return tilt_moving;
+}
+
+void pan_tilt_process(void)
+{
+  // pan control
+  if(pan_stoping==0x01)
+  {
+    pan_target_angle=manager_servos[pan_servo_id].current_value<<7;
+    pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+    pan_target_speed=0;
+    pan_moving=0x00;
+    pan_stoping=0x00;
+  }
+  else
+  {
+    if(pan_current_angle<pan_target_angle)
+    {
+      pan_current_angle+=pan_target_speed;
+      if(pan_current_angle>=pan_target_angle)
+      {
+        pan_current_angle=pan_target_angle;
+        pan_moving=0x00;
+      }
+    }
+    else if(pan_current_angle>pan_target_angle)
+    {
+      if(pan_current_angle<pan_target_speed)
+      {
+        pan_current_angle=pan_target_angle;
+        pan_moving=0x00;
+      }
+      else
+      {
+        pan_current_angle-=pan_target_speed;
+        if(pan_current_angle<=pan_target_angle)
+        {
+          pan_current_angle=pan_target_angle;
+         pan_moving=0x00;
+        }
+      }
+    }
+  }
+  manager_servos[pan_servo_id].current_value=(pan_current_angle>>7);
+  // tilt control
+  if(tilt_stoping==0x01)
+  {
+    tilt_target_angle=manager_servos[tilt_servo_id].current_value;
+    tilt_current_angle=manager_servos[tilt_servo_id].current_value;
+    tilt_target_speed=0;
+    tilt_moving=0x00;
+    tilt_stoping=0x00;
+  }
+  else
+  {
+    if(tilt_current_angle<tilt_target_angle)
+    {
+      tilt_current_angle+=tilt_target_speed;
+      if(tilt_current_angle>=tilt_target_angle)
+      {
+        tilt_current_angle=tilt_target_angle;
+        tilt_moving=0x00;
+      }
+    }
+    else if(tilt_current_angle>tilt_target_angle)
+    {
+      if(tilt_current_angle<tilt_target_speed)
+      { 
+        tilt_current_angle=tilt_target_angle;
+        tilt_moving=0x00;
+      }  
+      else
+      {
+        tilt_current_angle-=tilt_target_speed;
+        if(tilt_current_angle<=tilt_target_angle)
+        {
+          tilt_current_angle=tilt_target_angle;
+          tilt_moving=0x00;
+        }  
+      }
+    }  
+  }
+  manager_servos[tilt_servo_id].current_value=(tilt_current_angle>>7);
+}