Added initial supprt for the motion manager module.

Changed the way to detect which module is addressed by the dynamixel slave interface.

Makefile

@@ -15,6 +15,8 @@ TARGET_FILES+=src/bioloid_time.c
 TARGET_FILES+=src/bioloid_dyn_slave.c
 TARGET_FILES+=src/bioloid_dyn_master_sensors.c
 TARGET_FILES+=src/bioloid_dyn_master_servos.c
+TARGET_FILES+=src/motion_pages.c
+TARGET_FILES+=src/motion_manager.c
 TARGET_PROCESSOR=STM32F407VG
 
 HAL_PATH=../../STM32_processor/hal/f4

include/adc_dma.h

@@ -13,6 +13,7 @@ void adc_start(void);
 void adc_set_period(uint8_t period_ms);
 void adc_stop(void);
 // operation functions
+uint8_t adc_in_range(unsigned short int address,unsigned short int length);
 void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 
 #ifdef __cplusplus

include/bioloid_registers.h

@@ -11,6 +11,7 @@ extern "C" {
 #define BAUDRATE_OFFSET          ((unsigned short int)0x0004)
 #define RETURN_DELAY_OFFSET      ((unsigned short int)0x0005)
 #define RETURN_LEVEL_OFFSET      ((unsigned short int)0x0010)
+#define MM_PERIOD_OFFSET         ((unsigned short int)0x0010)
 
 #define LAST_EEPROM_OFFSET       ((unsigned short int)0x0020)
 
@@ -21,6 +22,8 @@ typedef enum {
   BIOLOID_ID                      = DEVICE_ID_OFFSET,
   BIOLOID_BAUD_RATE               = BAUDRATE_OFFSET,
   BIOLOID_RETURN_DELAY_TIME       = RETURN_DELAY_OFFSET,
+  BIOLOID_MM_PERIOD_L             = MM_PERIOD_OFFSET,
+  BIOLOID_MM_PERIOD_H             = MM_PERIOD_OFFSET+1,
   BIOLOID_RETURN_LEVEL            = RETURN_LEVEL_OFFSET,
   BIOLOID_USER1_LED_CNTRL         = 0x20, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2  | bit 1 |      bit 0
                                           //       |       |       |       | blink | toggle | value | internally used
@@ -65,12 +68,116 @@ typedef enum {
   BIOLOID_ADC_VREF_H              = 0x3F,
   BIOLOID_ADC_CH8_L               = 0x40,
   BIOLOID_ADC_CH8_H               = 0x41,
-  BIOLOID_ZIGBEE_CNTRL            = 0x42,
+  BIOLOID_ZIGBEE_CNTRL            = 0x42, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 |  bit 1  |    bit 0
+                                          //       |       |       |       |       |       | Enable  | Enable power
   BIOLOID_ZIGBEE_BAUDRATE         = 0x43,
   BIOLOID_ZIGBEE_OWN_ID_L         = 0x44,
   BIOLOID_ZIGBEE_OWN_ID_H         = 0x45,
   BIOLOID_ZIGBEE_REM_ID_L         = 0x46,
   BIOLOID_ZIGBEE_REM_ID_H         = 0x47,
+  BIOLOID_MM_NUM_SERVOS           = 0x48,
+  BIOLOID_MM_CNTRL                = 0x49, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 |     bit 2    |      bit 1      |    bit 0
+                                          //       |       |       |       |       | Enable power | Enable balance  | Enable manager
+  BIOLOID_MM_MODULE_EN0           = 0x4A, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0i
+                                          // Enable servo 0  |   assigned module     | Enable servo 1  |   assigned module
+                                          //                 |     000 -> none       |
+                                          //                 |     001 -> action     |
+                                          //                 |     010 -> walk       |
+                                          //                 |     011 -> joints     |
+  BIOLOID_MM_MODULE_EN1           = 0x4B, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 2  |   assigned module     | Enable servo 3  |   assigned module
+  BIOLOID_MM_MODULE_EN2           = 0x4C, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 4  |   assigned module     | Enable servo 5  |   assigned module
+  BIOLOID_MM_MODULE_EN3           = 0x4D, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 6  |   assigned module     | Enable servo 7  |   assigned module
+  BIOLOID_MM_MODULE_EN4           = 0x4E, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 8  |   assigned module     | Enable servo 9  |   assigned module
+  BIOLOID_MM_MODULE_EN5           = 0x4F, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 10 |   assigned module     | Enable servo 11 |   assigned module
+  BIOLOID_MM_MODULE_EN6           = 0x50, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 12 |   assigned module     | Enable servo 13 |   assigned module
+  BIOLOID_MM_MODULE_EN7           = 0x51, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 14 |   assigned module     | Enable servo 15 |   assigned module
+  BIOLOID_MM_MODULE_EN8           = 0x52, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 16 |   assigned module     | Enable servo 17 |   assigned module
+  BIOLOID_MM_MODULE_EN9           = 0x53, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 18 |   assigned module     | Enable servo 19 |   assigned module
+  BIOLOID_MM_MODULE_EN10          = 0x54, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 20 |   assigned module     | Enable servo 21 |   assigned module
+  BIOLOID_MM_MODULE_EN11          = 0x55, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 22 |   assigned module     | Enable servo 23 |   assigned module
+  BIOLOID_MM_MODULE_EN12          = 0x56, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 24 |   assigned module     | Enable servo 25 |   assigned module
+  BIOLOID_MM_MODULE_EN13          = 0x57, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 26 |   assigned module     | Enable servo 27 |   assigned module
+  BIOLOID_MM_MODULE_EN14          = 0x58, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 28 |   assigned module     | Enable servo 29 |   assigned module
+  BIOLOID_MM_MODULE_EN15          = 0x59, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0
+                                          // Enable servo 30 |   assigned module     | Enable servo 31 |   assigned module
+  BIOLOID_MM_SERVO0_CUR_POS_L     = 0x5A,
+  BIOLOID_MM_SERVO0_CUR_POS_H     = 0x5B,
+  BIOLOID_MM_SERVO1_CUR_POS_L     = 0x5C,
+  BIOLOID_MM_SERVO1_CUR_POS_H     = 0x5D,
+  BIOLOID_MM_SERVO2_CUR_POS_L     = 0x5E,
+  BIOLOID_MM_SERVO2_CUR_POS_H     = 0x5F,
+  BIOLOID_MM_SERVO3_CUR_POS_L     = 0x60,
+  BIOLOID_MM_SERVO3_CUR_POS_H     = 0x61,
+  BIOLOID_MM_SERVO4_CUR_POS_L     = 0x62,
+  BIOLOID_MM_SERVO4_CUR_POS_H     = 0x63,
+  BIOLOID_MM_SERVO5_CUR_POS_L     = 0x64,
+  BIOLOID_MM_SERVO5_CUR_POS_H     = 0x65,
+  BIOLOID_MM_SERVO6_CUR_POS_L     = 0x66,
+  BIOLOID_MM_SERVO6_CUR_POS_H     = 0x67,
+  BIOLOID_MM_SERVO7_CUR_POS_L     = 0x68,
+  BIOLOID_MM_SERVO7_CUR_POS_H     = 0x69,
+  BIOLOID_MM_SERVO8_CUR_POS_L     = 0x6A,
+  BIOLOID_MM_SERVO8_CUR_POS_H     = 0x6B,
+  BIOLOID_MM_SERVO9_CUR_POS_L     = 0x6C,
+  BIOLOID_MM_SERVO9_CUR_POS_H     = 0x6D,
+  BIOLOID_MM_SERVO10_CUR_POS_L    = 0x6E,
+  BIOLOID_MM_SERVO10_CUR_POS_H    = 0x6F,
+  BIOLOID_MM_SERVO11_CUR_POS_L    = 0x70,
+  BIOLOID_MM_SERVO11_CUR_POS_H    = 0x71,
+  BIOLOID_MM_SERVO12_CUR_POS_L    = 0x72,
+  BIOLOID_MM_SERVO12_CUR_POS_H    = 0x73,
+  BIOLOID_MM_SERVO13_CUR_POS_L    = 0x74,
+  BIOLOID_MM_SERVO13_CUR_POS_H    = 0x75,
+  BIOLOID_MM_SERVO14_CUR_POS_L    = 0x76,
+  BIOLOID_MM_SERVO14_CUR_POS_H    = 0x77,
+  BIOLOID_MM_SERVO15_CUR_POS_L    = 0x78,
+  BIOLOID_MM_SERVO15_CUR_POS_H    = 0x79,
+  BIOLOID_MM_SERVO16_CUR_POS_L    = 0x7A,
+  BIOLOID_MM_SERVO16_CUR_POS_H    = 0x7B,
+  BIOLOID_MM_SERVO17_CUR_POS_L    = 0x7C,
+  BIOLOID_MM_SERVO17_CUR_POS_H    = 0x7D,
+  BIOLOID_MM_SERVO18_CUR_POS_L    = 0x7E,
+  BIOLOID_MM_SERVO18_CUR_POS_H    = 0x7F,
+  BIOLOID_MM_SERVO19_CUR_POS_L    = 0x80,
+  BIOLOID_MM_SERVO19_CUR_POS_H    = 0x81,
+  BIOLOID_MM_SERVO20_CUR_POS_L    = 0x82,
+  BIOLOID_MM_SERVO20_CUR_POS_H    = 0x83,
+  BIOLOID_MM_SERVO21_CUR_POS_L    = 0x84,
+  BIOLOID_MM_SERVO21_CUR_POS_H    = 0x85,
+  BIOLOID_MM_SERVO22_CUR_POS_L    = 0x86,
+  BIOLOID_MM_SERVO22_CUR_POS_H    = 0x87,
+  BIOLOID_MM_SERVO23_CUR_POS_L    = 0x88,
+  BIOLOID_MM_SERVO23_CUR_POS_H    = 0x89,
+  BIOLOID_MM_SERVO24_CUR_POS_L    = 0x8A,
+  BIOLOID_MM_SERVO24_CUR_POS_H    = 0x8B,
+  BIOLOID_MM_SERVO25_CUR_POS_L    = 0x8C,
+  BIOLOID_MM_SERVO25_CUR_POS_H    = 0x8D,
+  BIOLOID_MM_SERVO26_CUR_POS_L    = 0x8E,
+  BIOLOID_MM_SERVO26_CUR_POS_H    = 0x8F,
+  BIOLOID_MM_SERVO27_CUR_POS_L    = 0x90,
+  BIOLOID_MM_SERVO27_CUR_POS_H    = 0x91,
+  BIOLOID_MM_SERVO28_CUR_POS_L    = 0x92,
+  BIOLOID_MM_SERVO28_CUR_POS_H    = 0x93,
+  BIOLOID_MM_SERVO29_CUR_POS_L    = 0x94,
+  BIOLOID_MM_SERVO29_CUR_POS_H    = 0x95,
+  BIOLOID_MM_SERVO30_CUR_POS_L    = 0x96,
+  BIOLOID_MM_SERVO30_CUR_POS_H    = 0x97,
+  BIOLOID_MM_SERVO31_CUR_POS_L    = 0x98,
+  BIOLOID_MM_SERVO31_CUR_POS_H    = 0x99
 
 } bioloid_registers;
 
@@ -89,6 +196,9 @@ typedef enum {
 #define      ZIGBEE_BASE_ADDRESS  0x42
 #define      ZIGBEE_MEM_LENGTH    6
 
+#define      MANAGER_BASE_ADDRESS 0x42
+#define      MANAGER_MEM_LENGTH   6
+
 #ifdef __cplusplus
 }
 #endif

include/gpio.h

@@ -21,6 +21,7 @@ void gpio_blink_led(led_t led_id, int16_t period_ms);
 uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id);
 void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void));
 // operation functions
