Skip to content
Snippets Groups Projects
Commit 160f7bf2 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the example to test the servo motion.

parent 86782abb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_manager/test/Makefile
+ 5
− 1
View file @ 160f7bf2
......@@ -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
+ 68
− 62
View file @ 160f7bf2
......@@ -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);
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment