From b9f5a422e0d066b4a1a2368f6c14277af8f025a8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 15 Jul 2016 18:34:00 +0200
Subject: [PATCH] Changed the way the detected servos are stored in memory for
 performance issues. The Id coincides with the vector position. Added a new
 attribute to the servo info structure to hold the motion module associated to
 the servo (ACTION or JOINTS for now). Added support to detect and handle the
 simulated dynamixel servos. Removed the dynamixel master initialization from
 the motion manager. Added a module to implement a pan&tilt unit.

---
 communications/include/dynamixel_master.h |   1 +
 communications/src/dynamixel_master.c     |   7 ++
 controllers/src/cm510.c                   |   9 +-
 dyn_devices/include/exp_board.h           |   1 +
 dyn_devices/include/exp_board_reg.h       |   3 +-
 dyn_devices/src/exp_board/exp_board.c     |   9 ++
 examples/sensors/Makefile                 |   2 +-
 examples/sensors/sensors.c                |   4 +-
 motion/include/motion_manager.h           |   4 +-
 motion/include/pan_tilt.h                 |  13 ++
 motion/src/action.c                       | 137 ++++++++++++----------
 motion/src/motion_manager.c               | 136 +++++++++++----------
 motion/src/pan_tilt.c                     |  51 ++++++++
 13 files changed, 240 insertions(+), 137 deletions(-)
 create mode 100644 motion/include/pan_tilt.h
 create mode 100644 motion/src/pan_tilt.c

diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h
index c1f76df..01f3d94 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 08b286d..95865d9 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 5e8a96e..91769ec 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 9fae29e..9d6e8b3 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 d8e30c7..28da3da 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 7f18d59..5098889 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 df76691..28b3e0c 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 a983364..09e3d42 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 3a61d63..32f7ff0 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 0000000..8ec1986
--- /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 622be3d..25c3ef1 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 ea71d59..d9735b5 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 0000000..a79e2fe
--- /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)
+{
+  
+}
-- 
GitLab