+uint8_t gpio_in_range(unsigned short int address,unsigned short int length);
 void gpio_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 

include/motion_manager.h

@@ -5,9 +5,9 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
 
-#define         MANAGER_MAX_NUM_SERVOS          31
+#define MANAGER_MAX_NUM_SERVOS          31
 
 typedef enum{MM_NONE          = 0,
              MM_ACTION        = 1,
@@ -49,10 +49,12 @@ inline void manager_set_module(uint8_t servo_id,TModules module);
 inline TModules manager_get_module(uint8_t servo_id);
 inline void manager_enable_servo(uint8_t servo_id);
 inline void manager_disable_servo(uint8_t servo_id);
-void manager_disable_servos(void);
 inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
 inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
 inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
+// operation functions
+uint8_t manager_in_range(unsigned short int address, unsigned short int length);
+void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 
 #ifdef __cplusplus
 }

include/motion_pages.h

@@ -5,7 +5,7 @@
 extern "C" {
 #endif
 
-#include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
 
 #define MAX_PAGES                     256 
 #define PAGE_MAX_NUM_SERVOS           31

include/zigbee.h

@@ -14,6 +14,7 @@ void zigbee_disable_power(void);
 void zigbee_enable(void);
 void zigbee_disable(void);
 // operation functions
+uint8_t zigbee_in_range(unsigned short int address,unsigned short int length);
 void zigbee_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 
 #ifdef __cplusplus

src/adc_dma.c

@@ -368,6 +368,12 @@ void adc_stop(void)
   HAL_TIM_OC_Stop_IT(&ADC_TIM_Handle, TIM_CHANNEL_4);
 }
 
+// operation functions
+uint8_t adc_in_range(unsigned short int address,unsigned short int length)
+{
+  return ram_in_window(ADC_BASE_ADDRESS,ADC_MEM_LENGTH,address,length);
+}
+
 void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
 {
   if(ram_in_range(BIOLOID_ADC_CNTRL,address,length))

src/bioloid_dyn_master_sensors.c

@@ -75,4 +75,8 @@ void bioloid_dyn_master_sensors_init(void)
   dyn_master_init(&bioloid_dyn_master_sensors,&bioloid_dyn_master_sensors_comm);
   bioloid_dyn_master_sensors.set_rx_mode=bioloid_dyn_master_sensors_set_rx_mode;
   bioloid_dyn_master_sensors.set_tx_mode=bioloid_dyn_master_sensors_set_tx_mode;
+
+  /* configure dynamixel master module */
+  dyn_master_set_rx_timeout(&bioloid_dyn_master_sensors,50);
+  dyn_master_set_return_level(&bioloid_dyn_master_sensors,return_all);
 }

src/bioloid_dyn_master_servos.c

@@ -87,14 +87,9 @@ void bioloid_dyn_master_servos_init(void)
   bioloid_dyn_master_servos.set_rx_mode=bioloid_dyn_master_servos_set_rx_mode;
   bioloid_dyn_master_servos.set_tx_mode=bioloid_dyn_master_servos_set_tx_mode;
 
-  bioloid_dyn_master_servos_enable_power();
-  HAL_Delay(2000);
-  if(dyn_master_ping(&bioloid_dyn_master_servos,0x04)==DYN_SUCCESS)
-    gpio_set_led(RXD_LED);
-  else
-    gpio_clear_led(RXD_LED);
-  bioloid_dyn_master_servos_disable_power();
-
+  /* configure dynamixel master module */
+  dyn_master_set_rx_timeout(&bioloid_dyn_master_servos,50);
+  dyn_master_set_return_level(&bioloid_dyn_master_servos,return_all);
 }
 
 inline void bioloid_dyn_master_servos_enable_power(void)

src/bioloid_dyn_slave.c

@@ -6,6 +6,7 @@
 #include "gpio.h"
 #include "adc_dma.h"
 #include "zigbee.h"
+#include "motion_manager.h"
 
 /* external interrupt pin */
 #define     DYN_SLAVE_EXT_INT_PIN             GPIO_PIN_8
@@ -66,14 +67,17 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
     ram_write_byte(BIOLOID_RETURN_LEVEL,data[BIOLOID_RETURN_LEVEL-address]);
   }
   // GPIO commands
-  if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length))
+  if(gpio_in_range(address,length))
     gpio_process_write_cmd(address,length,data);
   // ADC commands
-  if(ram_in_window(ADC_BASE_ADDRESS,ADC_MEM_LENGTH,address,length))
+  if(adc_in_range(address,length))
     adc_process_write_cmd(address,length,data);
   // Zigbee commands
-  if(ram_in_window(ZIGBEE_BASE_ADDRESS,ZIGBEE_MEM_LENGTH,address,length))
+  if(zigbee_in_range(address,length))
     zigbee_process_write_cmd(address,length,data);
