Added non blocking functions in the dynamixel master.

Added a genric dynamixel manager to handle dynamixel modules.

dynamixel_base/include/dynamixel_common.h

@@ -11,6 +11,7 @@
 #define     DYN_NO_DEVICE       2
 #define     DYN_TIMEOUT         3
 #define     DYN_COMM_ERROR      4
+#define     DYN_BUSY            5
 
 typedef struct{
   unsigned char *data_addr;

dynamixel_base/src/dynamixel_master.c

@@ -133,6 +133,29 @@ unsigned char dyn_master_wait_transmission(TDynamixelMaster *master)
     return DYN_COMM_ERROR;
 }
 
+unsigned char dyn_master_is_transmission_done(TDynamixelMaster *master)
+{
+  unsigned char error;
+
+  error=comm_is_send_done(master->comm_dev);
+  if(error==COMM_BUSY)
+    return DYN_BUSY;
+  else if(error==COMM_SUCCESS)
+  {
+    if(master->comm_dev->time!=0x00000000)
+    {
+      // start the new timeout
+      time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
+    }
+    return DYN_SUCCESS;
+  }
+  else
+  {
+    master->set_rx_mode();
+    return DYN_COMM_ERROR;
+  }
+}
+
 unsigned char dyn_master_send(TDynamixelMaster *master)
 {
   unsigned char error;
@@ -192,6 +215,429 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
   }
 }
 
