diff --git a/dynamixel_manager/test/Makefile b/dynamixel_manager/test/Makefile
index ab6ea4168a959f2d1f21bbf2388ba9372bd2b8c4..76bfa2cd86f2ccf8ed4898b663537f42595ef0eb 100755
--- a/dynamixel_manager/test/Makefile
+++ b/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 $@
diff --git a/dynamixel_manager/test/dyn_man_test.c b/dynamixel_manager/test/dyn_man_test.c
index e715b06fd882d3f5ce8918a9173570e6a4be1388..ba6351140a9b837f4889a2b53ca3b01a647ec4c1 100644
--- a/dynamixel_manager/test/dyn_man_test.c
+++ b/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);
}
}