+  // Manager commands
+  if(manager_in_range(address,length))
+    manager_process_write_cmd(address,length,data);
   // write eeprom
   for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
     EE_WriteVariable(i,data[j]);
@@ -143,7 +147,7 @@ void bioloid_dyn_slave_init(void)
   DYN_SLAVE_TIMER_ENABLE_CLK;
   bioloid_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
   bioloid_dyn_slave_tim_handle.Init.Period = 1000;
-  bioloid_dyn_slave_tim_handle.Init.Prescaler = 84;
+  bioloid_dyn_slave_tim_handle.Init.Prescaler = 42;
   bioloid_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   bioloid_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
   HAL_TIM_Base_Init(&bioloid_dyn_slave_tim_handle);

src/bioloid_stm32.c

@@ -7,10 +7,12 @@
 #include "bioloid_time.h"
 #include "bioloid_dyn_slave.h"
 #include "bioloid_dyn_master_sensors.h"
-#include "bioloid_dyn_master_servos.h"
+#include "motion_manager.h"
 
 int32_t main(void)
 {
+  uint16_t eeprom_data,period;
+
   HAL_Init();
   /* initialize the gpio */
   gpio_init();
@@ -29,8 +31,12 @@ int32_t main(void)
   bioloid_dyn_slave_start();
   /* initialize the dynamixel master module for the sensors */
   bioloid_dyn_master_sensors_init();
-  /* initialize the dynamixel master module for the servos */
-  bioloid_dyn_master_servos_init();
+  /* initialize motion manager module */
+  EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
+  period=eeprom_data&0x00FF;
+  EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
+  period+=((eeprom_data&0x00FF)<<8);
+  manager_init(period);
 
   while(1);
 }

src/eeprom.c

@@ -51,6 +51,7 @@
 #define    DEFAULT_DEVICE_ID         0x0002
 #define    DEFAULT_BAUDRATE          0x0001
 #define    DEFAULT_RETURN_DELAY      0x0000
+#define    DEFAULT_MM_PERIOD         0x01FF
 #define    DEFAULT_RETURN_LEVEL      0x0002
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
@@ -73,6 +74,10 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
                                                               BAUDRATE_OFFSET,
                                                               DEFAULT_RETURN_DELAY,// return delay time
                                                               RETURN_DELAY_OFFSET,
+                                                              DEFAULT_MM_PERIOD&0xFF,
+                                                              MM_PERIOD_OFFSET,
+                                                              DEFAULT_MM_PERIOD>>8,
+                                                              MM_PERIOD_OFFSET+1,
                                                               DEFAULT_RETURN_LEVEL,
                                                               RETURN_LEVEL_OFFSET};// return level
 

src/gpio.c

@@ -522,6 +522,12 @@ void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
   }
 }
 
+// operation functions
+uint8_t gpio_in_range(unsigned short int address,unsigned short int length)
+{
+  return ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length);
+}
+
 void gpio_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
 {
   if(ram_in_range(BIOLOID_RESET_PB_CNTRL,address,length))

src/motion_manager.c

+#include "bioloid_dyn_master_servos.h"
 #include "motion_manager.h"
-#include "dynamixel_master_uart_dma.h"
 #include "dyn_servos.h"
 #include "ram.h"
-#include "action.h"
-#include "stm32_time.h"
 
-#define      MOTION_TIMER                 TIM3
-#define      MOTION_TIMER_IRQn            TIM3_IRQn
-#define      MOTION_TIMER_IRQHandler      TIM3_IRQHandler
-#define      MOTION_TIMER_CLK             RCC_APB1Periph_TIM3
+#define MANAGER_TIMER                   TIM3
+#define ENABLE_MANAGER_TIMER_CLK        __HAL_RCC_TIM3_CLK_ENABLE()
+#define MANAGER_TIMER_IRQn              TIM3_IRQn
+#define MANAGER_TIMER_IRQHandler        TIM3_IRQHandler
 
 // private variables
-__IO uint16_t manager_motion_period;
+TIM_HandleTypeDef MANAGER_TIM_Handle;
+
+uint16_t manager_motion_period;
 uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
 // current angles used for all motion modules
-int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
 // balance values and configuration
-int64_t balance_offset[MANAGER_MAX_NUM_SERVOS];
-uint8_t balance_enabled;
-// package variables
-uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
-TWriteData write_data[MANAGER_MAX_NUM_SERVOS];
-uint8_t length[MANAGER_MAX_NUM_SERVOS];
-uint8_t address[MANAGER_MAX_NUM_SERVOS];
+int64_t manager_balance_offset[MANAGER_MAX_NUM_SERVOS];
+uint8_t manager_balance_enabled;
 
 // private functions
 void manager_send_motion_command(void)
 {
+  static uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+  static TWriteData write_data[MANAGER_MAX_NUM_SERVOS];
   uint8_t i,num=0;
 
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
@@ -40,7 +37,7 @@ void manager_send_motion_command(void)
     }
   }
   if(num>0)
