Added an example to test the operation of the dynamixel manager module.

dynamixel_manager/test/Makefile

@@ -4,12 +4,13 @@
 PROJECT_NAME=dynamixel_manager_test
 TARGET_FILES=$(wildcard *.c)
 TARGET_FILES+=$(wildcard ../src/*.c)
+TARGET_FILES+=$(wildcard ../src/modules/*.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
+INCLUDE_DIRS = -I../include -I../include/modules -I../../utils/include -I../../comm/include -I../../dynamixel_base/include
 
 CC = gcc
 
@@ -30,6 +31,9 @@ $(BUILD_PATH)/%.o: %.c
 $(BUILD_PATH)/%.o: ../src/%.c
 	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
 
+$(BUILD_PATH)/%.o: ../src/modules/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
 $(BUILD_PATH)/%.o: ../../utils/src/%.c
 	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
 

dynamixel_manager/test/dyn_man_test.c

@@ -3,6 +3,9 @@
 #include "dyn_devices.h"
 #include "dyn_module.h"
 #include "dyn_manager.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "dyn_servos.h"
 
 TDynamixelMaster dyn_master1;
 TDynamixelMaster dyn_master2;
@@ -97,11 +100,23 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns
     {
       if(address==0x0000)
         (*data)=SERVO_AX12A;
+      else if(address==0x0006)
+        (*data)=0x0000;
+      else if(address==0x0008)
+        (*data)=0x03FF;
+      else if(address==0x0024)
+        (*data)=0x01FF;
     } 
     if(id>10 && id<21)
     {
       if(address==0x0000)
         (*data)=SERVO_MX28;
+      else if(address==0x0006)
+        (*data)=0x0000;
+      else if(address==0x0008)
+        (*data)=0x0FFF;
+      else if(address==0x0024)
+        (*data)=0x07FF;
     } 
   }
   else if(master==&dyn_master2)
@@ -110,6 +125,12 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns
     {
       if(address==0x0000)
         (*data)=SERVO_XL320;
+      else if(address==0x0006)
+        (*data)=0x0000;
+      else if(address==0x0008)
+        (*data)=0x03FF;
+      else if(address==0x0025)
+        (*data)=0x01FF;
     } 
   }
 }
@@ -151,7 +172,7 @@ unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned c
   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_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
 {
   unsigned short int i,j;
 
@@ -164,18 +185,24 @@ unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char
   {
     printf("  servo %d data:\n",ids[j]);
     for(i=0;i<length;i++)
-      printf("    0x%x\n",data[j].data_addr[i]);
+      printf("    0x%x\n",data[j][i]);
   }
+/*  if(master==&dyn_master1 && address==P_CW_COMPLIANCE_SLOPE && length==4)
+  {
+    for(i=0;i<num;i++)
+      printf("%d,",data[i][2]+(data[i][3]<<8));
+    printf("\n");
+  }*/
 
   return DYN_SUCCESS;
 }
 
-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_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
 {
   return DYN_SUCCESS;
 }
 
-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_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
 {
   unsigned short int i;
 
@@ -190,12 +217,12 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
   return DYN_SUCCESS;
 }
 
-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_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
 {
   return DYN_SUCCESS;
 }
 
-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_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
 {
   unsigned short int i,j;
 
@@ -208,18 +235,18 @@ unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char
   {
     printf("  servo %d at address %d with length %d and data:\n",ids[j],address[j],length[j]);
     for(i=0;i<length[j];i++)
-      printf("    0x%x\n",data[j].data_addr[i]);
+      printf("    0x%x\n",data[j][i]);
   }
 
   return DYN_SUCCESS;
 }
 
-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_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
 {
   return DYN_SUCCESS;
 }
 
-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_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
 {
   unsigned short int i,j;
 
@@ -234,160 +261,27 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
   return DYN_SUCCESS;
 }
 
-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)
+unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
 {
   return DYN_SUCCESS;
 }
 
