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/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/controllers/src/cm510.c b/controllers/src/cm510.c
index 5e8a96e1cddbbe97876efc43b561972712a8931a..91769ecf9738f9afe7052993a456bb43cf3fd200 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,16 @@ void init_cm510(void)
 
 int16_t main(void)
 {
+  uint8_t num_servos;
+
   init_cm510();
   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/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/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index 7f18d59d7177c63dbb0accf8c5fc66ed31138f5f..5098889252e8bd11cf961db10a02c720031371f0 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];
 
@@ -90,6 +91,8 @@ uint8_t exp_board_init(uint8_t board_id)
       exp_board_uart_usb_present=0x00;
       return 0xFF;
     }
+    // read the number pf PWM servos
+    dyn_master_read_byte(exp_board_id,NUM_PWM_SERVOS,&exp_board_num_servos);
   }
   else
   {
@@ -99,6 +102,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 +430,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/sensors/Makefile b/examples/sensors/Makefile
index df7669117ab8fe61c5732222dbd76914b8693f31..28b3e0c24906717431c70ae444f406116244658b 100644
--- a/examples/sensors/Makefile
+++ b/examples/sensors/Makefile
@@ -14,7 +14,7 @@ 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
 
diff --git a/examples/sensors/sensors.c b/examples/sensors/sensors.c
index a983364fd8a22a685263c61eff9c65af62814bba..09e3d421dccda1589231feaf66187d98effccee8 100644
--- a/examples/sensors/sensors.c
+++ b/examples/sensors/sensors.c
@@ -21,7 +21,7 @@ void user_loop(void)
 {
   static main_states state=wait_start;
 
-  switch(state)
+/*  switch(state)
   {
     case wait_start: if(is_button_rising_edge(BTN_START))
                      {
@@ -41,5 +41,5 @@ void user_loop(void)
                        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));
                        break;
-  }
+  }*/
 }
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 3a61d63312d0bbb7b252442d7897c74ef981ed19..32f7ff0e5e63eca3db6345e9582f319a9c3ee906 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -7,15 +7,17 @@ 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 max_value;
   uint16_t min_value;
   uint16_t center_value;
   uint16_t current_value;
   uint8_t cc_slope;
   uint8_t ccw_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..8ec19866066cc131b7fb70969c80f801cd5c43b4
--- /dev/null
+++ b/motion/include/pan_tilt.h
@@ -0,0 +1,13 @@
+#ifndef PAN_TILT_H
+#define PAN_TILT_H
+
+// public functions
+void pan_tilt_init(void);
+void pan_tilt_set_angles(int8_t pan_angle,uint8_t titl_angle);
+void pan_set_angle(int8_t pan_angle);
+void pan_set_speed(uint8_t pan_speed);
+void tilt_set_angle(int8_t tilt_angle);
+void tilt_set_speed(int8_t tilt_speed);
+void pan_tilt_process(void);
+
+#endif
diff --git a/motion/src/action.c b/motion/src/action.c
index 622be3d8ea9f194856ddc391ac87e66a164d8288..25c3ef1b2516093e040a5d50baf0bb7425c6ee2d 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"
 
 /**************************************
@@ -22,11 +23,10 @@ typedef enum{ ZERO_FINISH, NONE_ZERO_FINISH} TStopMode;
 #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 mm_module_t manager_get_servo_module(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);
-extern uint8_t manager_num_servos;
 
 // private variables
 uint8_t action_finished;
@@ -93,12 +93,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=1;i<=MANAGER_MAX_NUM_SERVOS;i++)
   {
-    wpTargetAngle1024[i] = manager_get_servo_value(i);
-    ipLastOutSpeed1024[i] = 0;
-    ipMovingAngle1024[i] = 0;
-    ipGoalSpeed1024[i] = 0;
+    if(manager_get_servo_module(i)==MM_ACTION)
+    {
+      wpTargetAngle1024[i] = manager_get_servo_value(i);
+      ipLastOutSpeed1024[i] = 0;
+      ipMovingAngle1024[i] = 0;
+      ipGoalSpeed1024[i] = 0;
+    }
   }
   action_is_running=0x01;
 }
@@ -142,10 +145,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_get_servo_module(bID)==MM_ACTION)
+      {
+        wpStartAngle1024[bID] = manager_get_servo_value(bID);
+        ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
+      }
     }
 
     // Section ¾÷µ¥ÀÌÆ® ( PRE -> MAIN -> POST -> (PAUSE or PRE) ... )
@@ -154,17 +160,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_get_servo_module(bID)==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 +182,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_get_servo_module(bID)==MM_ACTION)
+          ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
     }
     else if( bSection == POST_SECTION )
     {
@@ -196,10 +204,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_get_servo_module(bID)==MM_ACTION)
+          ipLastOutSpeed1024[bID] = 0;
     }
 
     // PRE Section½Ã¿¡ ¸ðµç Áغñ¸¦ ÇÑ´Ù.
@@ -261,8 +268,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_get_servo_module(bID)==MM_ACTION)
+        {
           // ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÁÀ¸·Î ±ËÀûÀ» °è»ê
           ipAccelAngle1024[bID] = 0;
 
@@ -322,6 +331,7 @@ void action_process(void)
           {
             bpFinishType[bID] = NONE_ZERO_FINISH;
           }
+        }
       }
       //½Ã°£À» °è»êÇؼ­ ´Ù½Ã 7.8msec·Î ³ª´©´Ù(<<7)-±×½Ã°£µ¿¾È¿¡ 7.8msec°¡ ¸î°³µé¾î°¡´ÂÁö °è»êÇÑ °Í
       //´ÜÀ§ º¯È¯µÚ¿¡ °¢/¼Óµµ¸¦ ±¸ÇÏ°í(½Ã°£)±× ½Ã°£¿¡ ´Ù½Ã 7.8msec°¡ ¸î°³µé¾î°¡ÀÖ´ÂÁö °è»ê
@@ -354,8 +364,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_get_servo_module(bID)==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 +380,7 @@ void action_process(void)
             ipMainSpeed1024[bID] = 1023;
           if( ipMainSpeed1024[bID] < -1023 )
             ipMainSpeed1024[bID] = -1023;
+        }
       }
       wUnitTimeNum = wAccelStep; //PreSection
     }
@@ -380,53 +393,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_get_servo_module(bID)==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_set_servo_value(bID,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_set_servo_value(bID,wpStartAngle1024[bID] + ipAccelAngle1024[bID]);
             }
-            else
+            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( 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_set_servo_value(bID,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_set_servo_value(bID,wpStartAngle1024[bID] +  (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN>>1)) * wUnitTimeCount * 144) / 15) >> 9));
+                }
+                else // NONE_ZERO_FINISH
+                {
+                  // MAIN Section°ú µ¿ÀÏÇÏ°Ô ÀÛµ¿-µ¿ÀÏ
+                  // step¿¡¼­ ¾î¶²¼­º¸´Â °¡°í ¾î¶² ¼­º¸´Â ¼­¾ßÇÏ´Â »óȲÀÌ ¹ß»ýÇÒ ¼ö ÀÖÀ¸¹Ç·Î ÀÌ·¸°Ô ÇÒ ¼ö¹Û¿¡ ¾øÀ½
+                  manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID]) * wUnitTimeCount) / wUnitTimeNum));
+                  ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
+                }
               }
             }
           }
+          //set slopes
+          manager_set_servo_slopes(bID, action_current_page.header.slope[bID]);
         }
-        //set slopes
-        manager_set_servo_slopes(bID, action_current_page.header.slope[bID]);
       }
     }
   }
-
 }
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index ea71d59629ea9cefa857d5db77a607826f5be3dc..d9735b5d3258a112bac9f12c23f573c2a6d00554 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);
@@ -77,32 +78,36 @@ 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].cc_slope == 0) 
+        pdata[num*4] = 32;
+      else 
+        pdata[num*4] = 1<<(0x0F&manager_servos[i].cc_slope);
+
+      if(manager_servos[i].ccw_slope == 0)
+        pdata[num*4+1] = 32;
+      else 
+        pdata[num*4+1] = 1<<(0x0F&manager_servos[i].ccw_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); 
 }
 
@@ -110,58 +115,30 @@ 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(manager_servos[i].id);
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    if(manager_servos[i].module!=MM_NONE)
+      manager_servos[i].current_value=get_current_position(i);
 }
 
 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;
-    }  
-  }
-}
-
-void manager_set_index_value(uint8_t servo_id, uint16_t value)
-{
-  manager_servos[servo_id].current_value=value;
+  manager_servos[servo_id].cc_slope=slope;
+  manager_servos[servo_id].ccw_slope=slope;
 }
 
 void manager_set_servo_value(uint8_t servo_id, uint16_t value)
 {
-  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;
-    }  
-  }
+  manager_servos[servo_id].current_value=value;
 }
 
-uint16_t manager_get_index_value(uint8_t servo_id)
+uint16_t manager_get_servo_value(uint8_t servo_id)
 {
   return manager_servos[servo_id].current_value;
 }
 
-uint16_t manager_get_servo_value(uint8_t servo_id)
+mm_module_t manager_get_servo_module(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].module;
 }
 
 void manager_loop(void)
@@ -176,6 +153,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 +163,50 @@ 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].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;
+  }
+
   /* 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].max_value=1023;
+      manager_servos[id].min_value=0;
+      manager_servos[id].center_value=512;
+      manager_servos[id].current_value=get_current_position(id);
+      manager_servos[id].module=MM_ACTION;
+      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].max_value=1023;
+      manager_servos[id].min_value=0;
+      manager_servos[id].center_value=512;
+      manager_servos[id].current_value=get_current_position(id);
+      manager_servos[id].module=MM_JOINTS;
       manager_num_servos++;
     }
   } 
diff --git a/motion/src/pan_tilt.c b/motion/src/pan_tilt.c
new file mode 100644
index 0000000000000000000000000000000000000000..a79e2fe96732526234218130d5ef55c10de06ee1
--- /dev/null
+++ b/motion/src/pan_tilt.c
@@ -0,0 +1,51 @@
+#include "pan_tilt.h"
+
+// external functions
+extern uint16_t manager_get_servo_value(uint8_t servo_id);
+extern mm_module_t manager_get_servo_module(uint8_t servo_id);
+extern void manager_set_servo_value(uint8_t servo_id, uint16_t value);
+
+// private variables
+uint16_t pan_angle;
+uint16_t pan_speed;
+uint8_t pan_servo_id;
+uint16_t tilt_angle;
+uint16_t tilt_speed;
+uint8_t tilt_servo_id;
+
+// public functions
+void pan_tilt_init(uint8_t pan_id,uint8_t tilt_id)
+{
+  pan_servo_id=pan_id;
+  tilt_servo_id=tilt_id;
+}
+
+void pan_tilt_set_angles(int8_t pan_angle,uint8_t titl_angle)
+{
+
+}
+
+void pan_set_angle(int8_t pan_angle)
+{
+
+}
+
+void pan_set_speed(uint8_t pan_speed)
+{
+
+}
+
+void tilt_set_angle(int8_t tilt_angle)
+{
+
+}
+
+void tilt_set_speed(int8_t tilt_speed)
+{
+
+}
+
+void pan_tilt_process(void)
+{
+  
+}