-    dyn_master_sync_write(num,servo_ids,P_GOAL_POSITION_L,2,write_data);
+    dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,2,write_data);
 }
 
 inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
@@ -73,7 +70,7 @@ void manager_get_current_position(void)
   {
     if(!manager_servos[i].enabled)// servo is not enabled
     {
-      dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+      dyn_master_read_word(&bioloid_dyn_master_servos,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
       manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
     }
     ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
@@ -91,7 +88,7 @@ void manager_get_target_position(void)
     {
       if(manager_servos[i].module==MM_ACTION)
       {
-        manager_servos[i].current_angle=((current_angles[i]+balance_offset[i])>>9);
+        manager_servos[i].current_angle=((manager_current_angles[i]+manager_balance_offset[i])>>9);
         //>>16 from the action codification, <<7 from the manager codification
         manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
       }
@@ -104,50 +101,53 @@ void manager_balance(void)
 }
 
 // interrupt handlers
-void MOTION_TIMER_IRQHandler(void)
+void MANAGER_TIMER_IRQHandler(void)
 {
   uint16_t capture;
 
-  if(TIM_GetITStatus(MOTION_TIMER, TIM_IT_CC1)!=RESET)
+  if(__HAL_TIM_GET_FLAG(&MANAGER_TIM_Handle, TIM_FLAG_CC1) != RESET)
   {
-    TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
-    capture = TIM_GetCapture1(MOTION_TIMER);
-    TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
-    // call the action process
-    action_process();
-    // call the joint motion process
-    // joint_motion_process();
-    // call the walking process
-    // walking_process();
-    // get the target angles from all modules
-    manager_get_target_position();
-    // balance the robot
-    manager_balance();
-    // send the motion commands to the servos
-    manager_send_motion_command();
-    // get the disabled servos position
-//    manager_get_current_position();
+    if(__HAL_TIM_GET_IT_SOURCE(&MANAGER_TIM_Handle, TIM_IT_CC1) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&MANAGER_TIM_Handle, TIM_IT_CC1);
+      capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+      __HAL_TIM_SET_COMPARE(&MANAGER_TIM_Handle, TIM_CHANNEL_1, (capture + manager_motion_period));
+      // call the action process
+      // action_process();
+      // call the joint motion process
+      // joint_motion_process();
+      // call the walking process
+      // walking_process();
+      // get the target angles from all modules
+      // manager_get_target_position();
+      // balance the robot
+      // manager_balance();
+      // send the motion commands to the servos
+      // manager_send_motion_command();
+      // get the disabled servos position
+      // manager_get_current_position();
+    }
   }
 }
 
 // public functions
 void manager_init(uint16_t period_us)
 {
-  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-  TIM_OCInitTypeDef  TIM_OCInitStructure;
-  NVIC_InitTypeDef NVIC_InitStructure;
+  TIM_ClockConfigTypeDef sClockSourceConfig;
+  TIM_MasterConfigTypeDef sMasterConfig;
   uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
   uint16_t model,value;
   uint8_t i,num=0;
 
-  RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE);
+  /* initialize the dynamixel master module for the servos */
+  bioloid_dyn_master_servos_init();
 
   // enable power to the servos
-  dyn_master_enable_power();
-  delay_ms(1000);
+  bioloid_dyn_master_servos_enable_power();
+  HAL_Delay(1000);
 
   // detect the servos connected 
-  dyn_master_scan(&num,servo_ids); 
+  dyn_master_scan(&bioloid_dyn_master_servos,&num,servo_ids); 
   ram_data[BIOLOID_MM_NUM_SERVOS]=num;
   manager_num_servos=0;
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
@@ -155,7 +155,7 @@ void manager_init(uint16_t period_us)
     if(i==servo_ids[manager_num_servos])
     {
       // read the model of the i-th device
-      dyn_master_read_word(servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model);
+      dyn_master_read_word(&bioloid_dyn_master_servos,servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model);
       switch(model)
       {
         case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
@@ -225,17 +225,17 @@ void manager_init(uint16_t period_us)
       manager_servos[i].module=MM_NONE;
       manager_servos[i].enabled=0x00;
       // get the servo's current position
-      dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+      dyn_master_read_word(&bioloid_dyn_master_servos,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
       manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
       // read the servo limits
-      dyn_master_read_word(manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
+      dyn_master_read_word(&bioloid_dyn_master_servos,manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
       manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
-      dyn_master_read_word(manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
+      dyn_master_read_word(&bioloid_dyn_master_servos,manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
       manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
       // set the action current angles
-      current_angles[i]=manager_servos[i].current_angle<<9;
+      manager_current_angles[i]=manager_servos[i].current_angle<<9;
       manager_num_servos++;
     }
     else
@@ -255,39 +255,32 @@ void manager_init(uint16_t period_us)
       manager_servos[i].ccw_angle_limit=0;
     }
   }
-  dyn_master_disable_power();
-
-  // initialize the timer interrupts
-  NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-  NVIC_Init(&NVIC_InitStructure);
-
-  /* motion timer configuration */
-  TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
-  TIM_TimeBaseStructure.TIM_Prescaler = 42;
-  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-  TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure);
-
-  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
-  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
-  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+  bioloid_dyn_master_servos_disable_power();
+
+  /* configure timer */
+  ENABLE_MANAGER_TIMER_CLK;
+  MANAGER_TIM_Handle.Instance=MANAGER_TIMER;
+  MANAGER_TIM_Handle.Init.Period = 0xFFFF;
+  MANAGER_TIM_Handle.Init.Prescaler = 42;
+  MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+  HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 0);
+  HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
+  /* use the internal clock */
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
+  HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
+  /* disable master/slave mode */
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
+  /* configure ouptut counter channel 4 */
   manager_motion_period=(period_us*1000000)>>16;
-  TIM_OCInitStructure.TIM_Pulse = manager_motion_period;
-  TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure);
-  TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable);
-
-  ram_clear_bit(BIOLOID_MM_STATUS,0);
-
-  /* initialize motion modules */
-  action_init(period_us);
 
   /* initialize balance parameters */
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
-    balance_offset[i]=0;
-  balance_enabled=0x01;
+    manager_balance_offset[i]=0;
+  manager_balance_enabled=0x00;
 }
 
 uint16_t manager_get_period(void)
@@ -298,45 +291,45 @@ uint16_t manager_get_period(void)
 void manager_set_period(uint16_t period_us)
 {
   manager_motion_period=(period_us*1000000)>>16;
-  ram_write_word(BIOLOID_MM_PERIOD_H,period_us);
-  action_set_period(period_us);
+  ram_data[BIOLOID_MM_PERIOD_L]=period_us;
 }
 
 inline void manager_enable(void)
 {
+  TIM_OC_InitTypeDef TIM_OCInitStructure;
   uint16_t capture;
 
-  capture = TIM_GetCapture1(MOTION_TIMER);
-  TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
-  TIM_Cmd(MOTION_TIMER, ENABLE);
-  TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
-  ram_set_bit(BIOLOID_MM_STATUS,0);
+  TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+  TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+  TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+  capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+  TIM_OCInitStructure.Pulse = capture+manager_motion_period;
+  HAL_TIM_OC_ConfigChannel(&MANAGER_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
+  HAL_TIM_OC_Start_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+  ram_data[BIOLOID_MM_CNTRL]|=0x01;
 }
 
 inline void manager_disable(void)
 {
-  TIM_Cmd(MOTION_TIMER, DISABLE);
-  TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE);
-  ram_clear_bit(BIOLOID_MM_STATUS,0);
+  HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+  ram_data[BIOLOID_MM_CNTRL]&=0xFE;
 }
 
 inline uint8_t manager_is_enabled(void)
 {
-  return ram_data[BIOLOID_MM_STATUS]&0x01;
+  return ram_data[BIOLOID_MM_CNTRL]&0x01;
 }
 
 void manager_enable_balance(void)
 {
-  ram_set_bit(BIOLOID_MM_STATUS,1);
-  ram_set_bit(BIOLOID_MM_CONTROL,1);
-  balance_enabled=0x01;
+  manager_balance_enabled=0x01;
+  ram_data[BIOLOID_MM_CNTRL]|=0x02;
 }
 
 void manager_disable_balance(void)
 {
-  ram_clear_bit(BIOLOID_MM_STATUS,1);
-  ram_clear_bit(BIOLOID_MM_CONTROL,1);
-  balance_enabled=0x00;
+  manager_balance_enabled=0x00;
+  ram_data[BIOLOID_MM_CNTRL]&=0xFD;
 }
 
 inline uint8_t manager_get_num_servos(void)