+unsigned char dyn_master_is_reception_done(TDynamixelMaster *master)
+{
+  if(!master->packet_ready)
+  {
+    if(master->comm_dev->time!=0x00000000)
+    {
+      if(time_is_timeout(master->comm_dev->time))
+      {
+        comm_cancel_irq_receive(master->comm_dev);
+        comm_cancel_dma_receive(master->comm_dev);
+        master->received_bytes=0x00;
+        return DYN_TIMEOUT;
+      }
+      else
+        return DYN_BUSY;
+    }
+    else
+      return DYN_BUSY;
+  }
+  else
+  {
+    master->packet_ready=0x00;
+    return DYN_SUCCESS;
+  }
+}
+
+unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+    dyn_init_read_packet(master->tx_buffer,id,address,length);
+  else
+    dyn2_init_read_packet(master->tx_buffer,id,address,length);
+  master->rx_num_packets=0x01;
+  if(master->return_level==no_return || id==DYN_BROADCAST_ID)
+    master->rx_no_answer=0x01;
+  else
+    master->rx_no_answer=0x00;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+  {
+    // wait for the replay within the given timeout
+    if(master->return_level!=no_return && id!=DYN_BROADCAST_ID)
+    {
+      if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+      {
+        // check the input packet checksum
+        if(master->version==DYN_VER1)
+        {
+          if(dyn_check_checksum(master->rx_buffer)==0xFF)
+          {
+            if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+            {
+              if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+                return DYN_INST_ERROR;
+              else
+                return DYN_SUCCESS;
+            }
+            else 
+              return error;
+          }
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+        else
+        {
+          if(dyn2_check_checksum(master->rx_buffer)==0x01)
+          {
+            if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+            {
+              if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+                return DYN_INST_ERROR;
+              else
+                return DYN_SUCCESS;
+            }
+            else
+              return error;
+          }
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+      }
+      else
+        return error;
+    }
+    else
+      return DYN_SUCCESS;
+  }
+  else
+    return error;
+}
+
+unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+    dyn_init_write_packet(master->tx_buffer,id,address,length,data);
+  else
+    dyn2_init_write_packet(master->tx_buffer,id,address,length,data);
+  master->rx_num_packets=0x01;
+  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
+    master->rx_no_answer=0x00;
+  else
+    master->rx_no_answer=0x01;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+  {
+    // wait for the replay within the given timeout
+    if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
+    {
+      if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+      {
+        // check the input packet checksum
+        if(master->version==DYN_VER1)
+        {
+          if(dyn_check_checksum(master->rx_buffer)==0xFF)
+            return dyn_get_status_error(master->rx_buffer);
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+        else
+        {
+          if(dyn2_check_checksum(master->rx_buffer)==0x01)
+            return dyn2_get_status_error(master->rx_buffer);
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+      }
+      else
+        return error;
+    }
+    else
+      return DYN_SUCCESS;
+  }
+  else
+    return error;
+}
+
+unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+    dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  else
+    dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  master->rx_num_packets=0x01;
+  master->rx_no_answer=0x00;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error;
+
+  error=comm_is_send_done(master->comm_dev);
+  if(error==COMM_BUSY)
+    return DYN_BUSY;
+  else if(error==COMM_SUCCESS)
+    return DYN_SUCCESS;
+  else
+  {
+    master->set_rx_mode();
+    return DYN_COMM_ERROR;
+  }
+}
+
+unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length);
+    master->rx_num_packets=num;
+    if(master->return_level==no_return)
+      master->rx_no_answer=0x01;
+    else
+      master->rx_no_answer=0x00;
+    // enable transmission
+    master->set_tx_mode();
+    // send the data
+    if((error=dyn_master_send(master))!=DYN_SUCCESS)
+      master->set_rx_mode();
+
+    return error;
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  static unsigned char num_dev_done=0;
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+    {
+      // wait for the replay within the given timeout
+      if(master->return_level!=no_return)
+      {
+        if(num_dev_done<num)
+        {
+          if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+          {
+            if(dyn2_check_checksum(master->rx_buffer)==0x01)
+            {
+              if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+              {
+                while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
+                if(dyn2_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length)// not enough data
+                  return DYN_INST_ERROR;
+                else
+                {
+                  num_dev_done++;
+                  return DYN_BUSY;
+                }
+              }
+              else
+                return error;
+            }
+            else
+              return DYN_CHECKSUM_ERROR;
+          }
+          else
+            return error;
+        }
+        else 
+          return DYN_SUCCESS;
+      }
+      else
+        return DYN_SUCCESS;
+    }
+    else
+      return error;
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data);
+    master->rx_num_packets=0x01;
+    master->rx_no_answer=0x00;
+    // enable transmission
+    master->set_tx_mode();
+    // send the data
+    if((error=dyn_master_send(master))!=DYN_SUCCESS)
+      master->set_rx_mode();
+
+    return error;
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error;
+
+  if(master->version==DYN_VER2)
+  {
+    error=comm_is_send_done(master->comm_dev);
+    if(error==COMM_BUSY)
+      return DYN_BUSY;
+    else if(error==COMM_SUCCESS)
+      return DYN_SUCCESS;
+    else
+    {
+      master->set_rx_mode();
+      return DYN_COMM_ERROR;
+    }
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error,i,ver1_address[255],ver1_length[255];
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+  {
+    for(i=0;i<num;i++)
+    {
+      ver1_address[i]=address[i];
+      ver1_length[i]=length[i];
+    }
+    dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length);
+  }
+  else
+    dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
+  master->rx_num_packets=num;
+  if(master->return_level==no_return)
+    master->rx_no_answer=0x01;
+  else
+    master->rx_no_answer=0x00;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  static unsigned char num_dev_done=0;
+  unsigned char error;
+
+  if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+  {
+    // wait for the replay within the given timeout
+    if(master->return_level!=no_return)
+    {
+      if(num_dev_done<num)
+      {
+        if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+        {
+          // check the input packet checksum
+          if(master->version==DYN_VER1)
+          {
+            if(dyn_check_checksum(master->rx_buffer)==0xFF)
+            {
+              if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+              {
+                while(dyn_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
+                if(dyn_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length[num_dev_done])// not enough data
+                  return DYN_INST_ERROR;
+                else
+                {
+                  num_dev_done++;
+                  return DYN_BUSY;
+                }
+              }
+              else
+                return error;
+            }
+            else
+              return DYN_CHECKSUM_ERROR;
+          }
+          else
+          {
+            if(dyn2_check_checksum(master->rx_buffer)==0x01)
+            {
+              if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+              {
+                while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
+                if(dyn2_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length[num_dev_done])// not enough data
+                  return DYN_INST_ERROR;
+                else
+                {
+                  num_dev_done++;
+                  return DYN_BUSY;
+                }
+              }
+              else
+                return error;
+            }
+            else
+              return DYN_CHECKSUM_ERROR;
+          }
+        }
+        else
+          return error;
+      }
+      else
+        return DYN_SUCCESS;
+    }
+    else
+      return DYN_SUCCESS;
+  }
+  else
+    return error;
+}
+
 /* public functions */
 void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
 {
@@ -574,6 +1020,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
       {
         if(master->version==DYN_VER1)
         {
+          while(dyn_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
           if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
             error=DYN_INST_ERROR;
         }

dynamixel_manager/Makefile

+# setup
+# modified by zerom for WinARM 8/2010
+
+COMPILE_OPTS = -mlittle-endian -mthumb -mthumb-interwork
+COMPILE_OPTS += -Wall -O2 -fno-common
+#COMPILE_OPTS += -Wall -g -fno-common
+COMPILE_OPTS += -ffreestanding -nostdlib
+
+COMPILE_OPTS_M4_FPU = -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4
+COMPILE_OPTS_M0 = -mfloat-abi=softfp -mcpu=cortex-m0
+COMPILE_OPTS_M0plus = -mfloat-abi=softfp -mcpu=cortex-m0plus
+COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3
+
+COMM_PATH = ../comm
+UTILS_PATH = ../utils
+DYN_BASE_PATH = ../dynamixel_base
+
+INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include
+
+TCHAIN_PREFIX=arm-none-eabi-
+
+CC = $(TCHAIN_PREFIX)gcc
+CFLAGS = $(COMPILE_OPTS) $(INCLUDE_DIRS)
+
+AR = $(TCHAIN_PREFIX)ar
+ARFLAGS = cr
+
+DYNAMIXEL_OUT_M4_FPU = ./lib/dynamixel_manager_m4_fpu.a
+DYNAMIXEL_OUT_M0 = ./lib/dynamixel_manager_m0.a
+DYNAMIXEL_OUT_M0plus = ./lib/dynamixel_manager_m0plus.a
+DYNAMIXEL_OUT_M3 = ./lib/dynamixel_manager_m3.a
+
+SRC_DIR=./src/
+SRC=$(wildcard $(SRC_DIR)*.c)
+
+DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/
+DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M4_FPU_OBJS = $(patsubst %,$(DYNAMIXEL_M4_FPU_OBJ_DIR)%,$(DYNAMIXEL_M4_FPU_OBJS_TMP))
+
+DYNAMIXEL_M0_OBJ_DIR=build/m0/
+DYNAMIXEL_M0_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M0_OBJS = $(patsubst %,$(DYNAMIXEL_M0_OBJ_DIR)%,$(DYNAMIXEL_M0_OBJS_TMP))
+
+DYNAMIXEL_M0plus_OBJ_DIR=build/m0plus/
+DYNAMIXEL_M0plus_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M0plus_OBJS = $(patsubst %,$(DYNAMIXEL_M0plus_OBJ_DIR)%,$(DYNAMIXEL_M0plus_OBJS_TMP))
+
+DYNAMIXEL_M3_OBJ_DIR=build/m3/
+DYNAMIXEL_M3_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TMP))
+
+all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3)
+$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
+$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+mkdir_build: 
+	mkdir -p build/m4_fpu
+	mkdir -p build/m0
+	mkdir -p build/m0plus
+	mkdir -p build/m3
+$(DYNAMIXEL_OUT_M4_FPU): mkdir_build $(DYNAMIXEL_M4_FPU_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M4_FPU_OBJS)
+$(DYNAMIXEL_OUT_M0): mkdir_build $(DYNAMIXEL_M0_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M0_OBJS)
+$(DYNAMIXEL_OUT_M0plus): mkdir_build $(DYNAMIXEL_M0plus_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M0plus_OBJS)
+$(DYNAMIXEL_OUT_M3): mkdir_build $(DYNAMIXEL_M3_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M3_OBJS)
+
+clean:
+	rm -f $(DYNAMIXEL_M4_FPU_OBJS)
+	rm -f $(DYNAMIXEL_M0_OBJS)
+	rm -f $(DYNAMIXEL_M0plus_OBJS)
+	rm -f $(DYNAMIXEL_M3_OBJS)
+	rm -rf lib
+	rm -rf build

dynamixel_manager/include/dyn_devices.h

+#ifndef _DYN_DEVICES_H
+#define _DYN_DEVICES_H
+
+// servo modules
+#define       SERVO_DX113     0x0071
+#define       SERVO_DX116     0x0074
+#define       SERVO_DX117     0x0075
+#define       SERVO_AX12A     0x000C
+#define       SERVO_AX12W     0x012C
+#define       SERVO_AX18A     0x0012
+#define       SERVO_RX10      0x000A
+#define       SERVO_MX12W     0x0168
+#define       SERVO_MX28      0x001D
+#define       SERVO_RX24F     0x0018
+#define       SERVO_RX28      0x001C
+#define       SERVO_RX64      0x0040
+#define       SERVO_MX64      0x0136
+#define       SERVO_EX106     0x006B
+#define       SERVO_MX106     0x0140
+#define       SERVO_XL320     0x015E
+
+#define       IS_SERVO(model) (model==SERVO_DX113 || model==SERVO_DX116 || model==SERVO_DX117 || model==SERVO_AX12A \
+                               model==SERVO_AX12W || model==SERVO_AX18A || model==SERVO_RX10 || model==SERVO_MX12W \
+                               model==SERVO_MX28 || model==SERVO_RX24F || model==SERVO_RX28 || model==SERVO_RX64 \
+                               model==SERVO_MX64 || model==SERVO_EX106 || model==SERVO_MX106 || model==SERVO_XL320)
+
+#endif

