Updated the example to test the servo motion.

dynamixel_manager/test/Makefile

@@ -8,10 +8,11 @@ TARGET_FILES+=$(wildcard ../src/*.c)
 TARGET_FILES+=$(wildcard ../../utils/src/*.c)
 TARGET_FILES+=$(wildcard ../../comm/src/*.c)
 TARGET_FILES+=$(wildcard ../../memory/src/*.c)
+TARGET_FILES+=$(wildcard ../../scheduler/src/*.c)
 BUILD_PATH=build
 BIN_PATH=bin
 
-INCLUDE_DIRS = -I../include -I../include/modules -I../../utils/include -I../../comm/include -I ../../memory/include -I../../dynamixel_base/include
+INCLUDE_DIRS = -I../include -I../include/modules -I../../utils/include -I../../comm/include -I ../../memory/include -I../../dynamixel_base/include -I../../scheduler/include
 
 MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8 -DEEPROM_SIZE=64 -DRAM_SIZE=1024
 
@@ -46,6 +47,9 @@ $(BUILD_PATH)/%.o: ../../comm/src/%.c
 $(BUILD_PATH)/%.o: ../../memory/src/%.c
 	$(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
 
+$(BUILD_PATH)/%.o: ../../scheduler/src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
+
 $(OUT_FILE): make_dirs $(TEST_OBJS) 
 	$(CC) -g $(TEST_OBJS) -lpthread --output $@
 

dynamixel_manager/test/dyn_man_test.c

@@ -6,11 +6,15 @@
 #include "motion_manager.h"
 #include "action.h"
 #include "dyn_servos.h"
+#include "scheduler.h"
 
 TDynamixelMaster dyn_master1;
 TDynamixelMaster dyn_master2;
 TDynManager manager;
 TMemory memory;
+TScheduler scheduler;
+
+unsigned int SystemCoreClock=168000000;
 
 // timer functions
 unsigned long long int time_get_counts(void)
@@ -138,11 +142,11 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns
 
 unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
 {
-  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);
+//  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);
 
   return DYN_SUCCESS;
 }
@@ -156,14 +160,14 @@ unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned cha
 {
   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]);
+//  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;
 }
@@ -177,23 +181,23 @@ unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char
 {
   unsigned short int i,j;
 
-  if(master==&dyn_master1)
-    printf("Master 1\n");
-  else
-    printf("Master 2\n");
-  printf("  sync write operation at address %d with length %d\n",address,length);
-  for(j=0;j<num;j++)
-  {
-    printf("  servo %d data:\n",ids[j]);
-    for(i=0;i<length;i++)
-      printf("    0x%x\n",data[j][i]);
-  }
-/*  if(master==&dyn_master1 && address==P_CW_COMPLIANCE_SLOPE && length==4)
+//  if(master==&dyn_master1)
+//    printf("Master 1\n");
+//  else
+//    printf("Master 2\n");
+//  printf("  sync write operation at address %d with length %d\n",address,length);
+//  for(j=0;j<num;j++)
+//  {
+//    printf("  servo %d data:\n",ids[j]);
+//    for(i=0;i<length;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;
 }
@@ -207,13 +211,13 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
 {
   unsigned short int i;
 
-  if(master==&dyn_master1)
-    printf("Master 1\n");
-  else
-    printf("Master 2\n");
-  printf("  sync read operation at address %d with length %d for servos:\n",address,length);
-  for(i=0;i<num;i++)
-    printf("  servo %d:\n",ids[i]);
+//  if(master==&dyn_master1)
+//    printf("Master 1\n");
+//  else
+//    printf("Master 2\n");
+//  printf("  sync read operation at address %d with length %d for servos:\n",address,length);
+//  for(i=0;i<num;i++)
+//    printf("  servo %d:\n",ids[i]);
 
   return DYN_SUCCESS;
 }
@@ -227,17 +231,17 @@ unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char
 {
   unsigned short int i,j;
 
-  if(master==&dyn_master1)
-    printf("Master 1\n");
-  else
-    printf("Master 2\n");
-  printf("  bulk write operation:\n");
-  for(j=0;j<num;j++)
-  {
-    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][i]);
-  }
+//  if(master==&dyn_master1)
+//    printf("Master 1\n");
+//  else
+//    printf("Master 2\n");
+//  printf("  bulk write operation:\n");
+//  for(j=0;j<num;j++)
+//  {
+//    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][i]);
+//  }
 
   return DYN_SUCCESS;
 }
@@ -251,13 +255,13 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
 {
   unsigned short int i,j;
 
-  if(master==&dyn_master1)
-    printf("Master 1\n");
-  else
-    printf("Master 2\n");
-  printf("  bulk read operation:\n");
-  for(j=0;j<num;j++)
-    printf("  servo %d at address %d with length %d\n",ids[j],address[j],length[j]);
+//  if(master==&dyn_master1)
+//    printf("Master 1\n");
+//  else
+//    printf("Master 2\n");
+//  printf("  bulk read operation:\n");
+//  for(j=0;j<num;j++)
+//    printf("  servo %d at address %d with length %d\n",ids[j],address[j],length[j]);
 
   return DYN_SUCCESS;
 }
@@ -276,13 +280,8 @@ int main(void)
   TTime dyn_master2_timer; 
   TComm dyn_master2_comm;
 
-  // init modules
-  mmanager_init(&mmanager,&memory,16,128);
-  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));
-  
+  scheduler_init(&scheduler,1,100);
+
   // init masters
   time_init(&dyn_master1_timer,time_get_counts_per_us(),time_get_counts);
   comm_init(&dyn_master1_comm,0x01,&dyn_master1_timer);
@@ -291,10 +290,17 @@ int main(void)
   comm_init(&dyn_master2_comm,0x01,&dyn_master2_timer);
   dyn_master_init(&dyn_master2,&dyn_master2_comm,DYN_VER2);
 
-  dyn_manager_init(&manager,&memory,0x0000,0x0064);
+  dyn_manager_init(&manager,&memory,&scheduler,SCHED_CH1,0x0000,0x0064);
+  dyn_manager_set_period(&manager,14);
   dyn_manager_add_master(&manager,&dyn_master1);
   dyn_manager_add_master(&manager,&dyn_master2);
+  // init modules
+  mmanager_init(&mmanager,&memory,16,128);
+  action_init();
+  mmanager_add_module(&mmanager,action_get_module());
   dyn_manager_add_module(&manager,mmanager_get_dyn_module(&mmanager));
+  dyn_module_set_period(mmanager_get_dyn_module(&mmanager),1);
+  dyn_module_enable(mmanager_get_dyn_module(&mmanager));
   dyn_manager_scan(&manager);
 
   /* enable all servos and assign them to the ACTION module */
@@ -304,12 +310,12 @@ int main(void)
     mmanager_enable_servo(&mmanager,i);
   }
   /* load and start a page */
-  if(action_load_page(9))
+  if(action_load_page(7))
     action_start_page(); 
 
-  for(i=0;i<10;i++)
+  for(i=0;i<100;i++)
   {
-    printf("***** Iteration %d *****\n",i);
+//    printf("***** Iteration %d *****\n",i);
     dyn_manager_loop(&manager);
   }
 }