@@ -351,12 +344,12 @@ void manager_set_module(uint8_t servo_id,TModules module)
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
   {
     manager_servos[servo_id].module=module;
-    ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+    byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];
     if(servo_id%2)
       byte=(byte&0xF8)+((uint8_t)module);
     else
       byte=(byte&0x8F)+(((uint8_t)module)<<4);
-    ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+    ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
   }
 }
 
@@ -375,12 +368,12 @@ inline void manager_enable_servo(uint8_t servo_id)
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
   {
     manager_servos[servo_id].enabled=0x01;
-    ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+    byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];;
     if(servo_id%2)
       byte|=0x08;
     else
       byte|=0x80;
-    ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+    ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
   }
 }
 
@@ -391,19 +384,15 @@ inline void manager_disable_servo(uint8_t servo_id)
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
   {
     manager_servos[servo_id].enabled=0x00;
-    ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+    byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];
     if(servo_id%2)
       byte&=0xF7;
     else
       byte&=0x7F;
-    ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+    ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
   }
 }
 
-void manager_disable_servos(void)
-{
-}
-
 inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
 {
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
@@ -422,3 +411,54 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
   return manager_servos[servo_id].ccw_angle_limit;
 }
 
+// operation functions
+uint8_t manager_in_range(unsigned short int address, unsigned short int length)
+{
+  if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
+     ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  uint16_t word_value;
+  uint8_t byte_value,module,i,j;
+
+  if(ram_in_range(BIOLOID_MM_PERIOD_L,address,length) && ram_in_range(BIOLOID_MM_PERIOD_H,address,length))
+  {
+    word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8);
+    manager_set_period(word_value);
+  }
+  for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
+  {
+    if(ram_in_range(i,address,length))
+    {
+      byte_value=data[i-address];
+      if(byte_value&0x80) manager_enable_servo(j);
+      else manager_disable_servo(j);
+      module=(byte_value&0x70)>>4;
+      manager_set_module(j,module);
+      if(byte_value&0x08) manager_enable_servo(j+1);
+      else manager_disable_servo(j+1);
+      module=byte_value&0x07;
+      manager_set_module(j+1,module);
+    }
+  }
+  if(ram_in_range(BIOLOID_MM_CNTRL,address,length))
+  {
+    if(data[BIOLOID_MM_CNTRL-address]&0x01)
+      manager_enable();
+    else
+      manager_disable();
+    if(data[BIOLOID_MM_CNTRL-address]&0x02)
+      manager_enable_balance();
+    else
+      manager_disable_balance();
+    if(data[BIOLOID_MM_CNTRL-address]&0x04)
+      bioloid_dyn_master_servos_enable_power();
+    else
+      bioloid_dyn_master_servos_disable_power();
+  }
+}

src/zigbee.c

@@ -47,6 +47,11 @@ void zigbee_disable(void)
 }
 
 // operation functions
+uint8_t zigbee_in_range(unsigned short int address,unsigned short int length)
+{
+  return ram_in_window(ZIGBEE_BASE_ADDRESS,ZIGBEE_MEM_LENGTH,address,length);  
+}
+
 void zigbee_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
 {
   if(ram_in_range(BIOLOID_ZIGBEE_CNTRL,address,length))