dynamixel_manager/include/dyn_manager.h

+#ifndef _DYN_MANAGER_H
+#define _DYN_MANAGER_H
+
+#include "dynamixel_master.h"
+#include "dyn_module.h"
+
+#define    DYN_MANAGER_PERIODIC            0xFF
+
+#define    DYN_MANAGER_READ                  0
+#define    DYN_MANAGER_WRITE                 1
+
+// public functions
+void dyn_manager_init(void);
+void dyn_manager_scan(void);
+void dyn_manager_add_master(TDynamixelMaster *master);
+unsigned char dyn_manager_get_num_masters(void);
+void dyn_manager_add_module(TDynModule *module);
+unsigned char dyn_manager_get_num_modules(void);
+void dyn_manager_reg_single_op(unsigned char op,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data,unsigned char rep);
+void dyn_manager_reg_sync_op(unsigned char op,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data,unsigned char rep);
+void dyn_manager_reg_bulk_op(unsigned char op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data,unsigned char rep);
+void dyn_manager_loop(void);
+
+#endif 

dynamixel_manager/include/dyn_module.h

+#ifndef _DYN_MODULE_H
+#define _DYN_MODULE_H
+
+#define     MODULE_MAX_NUM_MODELS     32
+
+typedef struct{
+  unsigned short int models[MODULE_MAX_NUM_MODELS];
+  unsigned char num_models;
+  unsigned char period_count;
+  void (*init)(void);
+  void (*add_device)(unsigned char id,unsigned short int model);
+  void (*setup)(void);
+  void (*pre_process)(void);
+  void (*post_process)(void);
+}TDynModule;
+
+//public functions
+void dyn_module_init(TDynModule *module);
+void dyn_module_add_model(TDynModule *module,unsigned short int model);
+void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model);
+
+#endif

dynamixel_manager/src/dyn_module.c

+#include "dyn_module.h"
+
+void dyn_module_init(TDynModule *module)
+{
+  unsigned char i;
+
+  module->num_models=0x00;
+  for(i=0;i<MODULE_MAX_NUM_MODELS;i++)
+    module->models[i]=0x0000;
+  module->period_count=0x00;
+  module->init=0x00000000;
+  module->add_device=0x00000000;
+  module->setup=0x00000000;
+  module->pre_process=0x00000000;
+  module->post_process=0x00000000;
+}
+
+void dyn_module_add_model(TDynModule *module,unsigned short int model)
+{
+  unsigned char i;
+
+  for(i=0;i<module->num_models;i++)
+    if(module->models[i]==model)
+      return;
+  if(i<MODULE_MAX_NUM_MODELS)
+  {
+    module->models[i]=model;
+    module->num_models++;
+  }
+}
+
+void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model)
+{
+  unsigned char i;
+  
+  for(i=0;i<module->num_models;i++)
+  {
+    if(model==module->models[i])
+    {
+      if(module->add_device!=0x00000000)
+        module->add_device(id,model);
+    }
+  }
+}
+

dynamixel_manager/test/Makefile

