diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h index ea125c550f3593b27a54b4f6469a947ac5f14446..7090a876973b125ccfa0017e4b694cadf6ed7e8f 100644 --- a/dynamixel_base/include/dynamixel_common.h +++ b/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; diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index 62e202cc3f14488f04f8196f624fe335b40236f6..cfeb073cfbbfdce3f57a719c8efebf0beea2dd13 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/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; } diff --git a/dynamixel_manager/Makefile b/dynamixel_manager/Makefile new file mode 100755 index 0000000000000000000000000000000000000000..ab1e59b5b6dea777f90b49ffc73ebf51ef73e345 --- /dev/null +++ b/dynamixel_manager/Makefile @@ -0,0 +1,85 @@ +# 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 diff --git a/dynamixel_manager/include/dyn_devices.h b/dynamixel_manager/include/dyn_devices.h new file mode 100644 index 0000000000000000000000000000000000000000..c10582ff5b9f007c2e6aaf3538290bacaa46489d --- /dev/null +++ b/dynamixel_manager/include/dyn_devices.h @@ -0,0 +1,27 @@ +#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 diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h new file mode 100644 index 0000000000000000000000000000000000000000..67e2d7416bb671173daca67b165cc83cbae22e76 --- /dev/null +++ b/dynamixel_manager/include/dyn_manager.h @@ -0,0 +1,24 @@ +#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 diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h new file mode 100644 index 0000000000000000000000000000000000000000..9942ddc24c996b75d651e3afd6cf86d361125511 --- /dev/null +++ b/dynamixel_manager/include/dyn_module.h @@ -0,0 +1,22 @@ +#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 diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c new file mode 100644 index 0000000000000000000000000000000000000000..de9b61f7ced62cb97529d2eb2a5200d18b0db776 --- /dev/null +++ b/dynamixel_manager/src/dyn_manager.c @@ -0,0 +1,580 @@ +#include "dyn_manager.h" + +#define DYN_MANAGER_MAX_NUM_MASTERS 4 +#define DYN_MANAGER_MAX_NUM_MODULES 8 +#define DYN_MANAGER_MAX_NUM_DEVICES 32 +#define DYN_MANAGER_MAX_NUM_SINGLE_OP 16 +#define DYN_MANAGER_MAX_NUM_SYNC_OP 4 +#define DYN_MANAGER_MAX_NUM_BULK_OP 4 + +extern unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +extern unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +extern unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +extern unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +extern 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); +extern 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); +extern 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); +extern 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); +extern 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); +extern 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); +extern 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); +extern 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); + +typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,start_bulk_ops,wait_bulk_ops,ops_done} dyn_manager_states_t; + +typedef struct{ + unsigned char operation; + unsigned char id; + unsigned short int address; + unsigned short int length; + unsigned char *data; + unsigned char repetitions; +}TDynManagerSingleOp; + +typedef struct{ + unsigned char operation; + unsigned char num_devices; + unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned short int address; + unsigned short int length; + TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char repetitions; +}TDynManagerSyncOp; + +typedef struct{ + unsigned char operation; + unsigned char num_devices; + unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES]; + TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char repetitions; +}TDynManagerBulkOp; + +typedef struct{ + TDynManagerSingleOp single_op[DYN_MANAGER_MAX_NUM_SINGLE_OP]; + unsigned char num_single_op; + TDynManagerSyncOp sync_op[DYN_MANAGER_MAX_NUM_SYNC_OP]; + unsigned char num_sync_op; + TDynManagerBulkOp bulk_op[DYN_MANAGER_MAX_NUM_BULK_OP]; + unsigned char num_bulk_op; +}TDynManagerOps; + +typedef struct{ + unsigned short model; + unsigned char master_index; +}TDynManagerDevice; + +typedef struct{ + TDynamixelMaster *masters[DYN_MANAGER_MAX_NUM_MASTERS]; + TDynManagerOps operations[DYN_MANAGER_MAX_NUM_MASTERS]; + unsigned char num_masters; + TDynModule *modules[DYN_MANAGER_MAX_NUM_MODULES]; + unsigned char num_modules; + TDynManagerDevice devices[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char num_devices; +}TDynManager; + +// private variables +TDynManager dyn_manager; + +// private functions +unsigned char dyn_manager_check_id(unsigned char id) +{ + if(id>=DYN_MANAGER_MAX_NUM_DEVICES) + return 0xFF; + if(dyn_manager.devices[id].model==0xFFFF) + return 0xFF; + else + return dyn_manager.devices[id].master_index; +} + +// public functions +void dyn_manager_init(void) +{ + unsigned char i,j,k; + + for(i=0;i<DYN_MANAGER_MAX_NUM_MASTERS;i++) + { + dyn_manager.masters[i]=0x00000000; + for(j=0;j<DYN_MANAGER_MAX_NUM_SINGLE_OP;j++) + { + dyn_manager.operations[i].single_op[j].operation=0xFF; + dyn_manager.operations[i].single_op[j].id=0xFF; + dyn_manager.operations[i].single_op[j].address=0x0000; + dyn_manager.operations[i].single_op[j].length=0x0000; + dyn_manager.operations[i].single_op[j].data=0x00000000; + dyn_manager.operations[i].single_op[j].repetitions=0x00; + } + dyn_manager.operations[i].num_single_op=0; + for(j=0;j<DYN_MANAGER_MAX_NUM_SYNC_OP;j++) + { + dyn_manager.operations[i].sync_op[j].operation=0xFF; + dyn_manager.operations[i].sync_op[j].num_devices=0; + for(k=0;k<DYN_MANAGER_MAX_NUM_DEVICES;k++) + { + dyn_manager.operations[i].sync_op[j].ids[k]=0xFF; + dyn_manager.operations[i].sync_op[j].data[k].data_addr=0x00000000; + } + dyn_manager.operations[i].sync_op[j].address=0x0000; + dyn_manager.operations[i].sync_op[j].length=0x0000; + dyn_manager.operations[i].sync_op[j].repetitions=0x00; + } + dyn_manager.operations[i].num_sync_op=0; + for(j=0;j<DYN_MANAGER_MAX_NUM_BULK_OP;j++) + { + dyn_manager.operations[i].bulk_op[j].operation=0xFF; + dyn_manager.operations[i].bulk_op[j].num_devices=0; + for(k=0;k<DYN_MANAGER_MAX_NUM_DEVICES;k++) + { + dyn_manager.operations[i].bulk_op[j].ids[k]=0xFF; + dyn_manager.operations[i].bulk_op[j].address[k]=0x0000; + dyn_manager.operations[i].bulk_op[j].length[k]=0x0000; + dyn_manager.operations[i].bulk_op[j].data[k].data_addr=0x00000000; + } + dyn_manager.operations[i].bulk_op[j].repetitions=0x00; + } + dyn_manager.operations[i].num_bulk_op=0; + } + dyn_manager.num_masters=0; + for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++) + dyn_manager.modules[i]=0x00000000; + dyn_manager.num_modules=0; + for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) + { + dyn_manager.devices[i].model=0xFFFF; + dyn_manager.devices[i].master_index=0xFF; + } + dyn_manager.num_devices=0; +} + +void dyn_manager_scan(void) +{ + unsigned char servo_ids[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char i,j,k,num=0; + unsigned short int model; + + // call all the modules init function + for(i=0;i<dyn_manager.num_modules;i++) + if(dyn_manager.modules[i]->init!=0x00000000) + dyn_manager.modules[i]->init(); + + // scan all dynamixel buses + for(i=0;i<dyn_manager.num_masters;i++) + { + // scan the bus + dyn_master_scan(dyn_manager.masters[i],&num,servo_ids); + for(j=0;j<num;j++) + { + // get the servo module + dyn_master_read_word(dyn_manager.masters[i],servo_ids[j],0x0000,&model); + // store internal info + if(dyn_manager.devices[servo_ids[j]].master_index==0xFF && dyn_manager.devices[servo_ids[j]].model==0xFFFF)// device not yet initialized + { + dyn_manager.devices[servo_ids[j]].model=model; + dyn_manager.devices[servo_ids[j]].master_index=i; + dyn_manager.num_devices++; + // pass the infor to each of the modules + for(k=0;k<dyn_manager.num_modules;k++) + dyn_module_add_device(dyn_manager.modules[k],servo_ids[j],model); + } + // else ignore device + } + } + // configure the operations + for(i=0;i<dyn_manager.num_modules;i++) + if(dyn_manager.modules[i]->setup!=0x00000000) + dyn_manager.modules[i]->setup(); +} + +void dyn_manager_add_master(TDynamixelMaster *master) +{ + unsigned char i; + + for(i=0;i<dyn_manager.num_masters;i++) + if(dyn_manager.masters[i]==master) + return; + if(i<DYN_MANAGER_MAX_NUM_MASTERS) + { + dyn_manager.masters[i]=master; + dyn_manager.num_masters++; + } +} + +unsigned char dyn_manager_get_num_masters(void) +{ + return dyn_manager.num_masters; +} + +void dyn_manager_add_module(TDynModule *module) +{ + unsigned char i; + + for(i=0;i<dyn_manager.num_modules;i++) + if(dyn_manager.modules[i]==module) + return; + if(i<DYN_MANAGER_MAX_NUM_MODULES) + { + dyn_manager.modules[i]=module; + dyn_manager.num_modules++; + } +} + +unsigned char dyn_manager_get_num_modules(void) +{ + return dyn_manager.num_modules; +} + +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) +{ + unsigned char i,master_index; + + if((master_index=dyn_manager_check_id(id))==0xFF) + return; + for(i=0;i<dyn_manager.operations[master_index].num_single_op;i++) + { + if(dyn_manager.operations[master_index].single_op[i].operation==op && dyn_manager.operations[master_index].single_op[i].id==id) + if(dyn_manager.operations[master_index].single_op[i].address==address && dyn_manager.operations[master_index].single_op[i].length==length) + return; + } + if(i<DYN_MANAGER_MAX_NUM_SINGLE_OP) + { + dyn_manager.operations[master_index].single_op[i].operation=op; + dyn_manager.operations[master_index].single_op[i].id=id; + dyn_manager.operations[master_index].single_op[i].address=address; + dyn_manager.operations[master_index].single_op[i].length=length; + dyn_manager.operations[master_index].single_op[i].data=data; + dyn_manager.operations[master_index].single_op[i].repetitions=rep; + dyn_manager.operations[master_index].num_single_op++; + } +} + +void dyn_manager_remove_single_op(unsigned char master_index,unsigned char single_op_index) +{ + unsigned char i; + + if(master_index<dyn_manager.num_masters) + { + if(single_op_index<dyn_manager.operations[master_index].num_single_op) + { + if(single_op_index<dyn_manager.operations[master_index].num_single_op-1) + { + for(i=single_op_index;i<dyn_manager.operations[master_index].num_single_op;i++) + { + dyn_manager.operations[master_index].single_op[i].operation=dyn_manager.operations[master_index].single_op[i+1].operation; + dyn_manager.operations[master_index].single_op[i].id=dyn_manager.operations[master_index].single_op[i+1].id; + dyn_manager.operations[master_index].single_op[i].address=dyn_manager.operations[master_index].single_op[i+1].address; + dyn_manager.operations[master_index].single_op[i].length=dyn_manager.operations[master_index].single_op[i+1].length; + dyn_manager.operations[master_index].single_op[i].data=dyn_manager.operations[master_index].single_op[i+1].data; + dyn_manager.operations[master_index].single_op[i].repetitions=dyn_manager.operations[master_index].single_op[i+1].repetitions; + } + } + dyn_manager.operations[master_index].num_single_op--; + } + } +} + +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) +{ + unsigned char i,j,k,master_index,present=0x00,added=0x00; + + for(i=0;i<num;i++) + { + if((master_index=dyn_manager_check_id(ids[i]))!=0xFF) + { + added=0x00; + for(j=0;j<dyn_manager.operations[master_index].num_sync_op;j++) + { + if(dyn_manager.operations[master_index].sync_op[j].operation==op) + { + if(dyn_manager.operations[master_index].sync_op[j].address==address && dyn_manager.operations[master_index].sync_op[j].length==length) + { + present=0x00; + for(k=0;k<dyn_manager.operations[master_index].sync_op[i].num_devices;k++) + { + if(ids[i]==dyn_manager.operations[master_index].sync_op[j].ids[k]) + { + present=0x01; + added=0x01; + break; + } + } + if(present==0x00) + { + if(dyn_manager.operations[master_index].sync_op[j].num_devices<DYN_MANAGER_MAX_NUM_DEVICES) + { + dyn_manager.operations[master_index].sync_op[j].ids[dyn_manager.operations[master_index].sync_op[j].num_devices]=ids[i]; + dyn_manager.operations[master_index].sync_op[j].data[dyn_manager.operations[master_index].sync_op[j].num_devices].data_addr=data[i].data_addr; + dyn_manager.operations[master_index].sync_op[j].num_devices++; + added=0x01; + } + } + } + } + } + if(added==0x00) + { + if(j<DYN_MANAGER_MAX_NUM_SYNC_OP) + { + dyn_manager.operations[master_index].sync_op[j].operation=op; + dyn_manager.operations[master_index].sync_op[j].address=address; + dyn_manager.operations[master_index].sync_op[j].length=length; + dyn_manager.operations[master_index].sync_op[j].repetitions=rep; + dyn_manager.operations[master_index].sync_op[j].ids[dyn_manager.operations[master_index].sync_op[j].num_devices]=ids[i]; + dyn_manager.operations[master_index].sync_op[j].data[dyn_manager.operations[master_index].sync_op[j].num_devices].data_addr=data[i].data_addr; + dyn_manager.operations[master_index].sync_op[j].num_devices++; + dyn_manager.operations[master_index].num_sync_op++; + } + } + } + } +} + +void dyn_manager_remove_sync_op(unsigned char master_index,unsigned char sync_op_index) +{ + unsigned char i,j; + + if(master_index<dyn_manager.num_masters) + { + if(sync_op_index<dyn_manager.operations[master_index].num_sync_op) + { + if(sync_op_index<dyn_manager.operations[master_index].num_sync_op-1) + { + for(i=sync_op_index;i<dyn_manager.operations[master_index].num_sync_op;i++) + { + dyn_manager.operations[master_index].sync_op[i].operation=dyn_manager.operations[master_index].sync_op[i+1].operation; + dyn_manager.operations[master_index].sync_op[i].address=dyn_manager.operations[master_index].sync_op[i+1].address; + dyn_manager.operations[master_index].sync_op[i].length=dyn_manager.operations[master_index].sync_op[i+1].length; + dyn_manager.operations[master_index].sync_op[i].repetitions=dyn_manager.operations[master_index].sync_op[i+1].repetitions; + dyn_manager.operations[master_index].sync_op[i].num_devices=dyn_manager.operations[master_index].sync_op[i+1].num_devices; + for(j=0;j<dyn_manager.operations[master_index].sync_op[i].num_devices;j++) + { + dyn_manager.operations[master_index].sync_op[i].ids[j]=dyn_manager.operations[master_index].sync_op[i+1].ids[j]; + dyn_manager.operations[master_index].sync_op[i].data[j].data_addr=dyn_manager.operations[master_index].sync_op[i+1].data[j].data_addr; + } + } + } + dyn_manager.operations[master_index].num_sync_op--; + } + } +} + +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) +{ + unsigned char i,j,k,master_index,present=0x00,added=0x00; + + for(i=0;i<num;i++) + { + if((master_index=dyn_manager_check_id(ids[i]))!=0xFF) + { + added=0x00; + for(j=0;j<dyn_manager.operations[master_index].num_bulk_op;j++) + { + if(dyn_manager.operations[master_index].bulk_op[j].operation==op) + { + present=0x00; + for(k=0;k<dyn_manager.operations[master_index].bulk_op[i].num_devices;k++) + { + if(ids[i]==dyn_manager.operations[master_index].bulk_op[j].ids[k]) + { + if(dyn_manager.operations[master_index].bulk_op[j].address[k]!=address[i] || dyn_manager.operations[master_index].bulk_op[j].length[k]!=length[i]) + added=0x00; + else + added=0x01; + present=0x01; + break; + } + } + if(present==0x00) + { + if(dyn_manager.operations[master_index].bulk_op[j].num_devices<DYN_MANAGER_MAX_NUM_DEVICES) + { + dyn_manager.operations[master_index].bulk_op[j].ids[dyn_manager.operations[master_index].bulk_op[j].num_devices]=ids[i]; + dyn_manager.operations[master_index].bulk_op[j].data[dyn_manager.operations[master_index].bulk_op[j].num_devices].data_addr=data[i].data_addr; + dyn_manager.operations[master_index].bulk_op[j].address[dyn_manager.operations[master_index].bulk_op[j].num_devices]=address[i]; + dyn_manager.operations[master_index].bulk_op[j].length[dyn_manager.operations[master_index].bulk_op[j].num_devices]=length[i]; + dyn_manager.operations[master_index].bulk_op[j].num_devices++; + added=0x01; + } + } + } + } + if(added==0x00) + { + if(j<DYN_MANAGER_MAX_NUM_SYNC_OP) + { + dyn_manager.operations[master_index].bulk_op[j].operation=op; + dyn_manager.operations[master_index].bulk_op[j].repetitions=rep; + dyn_manager.operations[master_index].bulk_op[j].ids[dyn_manager.operations[master_index].bulk_op[j].num_devices]=ids[i]; + dyn_manager.operations[master_index].bulk_op[j].data[dyn_manager.operations[master_index].bulk_op[j].num_devices].data_addr=data[i].data_addr; + dyn_manager.operations[master_index].bulk_op[j].address[dyn_manager.operations[master_index].bulk_op[j].num_devices]=address[i]; + dyn_manager.operations[master_index].bulk_op[j].length[dyn_manager.operations[master_index].bulk_op[j].num_devices]=length[i]; + dyn_manager.operations[master_index].bulk_op[j].num_devices++; + dyn_manager.operations[master_index].num_bulk_op++; + } + } + } + } +} + +void dyn_manager_remove_bulk_op(unsigned char master_index,unsigned char bulk_op_index) +{ + unsigned char i,j; + + if(master_index<dyn_manager.num_masters) + { + if(bulk_op_index<dyn_manager.operations[master_index].num_bulk_op) + { + if(bulk_op_index<dyn_manager.operations[master_index].num_bulk_op-1) + { + for(i=bulk_op_index;i<dyn_manager.operations[master_index].num_bulk_op;i++) + { + dyn_manager.operations[master_index].bulk_op[i].operation=dyn_manager.operations[master_index].bulk_op[i+1].operation; + dyn_manager.operations[master_index].bulk_op[i].repetitions=dyn_manager.operations[master_index].bulk_op[i+1].repetitions; + dyn_manager.operations[master_index].bulk_op[i].num_devices=dyn_manager.operations[master_index].bulk_op[i+1].num_devices; + for(j=0;j<dyn_manager.operations[master_index].bulk_op[i].num_devices;j++) + { + dyn_manager.operations[master_index].bulk_op[i].address[j]=dyn_manager.operations[master_index].bulk_op[i+1].address[j]; + dyn_manager.operations[master_index].bulk_op[i].length[j]=dyn_manager.operations[master_index].bulk_op[i+1].length[j]; + dyn_manager.operations[master_index].bulk_op[i].ids[j]=dyn_manager.operations[master_index].bulk_op[i+1].ids[j]; + dyn_manager.operations[master_index].bulk_op[i].data[j].data_addr=dyn_manager.operations[master_index].bulk_op[i+1].data[j].data_addr; + } + } + } + dyn_manager.operations[master_index].num_bulk_op--; + } + } +} + +void dyn_manager_loop(void) +{ + unsigned char i,done,loop_done=dyn_manager.num_masters; + dyn_manager_states_t states[DYN_MANAGER_MAX_NUM_MASTERS]; + unsigned char current_single_op[DYN_MANAGER_MAX_NUM_MASTERS]; + unsigned char current_sync_op[DYN_MANAGER_MAX_NUM_MASTERS]; + unsigned char current_bulk_op[DYN_MANAGER_MAX_NUM_MASTERS]; + TDynManagerSingleOp *single_op[DYN_MANAGER_MAX_NUM_MASTERS]; + TDynManagerSyncOp *sync_op[DYN_MANAGER_MAX_NUM_MASTERS]; + TDynManagerBulkOp *bulk_op[DYN_MANAGER_MAX_NUM_MASTERS]; + + // initialize the remaining operations + for(i=0;i<dyn_manager.num_masters;i++) + { + states[i]=start_single_ops; + current_single_op[i]=0; + current_sync_op[i]=0; + current_bulk_op[i]=0; + } + + // execute the pre_process functions for all modules + for(i=0;i<dyn_manager.num_modules;i++) + if(dyn_manager.modules[i]->pre_process!=0x00000000) + dyn_manager.modules[i]->pre_process(); + + // send all the commands + do{ + for(i=0;i<dyn_manager.num_masters;i++) + { + switch(states[i]) + { + case start_single_ops: if(current_single_op[i]>=dyn_manager.operations[i].num_single_op) + states[i]=start_sync_ops; + else + { + single_op[i]=&dyn_manager.operations[i].single_op[current_single_op[i]]; + if(single_op[i]->operation==DYN_MANAGER_READ) + dyn_master_start_read_table(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data); + else + dyn_master_start_write_table(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data); + states[i]=wait_single_ops; + } + break; + case wait_single_ops: if(single_op[i]->operation==DYN_MANAGER_READ) + done=dyn_master_is_read_table_done(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data); + else + done=dyn_master_is_write_table_done(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data); + if(done==DYN_BUSY) + states[i]=wait_single_ops; + else + { + if(single_op[i]->repetitions>0 && single_op[i]->repetitions!=0xFF) + single_op[i]->repetitions--; + if(single_op[i]->repetitions==0)// remove operation from list + dyn_manager_remove_single_op(i,current_single_op[i]); + else + current_single_op[i]++; + states[i]=start_single_ops; + } + break; + case start_sync_ops: if(current_sync_op[i]>=dyn_manager.operations[i].num_sync_op) + states[i]=start_bulk_ops; + else + { + sync_op[i]=&dyn_manager.operations[i].sync_op[current_sync_op[i]]; + if(sync_op[i]->operation==DYN_MANAGER_READ) + dyn_master_start_sync_read(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data); + else + dyn_master_start_sync_write(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data); + states[i]=wait_sync_ops; + } + break; + case wait_sync_ops: if(sync_op[i]->operation==DYN_MANAGER_READ) + done=dyn_master_is_sync_read_done(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data); + else + done=dyn_master_is_sync_write_done(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data); + if(done==DYN_BUSY) + states[i]=wait_sync_ops; + else + { + if(sync_op[i]->repetitions>0 && sync_op[i]->repetitions!=0xFF) + sync_op[i]->repetitions--; + if(sync_op[i]->repetitions==0)// remove operation from list + dyn_manager_remove_sync_op(i,current_sync_op[i]); + else + current_sync_op[i]++; + states[i]=start_sync_ops; + } + break; + case start_bulk_ops: if(current_bulk_op[i]>=dyn_manager.operations[i].num_bulk_op) + { + states[i]=ops_done; + loop_done--; + } + else + { + bulk_op[i]=&dyn_manager.operations[i].bulk_op[current_bulk_op[i]]; + if(bulk_op[i]->operation==DYN_MANAGER_READ) + dyn_master_start_bulk_read(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data); + else + dyn_master_start_bulk_write(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data); + states[i]=wait_bulk_ops; + } + break; + case wait_bulk_ops: if(bulk_op[i]->operation==DYN_MANAGER_READ) + done=dyn_master_is_bulk_read_done(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data); + else + done=dyn_master_is_bulk_write_done(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data); + if(done==DYN_BUSY) + states[i]=wait_bulk_ops; + else + { + if(bulk_op[i]->repetitions>0 && bulk_op[i]->repetitions!=0xFF) + bulk_op[i]->repetitions--; + if(bulk_op[i]->repetitions==0)// remove operation from list + dyn_manager_remove_bulk_op(i,current_bulk_op[i]); + else + current_bulk_op[i]++; + states[i]=start_bulk_ops; + } + break; + case ops_done: states[i]=ops_done; + break; + } + } + }while(loop_done>0); + + // execute the pre_process functions for all modules + for(i=0;i<dyn_manager.num_modules;i++) + if(dyn_manager.modules[i]->post_process!=0x00000000) + dyn_manager.modules[i]->post_process(); +} diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c new file mode 100644 index 0000000000000000000000000000000000000000..7900055fd55793b86e01a3898e9daf1eebea2571 --- /dev/null +++ b/dynamixel_manager/src/dyn_module.c @@ -0,0 +1,45 @@ +#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); + } + } +} + diff --git a/dynamixel_manager/test/Makefile b/dynamixel_manager/test/Makefile new file mode 100755 index 0000000000000000000000000000000000000000..c57e9c1ab38c150a85bc5f1300b63950813da197 --- /dev/null +++ b/dynamixel_manager/test/Makefile @@ -0,0 +1,44 @@ +# 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) diff --git a/dynamixel_manager/test/dyn_man_test.c b/dynamixel_manager/test/dyn_man_test.c new file mode 100644 index 0000000000000000000000000000000000000000..e6a2fd47dc6d449c6b879b32b46bb2688e1b2005 --- /dev/null +++ b/dynamixel_manager/test/dyn_man_test.c @@ -0,0 +1,313 @@ +#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(); +}