-// 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)
-{
-  TWriteData data[8];
-  unsigned char servo_ids[8];
-  unsigned short int address[8],length[8];
-
-  servo_ids[0]=12;
-  servo_ids[1]=21;
-
-  data[0].data_addr=&sync_op_data[4];
-  data[1].data_addr=&sync_op_data[8];
-
-  printf("module1: setup function\n");
-  dyn_manager_single_op_new(&manager,DYN_MANAGER_WRITE,11,0x0000,2,&single_op_data[0]);
-  dyn_manager_single_op_new(&manager,DYN_MANAGER_READ,11,0x0020,4,&single_op_data[0]);
-  dyn_manager_single_op_new(&manager,DYN_MANAGER_WRITE,21,0x0050,6,&single_op_data[0]);
-  dyn_manager_single_op_new(&manager,DYN_MANAGER_READ,22,0x0070,8,&single_op_data[0]);
-  dyn_manager_sync_op_new(&manager,DYN_MANAGER_WRITE,2,servo_ids,0x0000,2,data);
-  dyn_manager_sync_op_new(&manager,DYN_MANAGER_READ,2,servo_ids,0x0000,2,data);
-  servo_ids[0]=12;
-  servo_ids[1]=13;
-  servo_ids[2]=14;
-  servo_ids[3]=15;
-  servo_ids[4]=21;
-  servo_ids[5]=22;
-  servo_ids[6]=23;
-  servo_ids[7]=24;
-  data[0].data_addr=&sync_op_data[0];
-  address[0]=0;
-  length[0]=1;
-  data[1].data_addr=&sync_op_data[1];
-  address[1]=1;
-  length[1]=2;
-  data[2].data_addr=&sync_op_data[3];
-  address[2]=3;
-  length[2]=4;
-  data[3].data_addr=&sync_op_data[7];
-  address[3]=7;
-  length[3]=8;
-  data[4].data_addr=&sync_op_data[15];
-  address[4]=15;
-  length[4]=8;
-  data[5].data_addr=&sync_op_data[23];
-  address[5]=23;
-  length[5]=4;
-  data[6].data_addr=&sync_op_data[27];
-  address[6]=27;
-  length[6]=2;
-  data[7].data_addr=&sync_op_data[29];
-  address[7]=29;
-  length[7]=1;
-  dyn_manager_bulk_op_new(&manager,DYN_MANAGER_WRITE,8,servo_ids,address,length,data);
-  dyn_manager_bulk_op_new(&manager,DYN_MANAGER_READ,8,servo_ids,address,length,data);
-}
-
-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");
-  dyn_manager_single_op_new(&manager,DYN_MANAGER_WRITE,1,0x0050,6,&single_op_data[0]);
-  dyn_manager_single_op_new(&manager,DYN_MANAGER_READ,2,0x0070,8,&single_op_data[0]);
-}
-
-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;
+  TMotionManager mmanager;
   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);
-  dyn_module_set_init_fctn(&module1,module1_init);
-  dyn_module_set_add_device_fctn(&module1,module1_add_device);
-  dyn_module_set_setup_fctn(&module1,module1_setup);
-  dyn_module_set_pre_process_fctn(&module1,module1_pre_process);
-  dyn_module_set_post_process_fctn(&module1,module1_post_process);
-  dyn_module_set_period(&module1,1);
-
-  dyn_module_init(&module2);
-  dyn_module_add_model(&module2,SERVO_AX12A);
-  dyn_module_set_init_fctn(&module2,module1_init);
-  dyn_module_set_add_device_fctn(&module2,module1_add_device);
-  dyn_module_set_setup_fctn(&module2,module1_setup);
-  dyn_module_set_pre_process_fctn(&module2,module1_pre_process);
-  dyn_module_set_post_process_fctn(&module2,module1_post_process);
-  dyn_module_set_period(&module2,1);
-
+  mmanager_init(&mmanager);
+  action_init();
+  mmanager_add_module(&mmanager,action_get_module());
+  dyn_module_set_period(mmanager_get_dyn_module(&mmanager),1);
+  dyn_module_enable(mmanager_get_dyn_module(&mmanager));
+  
   // init masters
   time_init(&dyn_master1_timer,time_get_counts_per_us(),time_get_counts);
   comm_init(&dyn_master1_comm,0x01,&dyn_master1_timer);
@@ -399,10 +293,22 @@ int main(void)
   dyn_manager_init(&manager);
   dyn_manager_add_master(&manager,&dyn_master1);
   dyn_manager_add_master(&manager,&dyn_master2);
-  dyn_manager_add_module(&manager,&module1);
-  dyn_manager_add_module(&manager,&module2);
+  dyn_manager_add_module(&manager,mmanager_get_dyn_module(&mmanager));
   dyn_manager_scan(&manager);
 
+  /* enable all servos and assign them to the ACTION module */
+  for(i=1;i<25;i++)
+  {
+    mmanager_set_module(&mmanager,i,MM_ACTION);
+    mmanager_enable_servo(&mmanager,i);
+  }
+  /* load and start a page */
+  if(action_load_page(9))
+    action_start_page(); 
+
   for(i=0;i<10;i++)
+  {
+    printf("***** Iteration %d *****\n",i);
     dyn_manager_loop(&manager);
+  }
 }