+# setup
+# modified by zerom for WinARM 8/2010
+
+PROJECT_NAME=dynamixel_manager_test
+TARGET_FILES=$(wildcard *.c)
+TARGET_FILES+=$(wildcard ../src/*.c)
+TARGET_FILES+=$(wildcard ../../utils/src/*.c)
+TARGET_FILES+=$(wildcard ../../comm/src/*.c)
+BUILD_PATH=build
+BIN_PATH=bin
+
+INCLUDE_DIRS = -I../include -I../../utils/include -I../../comm/include -I../../dynamixel_base/include
+
+CC = gcc
+
+UTILS_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
+UTILS_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(UTILS_OBJS_TMP))
+
+OUT_FILE=$(BIN_PATH)/$(PROJECT_NAME)
+
+all: $(OUT_FILE)
+
+make_dirs: 
+	mkdir -p $(BUILD_PATH)
+	mkdir -p $(BIN_PATH)
+
+$(BUILD_PATH)/%.o: %.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../../utils/src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../../comm/src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(OUT_FILE): make_dirs $(UTILS_OBJS) 
+	$(CC) -g $(UTILS_OBJS) -lpthread --output $@
+
+clean:
+	-rm -rf $(BUILD_PATH)
+	-rm -rf $(BIN_PATH)

dynamixel_manager/test/dyn_man_test.c

+#include <stdio.h>
+#include <stdlib.h>
+#include "dyn_devices.h"
+#include "dyn_module.h"
+#include "dyn_manager.h"
+
+TDynamixelMaster dyn_master1;
+TDynamixelMaster dyn_master2;
+
+// timer functions
+unsigned long long int time_get_counts(void)
+{
+  static unsigned long long int count=0;
+ 
+  count+=10000;
+  return count;
+}
+
+unsigned int time_get_counts_per_us(void)
+{
+  return 10000;
+}
+
+// dynamixel master functions
+unsigned char dyn_master_irq_receive_cb(void *dyn_master,unsigned char byte)
+{
+
+}
+
+unsigned char dyn_master_dma_send_cb(void *dyn_master)
+{
+
+}
+
+unsigned char dyn_master_dma_receive_cb(void *dyn_master)
+{
+
+}
+
+void dummy_dyn_master_set_tx_mode(void)
+{
+
+}
+
+void dummy_dyn_master_set_rx_mode(void)
+{
+
+}
+
+void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
+{
+  /* assign communication functions */
+  dev->irq_receive_cb=dyn_master_irq_receive_cb;
+  dev->dma_send_cb=dyn_master_dma_send_cb;
+  dev->dma_receive_cb=dyn_master_dma_receive_cb;
+  master->comm_dev=dev;
+  master->version=version;
+  dev->data=master;
+  /* initialize the internal callbacks */
+  master->set_tx_mode=dummy_dyn_master_set_tx_mode;
+  master->set_rx_mode=dummy_dyn_master_set_rx_mode;
+  /* initialize internal variables */
+  master->packet_ready=0x00;
+  master->received_bytes=0x00;
+  master->rx_timeout_ms=50;
+  master->rx_no_answer=0x00;
+  master->rx_num_packets=0x00;
+  master->return_level=return_all;
+
+  master->set_rx_mode();
+}
+
+void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
+{
+  unsigned int i;
+
+  if(master==&dyn_master1)
+  {
+    for(i=0;i<20;i++)
+      ids[i]=i+1;
+    (*num)=20;
+  }
+  else if(master==&dyn_master2)
+  {
+    for(i=0;i<4;i++)
+      ids[i]=i+21;
+    (*num)=4;
+  }
+}
+
+unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
+{
+  if(master==&dyn_master1)
+  {
+    if(id>0 && id<11)
+    {
+      if(address==0x0000)
+        (*data)=SERVO_AX12A;
+    } 
+    if(id>10 && id<21)
+    {
+      if(address==0x0000)
+        (*data)=SERVO_MX28;
+    } 
+  }
+  else if(master==&dyn_master2)
+  {
+    if(id>20 && id<25)
+    {
+      if(address==0x0000)
+        (*data)=SERVO_XL320;
+    } 
+  }
+}
+
+unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned short int i;
+
+  if(master==&dyn_master1)
+    printf("Master 1\n");
+  else
+    printf("Master 2\n");
+  printf("  read operation to servo %d at address %d with length %d\n",id,address,length);
+  printf("  data:\n");
+  for(i=0;i<length;i++)
+    printf("    0x%x\n",data[i]);
+
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned short int i;
+
+  if(master==&dyn_master1)
+    printf("Master 1\n");
+  else
+    printf("Master 2\n");
+  printf("  write operation to servo %d at address %d with length %d\n",id,address,length);
+  printf("  data:\n");
+  for(i=0;i<length;i++)
+    printf("    0x%x\n",data[i]);
+
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+// module functions
+unsigned char single_op_data[8];
+unsigned char sync_op_data[64];
+unsigned char bulk_op_data[64];
+
+void module1_init(void)
+{
+  unsigned int i;
+
+  printf("module1: init function\n");
+  for(i=0;i<8;i++)
+    single_op_data[i]=i;
+  for(i=0;i<64;i++)
+  {
+    sync_op_data[i]=i;
+    bulk_op_data[i]=i;
+  }
+}
+
+void module1_add_device(unsigned char id,unsigned short int model)
+{
+  printf("module1: add_device function\n");
+  printf("  ID: %d model: %d\n",id,model);
+}
+
+void module1_setup(void)
+{
+  printf("module1: setup function\n");
+  dyn_manager_reg_single_op(DYN_MANAGER_WRITE,11,0x0000,2,&single_op_data[0],DYN_MANAGER_PERIODIC);
+  dyn_manager_reg_single_op(DYN_MANAGER_WRITE,11,0x0000,1,&single_op_data[2],2);
+  dyn_manager_reg_single_op(DYN_MANAGER_READ,13,0x0001,2,&single_op_data[3],6);
+  dyn_manager_reg_single_op(DYN_MANAGER_READ,15,0x0005,4,&single_op_data[0],8);
+}
+
+void module1_pre_process(void)
+{
+  printf("module1: pre_process function\n");
+}
+
+void module1_post_process(void)
+{
+  printf("module1: post_process function\n");
+}
+
+void module2_init(void)
+{
+  printf("module2: init function\n");
+}
+
+void module2_add_device(unsigned char id,unsigned short int model)
+{
+  printf("module2: add_device function\n");
+  printf("  ID: %d model: %d\n",id,model);
+}
+
+void module2_setup(void)
+{
+  printf("module2: setup function\n");
+}
+
+void module2_pre_process(void)
+{
+  printf("module2: pre_process function\n");
+}
+
+void module2_post_process(void)
+{
+  printf("module2: post_process function\n");
+}
+
+int main(void)
+{
+  int i;
+  TDynModule module1;
+  TDynModule module2;
+  TTime dyn_master1_timer; 
+  TComm dyn_master1_comm;
+  TTime dyn_master2_timer; 
+  TComm dyn_master2_comm;
+
+  // init modules
+  dyn_module_init(&module1);
+  dyn_module_add_model(&module1,SERVO_MX28);
+  dyn_module_add_model(&module1,SERVO_MX28);// this should be ignored
+  dyn_module_add_model(&module1,SERVO_XL320);
+  module1.init=module1_init;
+  module1.add_device=module1_add_device;
+  module1.setup=module1_setup;
+  module1.pre_process=module1_pre_process;
+  module1.post_process=module1_post_process;
+  dyn_module_init(&module2);
+  dyn_module_add_model(&module2,SERVO_AX12A);
+  module2.init=module2_init;
+  module2.add_device=module2_add_device;
+  module2.setup=module2_setup;
+  module2.pre_process=module2_pre_process;
+  module2.post_process=module2_post_process;
+
+  // init masters
+  time_init(&dyn_master1_timer,time_get_counts_per_us(),time_get_counts);
+  comm_init(&dyn_master1_comm,0x01,&dyn_master1_timer);
+  dyn_master_init(&dyn_master1,&dyn_master1_comm,DYN_VER1);
+  time_init(&dyn_master2_timer,time_get_counts_per_us(),time_get_counts);
+  comm_init(&dyn_master2_comm,0x01,&dyn_master2_timer);
+  dyn_master_init(&dyn_master2,&dyn_master2_comm,DYN_VER2);
+
+  dyn_manager_init();
+  dyn_manager_add_master(&dyn_master1);
+  dyn_manager_add_master(&dyn_master2);
+  dyn_manager_add_module(&module1);
+  dyn_manager_add_module(&module2);
+  dyn_manager_scan();
+
+  for(i=0;i<10;i++)
+    dyn_manager_loop();
+}