diff --git a/dynamixel_base/Makefile b/dynamixel_base/Makefile
index fef5dc81ca91d8ee5c4e03d28c6f842ab9f5af02..4f63efe978f1c1ef546c38a513cfb7ebcbe26bb3 100755
--- a/dynamixel_base/Makefile
+++ b/dynamixel_base/Makefile
@@ -14,8 +14,11 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3
COMM_PATH = ../comm
UTILS_PATH = ../utils
MEMORY_PATH = ../memory
+SCHEDULER_PATH = ../scheduler
-INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include
+INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include -I$(SCHEDULER_PATH)/include
+
+MACROS = -DMAX_DYN_SLAVE_TX_BUFFER_LEN=256 -DMAX_DYN_SLAVE_RX_BUFFER_LEN=256 -DMAX_DYN_SLAVE_REG_BUFFER_LEN=256 -DMAX_NUM_SLAVE_DEVICES=4 -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256
DOC_DIR = ./doc
@@ -53,13 +56,13 @@ DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TM
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 $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0) -o $@ $<
$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M0plus) -o $@ $<
$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c
- $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+ $(CC) -c $(CFLAGS) $(MACROS) $(COMPILE_OPTS_M3) -o $@ $<
mkdir_build:
mkdir -p build/m4_fpu
mkdir -p build/m0
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 7da7c90be7873723a8b9b27c5db894b792ad787e..8ee13ea4ea16273afe5b31e7ec1b20ef404d227b 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -6,11 +6,11 @@
#include "dynamixel2.h"
#ifndef MAX_DYN_MASTER_TX_BUFFER_LEN
- #define MAX_DYN_MASTER_TX_BUFFER_LEN 256
+ #error "Please, specify the maximum TX buffer length with the MAX_DYN_MASTER_TX_BUFFER_LEN macro"
#endif
#ifndef MAX_DYN_MASTER_RX_BUFFER_LEN
- #define MAX_DYN_MASTER_RX_BUFFER_LEN 256
+ #error "Please, specify the maximum RX buffer length with the MAX_DYN_MASTER_TX_BUFFER_LEN macro"
#endif
/**
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 37902b616fd26d24244cd6cfbba0adf54164e5d6..cac4d0b20b0e4417b2902f36f1ad85d3ea1ac5dc 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -4,52 +4,47 @@
#include "comm.h"
#include "dynamixel.h"
#include "dynamixel2.h"
-#include "dynamixel_slave_registers.h"
-#include "ram.h"
+#include "scheduler.h"
+#include "dynamixel_slave_device.h"
#ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
- #define MAX_DYN_SLAVE_TX_BUFFER_LEN 256
+ #error "Please, specify the maximum TX buffer length with the MAX_DYN_SLAVE_TX_BUFFER_LEN macro"
#endif
#ifndef MAX_DYN_SLAVE_RX_BUFFER_LEN
- #define MAX_DYN_SLAVE_RX_BUFFER_LEN 256
+ #error "Please, specify the maximum RX buffer length with the MAX_DYN_SLAVE_RX_BUFFER_LEN macro"
#endif
-#ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN
- #define MAX_DYN_SLAVE_REG_BUFFER_LEN 256
+#ifndef MAX_NUM_SLAVE_DEVICES
+ #error "Please, specify the maximum number of slave devices with the MAX_NUM_SLAVE_DEVICES macro"
#endif
/**
* \brief
*
*/
-typedef struct
+typedef struct TDynamixelSlave
{
/**
* \brief
*
*/
- TComm *comm_dev;
- /**
- * \brief
- *
- */
- TDynVersion version;
+ TScheduler *scheduler;
/**
* \brief
*
*/
- unsigned char address;
+ sched_channel_t sch_channel;
/**
* \brief
*
*/
- unsigned char return_delay;
+ TComm *comm_dev;
/**
* \brief
*
*/
- return_level_t return_level;
+ TDynVersion version;
/**
* \brief
*
@@ -84,47 +79,12 @@ typedef struct
* \brief
*
*/
- unsigned short int reg_address;
- /**
- * \brief
- *
- */
- unsigned short int reg_length;
- /**
- * \brief
- *
- */
- unsigned char reg_buffer[MAX_DYN_SLAVE_REG_BUFFER_LEN];
- /**
- * \brief
- *
- */
- unsigned short int sync_bulk_address;
+ unsigned char num_slave_devices;
/**
* \brief
*
*/
- unsigned short int sync_bulk_length;
- /**
- * \brief
- *
- */
- unsigned char sync_bulk_prev_id;
- /**
- * \brief
- *
- */
- unsigned char sync_read_pending;
- /**
- * \brief
- *
- */
- unsigned char bulk_read_pending;
- /**
- * \brief
- *
- */
- TMemModule mem_module;
+ TDynamixelSlaveDevice *slave_devices[MAX_NUM_SLAVE_DEVICES];
/**
* \brief
*
@@ -140,26 +100,6 @@ typedef struct
*
*/
void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate);
- /**
- * \brief
- *
- */
- void (*on_ping)(void);
- /**
- * \brief
- *
- */
- unsigned char (*on_read)(unsigned short int address,unsigned short int length,unsigned char *data);
- /**
- * \brief
- *
- */
- unsigned char (*on_write)(unsigned short int address,unsigned short int length,unsigned char *data);
- /**
- * \brief
- *
- */
- unsigned char (*on_reset)(void);
/**
* \brief
*
@@ -172,57 +112,27 @@ typedef struct
* \brief
*
*/
-unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version);
-/**
- * \brief
- *
- */
-void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
-/**
- * \brief
- *
- */
-inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms);
-/**
- * \brief
- *
- */
-inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address);
-/**
- * \brief
- *
- */
-inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave);
+unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version);
/**
* \brief
*
*/
-inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay);
+unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device);
/**
* \brief
*
*/
-inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave);
-/**
- * \brief
- *
- */
-inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level);
+void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
/**
* \brief
*
*/
-inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave);
+inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms);
/**
* \brief
*
*/
inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave);
-/**
- * \brief
- *
- */
-void dyn_slave_loop(TDynamixelSlave *slave);
#endif
diff --git a/dynamixel_base/include/dynamixel_slave_device.h b/dynamixel_base/include/dynamixel_slave_device.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7b9c40bc9d310ab8bda6f3c528e1f03c50b4b67
--- /dev/null
+++ b/dynamixel_base/include/dynamixel_slave_device.h
@@ -0,0 +1,159 @@
+#ifndef _DYNAMIXEL_SLAVE_DEVICE_H
+#define _DYNAMIXEL_SLAVE_DEVICE_H
+
+#include "dynamixel_slave_registers.h"
+#include "dynamixel.h"
+#include "dynamixel2.h"
+#include "ram.h"
+
+#ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN
+ #error "Please, specify the maximum registered buffer length with the MAX_DYN_SLAVE_REG_BUFFER_LEN macro"
+#endif
+
+/**
+ * \brief
+ *
+ */
+typedef struct
+{
+ /**
+ * \brief
+ *
+ */
+ struct TDynamixelSlave *slave;
+ /**
+ * \brief
+ *
+ */
+ unsigned char address;
+ /**
+ * \brief
+ *
+ */
+ unsigned char return_delay;
+ /**
+ * \brief
+ *
+ */
+ return_level_t return_level;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int reg_address;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int reg_length;
+ /**
+ * \brief
+ *
+ */
+ unsigned char reg_buffer[MAX_DYN_SLAVE_REG_BUFFER_LEN];
+ /**
+ * \brief
+ *
+ */
+ unsigned short int sync_bulk_address;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int sync_bulk_length;
+ /**
+ * \brief
+ *
+ */
+ unsigned char sync_bulk_prev_id;
+ /**
+ * \brief
+ *
+ */
+ unsigned char sync_read_pending;
+ /**
+ * \brief
+ *
+ */
+ unsigned char bulk_read_pending;
+ /**
+ * \brief
+ *
+ */
+ TMemory memory;
+ /**
+ * \brief
+ *
+ */
+ TMemModule mem_module;
+ /**
+ * \brief
+ *
+ */
+ void (*on_ping)(void);
+ /**
+ * \brief
+ *
+ */
+ unsigned char (*on_read)(unsigned short int address,unsigned short int length,unsigned char *data);
+ /**
+ * \brief
+ *
+ */
+ unsigned char (*on_write)(unsigned short int address,unsigned short int length,unsigned char *data);
+ /**
+ * \brief
+ *
+ */
+ unsigned char (*on_reset)(void);
+}TDynamixelSlaveDevice;
+
+/* public functions */
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char address,unsigned char eeprom_base_address1,unsigned char eeprom_base_address2);
+/**
+ * \brief
+ *
+ */
+inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address);
+/**
+ * \brief
+ *
+ */
+inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device);
+/**
+ * \brief
+ *
+ */
+inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay);
+/**
+ * \brief
+ *
+ */
+inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device);
+/**
+ * \brief
+ *
+ */
+inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level);
+/**
+ * \brief
+ *
+ */
+inline return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device);
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned char *length, unsigned char *data);
+/**
+ * \brief
+ *
+ */
+unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned short int *length, unsigned char *data);
+
+
+#endif
diff --git a/dynamixel_base/include/dynamixel_slave_registers.h b/dynamixel_base/include/dynamixel_slave_registers.h
index 8a3a01bd147a1913bc031aba1c32c97a49960954..2a09dc5829e22af41fd05bd411e17e653d48c222 100644
--- a/dynamixel_base/include/dynamixel_slave_registers.h
+++ b/dynamixel_base/include/dynamixel_slave_registers.h
@@ -1,25 +1,17 @@
#ifndef _DYNAMIXEL_SLAVE_REGISTERS_H
#define _DYNAMIXEL_SLAVE_REGISTERS_H
-#ifndef EEPROM_DYN_SLAVE_BASE_ADDRESS1
- #define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000)
-#endif
-
#define EEPROM_DYN_SLAVE_LENGTH1 6
-#ifndef EEPROM_DYN_SLAVE_BASE_ADDRESS2
- #define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0000)
-#endif
-
#define EEPROM_DYN_SLAVE_LENGTH2 1
-#define DEVICE_MODEL (EEPROM_DYN_SLAVE_BASE_ADDRESS1)
-#define FIRMWARE_VERSION (EEPROM_DYN_SLAVE_BASE_ADDRESS1+2)
-#define DEVICE_ID (EEPROM_DYN_SLAVE_BASE_ADDRESS1+3)
-#define BAUDRATE (EEPROM_DYN_SLAVE_BASE_ADDRESS1+4)
-#define RETURN_DELAY (EEPROM_DYN_SLAVE_BASE_ADDRESS1+5)
+#define DEVICE_MODEL 0
+#define FIRMWARE_VERSION 2
+#define DEVICE_ID 3
+#define BAUDRATE 4
+#define RETURN_DELAY 5
-#define RETURN_LEVEL (EEPROM_DYN_SLAVE_BASE_ADDRESS2)
+#define RETURN_LEVEL 0
#ifndef DEFAULT_DEVICE_MODEL
#define DEFAULT_DEVICE_MODEL 0x7300
@@ -41,5 +33,15 @@
#define DEFAULT_RETURN_LEVEL 0x0002
#endif
+#define dyn_slave_control_eeprom_data(name,section_name,base_address1,base_address2) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_DEVICE_MODEL&0x00FF,base_address1+DEVICE_MODEL, \
+ (DEFAULT_DEVICE_MODEL>>8)&0x00FF,base_address1+DEVICE_MODEL+1, \
+ DEFAULT_FIRMWARE_VERSION,base_address1+FIRMWARE_VERSION, \
+ DEFAULT_DEVICE_ID,base_address1+DEVICE_ID, \
+ DEFAULT_BAUDRATE,base_address1+BAUDRATE, \
+ DEFAULT_RETURN_DELAY,base_address1+RETURN_DELAY, \
+ DEFAULT_RETURN_LEVEL,base_address2+RETURN_LEVEL};
+
+
#endif
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index e1818981fa23d44bc06a1f2ae9d8159010b2acab..77232d7e05ac6300e7756d1ac487fe0a6be1c089 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -1,44 +1,6 @@
#include "dynamixel_slave.h"
-/* EEPROM data */
-unsigned short int dyn_slave_eeprom_data[] __attribute__ ((section (".eeprom")))={DEFAULT_DEVICE_MODEL&0x00FF,DEVICE_MODEL,
- (DEFAULT_DEVICE_MODEL>>8)&0x00FF,DEVICE_MODEL+1,
- DEFAULT_FIRMWARE_VERSION,FIRMWARE_VERSION,
- DEFAULT_DEVICE_ID,DEVICE_ID,
- DEFAULT_BAUDRATE,BAUDRATE,
- DEFAULT_RETURN_DELAY,RETURN_DELAY,
- DEFAULT_RETURN_LEVEL,RETURN_LEVEL};
-
-/* private functions */
-void dyn_slave_write_cmd(TDynamixelSlave *dyn_slave,unsigned short int address,unsigned short int length,unsigned char *data)
-{
- if(ram_in_range(DEVICE_ID,address,length))
- {
- dyn_slave_set_address(dyn_slave,data[DEVICE_ID-address]);
- ram_data[DEVICE_ID]=data[DEVICE_ID-address];
- }
- if(ram_in_range(BAUDRATE,address,length))
- {
- dyn_slave_set_baudrate(dyn_slave,2000000/(data[BAUDRATE-address]+1));
- ram_data[BAUDRATE]=data[BAUDRATE-address];
- }
- if(ram_in_range(RETURN_DELAY,address,length))
- {
- dyn_slave_set_return_delay(dyn_slave,data[RETURN_DELAY-address]);
- ram_data[RETURN_DELAY]=data[RETURN_DELAY-address];
- }
- if(ram_in_range(RETURN_LEVEL,address,length))
- {
- dyn_slave_set_return_level(dyn_slave,data[RETURN_LEVEL-address]);
- ram_data[RETURN_LEVEL]=data[RETURN_LEVEL-address];
- }
-}
-
-void dyn_slave_read_cmd(TDynamixelSlave *dyn_slave,unsigned short int address,unsigned short int length,unsigned char *data)
-{
- ram_read_table(address,length,data);
-}
-
+// communication functions
unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte)
{
if(dyn_slave!=0x00000000)
@@ -152,44 +114,24 @@ void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate)
}
-void dummy_dyn_slave_on_ping(void)
-{
-
-}
-
-unsigned char dummy_dyn_slave_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- return DYN_SUCCESS;
-}
-
-unsigned char dummy_dyn_slave_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- return DYN_SUCCESS;
-}
-
-unsigned char dummy_dyn_slave_on_reset(void)
-{
- return DYN_SUCCESS;
-}
-
unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
{
return DYN_NO_DEVICE;
}
-void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,unsigned short int length, unsigned char *data)
+void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,unsigned char error,unsigned short int length, unsigned char *data,unsigned char return_delay)
{
if(slave!=0x00000000)
{
// wait until the previous transmission has ended (if any)
while(comm_is_send_done(slave->comm_dev)==COMM_BUSY);
- if(slave->return_delay>0)
+ if(return_delay>0)
if(slave->comm_dev->time!=0x00000000)
- time_delay_us(slave->comm_dev->time,slave->return_delay<<1);
+ time_delay_us(slave->comm_dev->time,return_delay<<1);
if(slave->version==DYN_VER1)
{
// create the status packet
- dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
+ dyn_init_status_packet(slave->tx_buffer,address,error,length,data);
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
@@ -198,7 +140,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,uns
else
{
// create the status packet
- dyn2_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
+ dyn2_init_status_packet(slave->tx_buffer,address,error,length,data);
// set the tx mode, if necessary
slave->set_tx_mode();
// start transmission by DMA
@@ -207,272 +149,46 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,uns
}
}
-void dyn_v1_slave_loop(TDynamixelSlave *slave)
-{
- static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,length,address,prev_id,id;
-
- if(slave!=0x000000000)
- {
- id=dyn_get_id(slave->rx_buffer);
- if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
- {
- // check the packet checksum
- if(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay
- {
- // process the packet
- switch(dyn_get_instruction(slave->rx_buffer))
- {
- case DYN_PING: slave->on_ping();
- if(id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data);
- if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data);
- error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
- slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_ACTION: if(slave->reg_address!=0xFFFF)
- {
- error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
- slave->reg_address=0xFFFF;
- }
- else
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_RESET:
- break;
- case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
- error=slave->on_write(address,length,data);
- break;
- case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->bulk_read_pending=0x01;
- }
- }
- break;
- case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- default:
- break;
- }
- }
- else
- {
- // send a checksum error answer
- if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
- }
- }
- else
- {
- if(slave->bulk_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->bulk_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- else// the packet is addressed to another device, so relay it
- {
- if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
- {
- // set the tx mode, if necessary
- slave->set_tx_mode();
- // start transmission by DMA
- comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
- }
- }
- }
- }
-}
-
-void dyn_v2_slave_loop(TDynamixelSlave *slave)
+void dyn_slave_loop(TDynamixelSlave *slave)
{
- static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,prev_id,id;
- static unsigned short int length,address;
+ unsigned char i,send_status,error,data[MAX_DYN_SLAVE_TX_BUFFER_LEN];
+ unsigned short int length;
if(slave!=0x00000000)
{
- id=dyn2_get_id(slave->rx_buffer);
- if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ if(slave->packet_ready)// check if a new instruction packet has been received
{
- // check the packet checksum
- if(dyn2_check_checksum(slave->rx_buffer)==0x01)// the incomming packet is okay
- {
- // process the packet
- switch(dyn2_get_instruction(slave->rx_buffer))
- {
- case DYN_PING: slave->on_ping();
- if(id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=slave->on_read(dyn2_get_read_address(slave->rx_buffer),dyn2_get_read_length(slave->rx_buffer),data);
- if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn2_get_read_length(slave->rx_buffer),data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_WRITE: length=dyn2_get_write_data(slave->rx_buffer,data);
- error=slave->on_write(dyn2_get_write_address(slave->rx_buffer),length,data);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_REG_WRITE: slave->reg_length=dyn2_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
- slave->reg_address=dyn2_get_reg_write_address(slave->rx_buffer);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_ACTION: if(slave->reg_address!=0xFFFF)
- {
- error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
- slave->reg_address=0xFFFF;
- }
- else
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_RESET:
- break;
- case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->sync_read_pending=0x01;
- }
- }
- break;
- case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
- error=slave->on_write(address,length,data);
- break;
- case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->bulk_read_pending=0x01;
- }
- }
- break;
- case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))
- error=slave->on_write(address,length,data);
- break;
- default:
- break;
- }
- }
- else
+ slave->packet_ready=0x00;
+ // check address
+ for(i=0;i<slave->num_slave_devices;i++)
{
- // send a checksum error answer
- if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
+ if(slave->version==DYN_VER1)
+ send_status=dyn_v1_slave_loop(slave->slave_devices[i],slave->rx_buffer,&error,(unsigned char *)&length,data);
+ else
+ send_status=dyn_v2_slave_loop(slave->slave_devices[i],slave->rx_buffer,&error,&length,data);
+ if(send_status==0x01)
+ dyn_slave_send_status_packet(slave,slave->slave_devices[i]->address,error,length,data,slave->slave_devices[i]->return_delay);
}
+ if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
+ {
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+ }
}
else
{
- if(slave->bulk_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->bulk_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- else if(slave->sync_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->sync_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- else// the packet is addressed to another device, so relay it
+ if(slave->comm_dev->time!=0x00000000)
{
- if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
- {
- // set the tx mode, if necessary
- slave->set_tx_mode();
- // start transmission by DMA
- comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+8);
+ if(time_is_timeout(slave->comm_dev->time))
+ {
+ /* cancel any IRQ or DMA reception */
+ comm_cancel_irq_receive(slave->comm_dev);
+ comm_cancel_dma_receive(slave->comm_dev);
+ slave->received_bytes=0;
+ /* enable reception by IRQ */
+ comm_receive_irq(slave->comm_dev,0);
}
}
}
@@ -480,8 +196,10 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave)
}
/* public functions */
-unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version)
+unsigned char dyn_slave_init(TDynamixelSlave *slave,TComm *dev,TScheduler *scheduler,sched_channel_t ch,TDynVersion version)
{
+ unsigned char i;
+
/* assign communication functions */
dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb;
dev->dma_send_cb=(unsigned char (*)(void *))dyn_slave_dma_send_cb;
@@ -493,37 +211,20 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,u
slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
slave->set_rx_mode=dummy_dyn_slave_set_rx_mode;
slave->set_baudrate=dummy_dyn_slave_set_baudrate;
- slave->on_ping=dummy_dyn_slave_on_ping;
- slave->on_read=dummy_dyn_slave_on_read;
- slave->on_write=dummy_dyn_slave_on_write;
- slave->on_reset=dummy_dyn_slave_on_reset;
slave->on_relay=dummy_dyn_slave_on_relay;
/* initialize internal variables */
- slave->address=address;
- slave->return_delay=0x00;
- slave->return_level=return_all;
slave->packet_ready=0x00;
slave->rx_timeout_ms=50;
slave->received_bytes=0x00;
- slave->reg_address=0xFFFF;
- slave->reg_length=0x0000;
- slave->sync_bulk_address=0x0000;
- slave->sync_bulk_length=0x0000;
- slave->sync_bulk_prev_id=0x00;
- slave->sync_read_pending=0x00;
- slave->bulk_read_pending=0x00;
+ /* initialize slave devices */
+ slave->num_slave_devices=0;
+ for(i=0;i<MAX_NUM_SLAVE_DEVICES;i++)
+ slave->slave_devices[i]=0x00000000;
- /* initialize memory module */
- mem_module_init(&slave->mem_module);
- slave->mem_module.data=slave;
- slave->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_write_cmd;
- slave->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_read_cmd;
- if(!mem_module_add_eeprom_segment(&slave->mem_module,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_LENGTH1))
- return 0x00;
- if(!mem_module_add_eeprom_segment(&slave->mem_module,EEPROM_DYN_SLAVE_BASE_ADDRESS2,EEPROM_DYN_SLAVE_LENGTH2))
- return 0x00;
- if(!mem_add_module(memory,&slave->mem_module))
- return 0x00;
+ /* assigna a scheduler channel */
+ scheduler_set_channel(scheduler,ch,(void (*)(void *))dyn_slave_loop,1,slave);
+ slave->scheduler=scheduler;
+ slave->sch_channel=ch;
slave->set_rx_mode();
/* enable reception by IRQ */
@@ -532,57 +233,33 @@ unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,u
return 0x01;
}
-void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
-{
- slave->set_baudrate(slave->comm_dev,baudrate);
-}
-
-void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
-{
- if(slave!=0x00000000)
- slave->rx_timeout_ms=timeout_ms;
-}
-
-void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address)
+unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device)
{
if(slave!=0x00000000)
- slave->address=address;
-}
-
-unsigned char dyn_slave_get_address(TDynamixelSlave *slave)
-{
- if(slave!=0x00000000)
- return slave->address;
- else
- return 0xFF;
-}
-
-void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay)
-{
- if(slave!=0x00000000)
- slave->return_delay=delay;
-}
-
-unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave)
-{
- if(slave!=0x00000000)
- return slave->return_delay;
+ {
+ if(slave->num_slave_devices<=(MAX_NUM_SLAVE_DEVICES-1))
+ {
+ slave->slave_devices[slave->num_slave_devices]=device;
+ slave->num_slave_devices++;
+ device->slave=slave;
+ return 0x01;
+ }
+ else
+ return 0x00;
+ }
else
- return 0xFF;
+ return 0x00;
}
-void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level)
+void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
{
- if(slave!=0x00000000)
- slave->return_level=level;
+ slave->set_baudrate(slave->comm_dev,baudrate);
}
-return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
+void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
{
if(slave!=0x00000000)
- return slave->return_level;
- else
- return return_all;
+ slave->rx_timeout_ms=timeout_ms;
}
TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
@@ -593,33 +270,3 @@ TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
return DYN_VER1;
}
-void dyn_slave_loop(TDynamixelSlave *slave)
-{
- if(slave!=0x00000000)
- {
- if(slave->packet_ready)// check if a new instruction packet has been received
- {
- slave->packet_ready=0x00;
- // check address
- if(slave->version==DYN_VER1)
- dyn_v1_slave_loop(slave);
- else
- dyn_v2_slave_loop(slave);
- }
- else
- {
- if(slave->comm_dev->time!=0x00000000)
- {
- if(time_is_timeout(slave->comm_dev->time))
- {
- /* cancel any IRQ or DMA reception */
- comm_cancel_irq_receive(slave->comm_dev);
- comm_cancel_dma_receive(slave->comm_dev);
- slave->received_bytes=0;
- /* enable reception by IRQ */
- comm_receive_irq(slave->comm_dev,0);
- }
- }
- }
- }
-}
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
new file mode 100644
index 0000000000000000000000000000000000000000..19d833975dc93b2c8e15411e163c8c403f892012
--- /dev/null
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -0,0 +1,450 @@
+#include "dynamixel_slave_device.h"
+#include "dynamixel_slave.h"
+
+/* private functions */
+void dyn_slave_device_write_cmd(TDynamixelSlaveDevice *device,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_range(DEVICE_ID,address,length))
+ {
+ dyn_slave_device_set_address(device,data[DEVICE_ID-address]);
+ ram_data[DEVICE_ID]=data[DEVICE_ID-address];
+ }
+ if(ram_in_range(BAUDRATE,address,length))
+ {
+ dyn_slave_set_baudrate(device->slave,2000000/(data[BAUDRATE-address]+1));
+ ram_data[BAUDRATE]=data[BAUDRATE-address];
+ }
+ if(ram_in_range(RETURN_DELAY,address,length))
+ {
+ dyn_slave_device_set_return_delay(device,data[RETURN_DELAY-address]);
+ ram_data[RETURN_DELAY]=data[RETURN_DELAY-address];
+ }
+ if(ram_in_range(RETURN_LEVEL,address,length))
+ {
+ dyn_slave_device_set_return_level(device,data[RETURN_LEVEL-address]);
+ ram_data[RETURN_LEVEL]=data[RETURN_LEVEL-address];
+ }
+}
+
+void dyn_slave_device_read_cmd(TDynamixelSlaveDevice *devicee,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(address,length,data);
+}
+
+void dummy_dyn_slave_device_on_ping(void)
+{
+
+}
+
+unsigned char dummy_dyn_slave_device_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dummy_dyn_slave_device_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dummy_dyn_slave_device_on_reset(void)
+{
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_v1_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned char *length,unsigned char *data)
+{
+ static unsigned char address,prev_id,id;
+ unsigned char send_status=0x00;
+
+ if(device!=0x000000000)
+ {
+ id=dyn_get_id(rx_buffer);
+ if(id==device->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ if(dyn_check_checksum(rx_buffer)==0xFF)// the incomming packet is okay
+ {
+ // process the packet
+ switch(dyn_get_instruction(rx_buffer))
+ {
+ case DYN_PING: device->on_ping();
+ if(id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ *error=DYN_NO_ERROR;
+ *length=0;
+ }
+ break;
+ case DYN_READ: (*error)=device->on_read(dyn_get_read_address(rx_buffer),dyn_get_read_length(rx_buffer),data);
+ if(device->return_level!=no_return && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=dyn_get_read_length(rx_buffer);
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ break;
+ case DYN_WRITE: (*length)=dyn_get_write_data(rx_buffer,data);
+ (*error)=device->on_write(dyn_get_write_address(rx_buffer),*length,data);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*length)=0;
+ if((*error)!=DYN_NO_ERROR)
+ (*error)=DYN_INST_ERROR;
+ }
+ break;
+ case DYN_REG_WRITE: device->reg_length=dyn_get_reg_write_data(rx_buffer,device->reg_buffer);
+ device->reg_address=dyn_get_reg_write_address(rx_buffer);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_ACTION: if(device->reg_address!=0xFFFF)
+ {
+ (*error)=device->on_write(device->reg_address,device->reg_length,device->reg_buffer);
+ device->reg_address=0xFFFF;
+ }
+ else
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: send_status=0x01;
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ break;
+ case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(rx_buffer,device->address,&address,length,data))// the device is addressed
+ (*error)=device->on_write(address,*length,data);
+ break;
+ case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(rx_buffer,device->address,&address,length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ send_status=0x01;
+ (*error)=device->on_read(address,*length,data);
+ if((*error)!=DYN_NO_ERROR)
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ device->sync_bulk_address=address;
+ device->sync_bulk_length=*length;
+ device->sync_bulk_prev_id=prev_id;
+ device->bulk_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_BULK_WRITE: send_status=0x01;
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(rx_buffer)!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_CHECKSUM_ERROR;
+ (*length)=0;
+ }
+ }
+ }
+ else
+ {
+ if(device->bulk_read_pending)
+ {
+ if(id==device->sync_bulk_prev_id)
+ {
+ device->bulk_read_pending=0x00;
+ send_status=0x01;
+ (*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
+ if((*error)==DYN_NO_ERROR)
+ (*length)=device->sync_bulk_length;
+ else
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ }
+ }
+ }
+
+ return send_status;
+}
+
+unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_buffer,unsigned char *error,unsigned short int *length,unsigned char *data)
+{
+ static unsigned char prev_id,id;
+ static unsigned short int address;
+ unsigned char send_status=0x00;
+
+ if(device!=0x00000000)
+ {
+ id=dyn2_get_id(rx_buffer);
+ if(id==device->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ if(dyn2_check_checksum(rx_buffer)==0x01)// the incomming packet is okay
+ {
+ // process the packet
+ switch(dyn2_get_instruction(rx_buffer))
+ {
+ case DYN_PING: device->on_ping();
+ if(id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_READ: (*error)=device->on_read(dyn2_get_read_address(rx_buffer),dyn2_get_read_length(rx_buffer),data);
+ if(device->return_level!=no_return && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=dyn2_get_read_length(rx_buffer);
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ break;
+ case DYN_WRITE: (*length)=dyn2_get_write_data(rx_buffer,data);
+ (*error)=device->on_write(dyn2_get_write_address(rx_buffer),*length,data);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*length)=0;
+ if((*error)!=DYN_NO_ERROR)
+ (*error)=DYN_INST_ERROR;
+ }
+ break;
+ case DYN_REG_WRITE: device->reg_length=dyn2_get_reg_write_data(rx_buffer,device->reg_buffer);
+ device->reg_address=dyn2_get_reg_write_address(rx_buffer);
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_ACTION: if(device->reg_address!=0xFFFF)
+ {
+ (*error)=device->on_write(device->reg_address,device->reg_length,device->reg_buffer);
+ device->reg_address=0xFFFF;
+ }
+ else
+ if(device->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_NO_ERROR;
+ (*length)=0;
+ }
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(rx_buffer,device->address,&address,length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ (*error)=device->on_read(address,*length,data);
+ send_status=0x01;
+ if((*error)!=DYN_NO_ERROR)
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ device->sync_bulk_address=address;
+ device->sync_bulk_length=*length;
+ device->sync_bulk_prev_id=prev_id;
+ device->sync_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(rx_buffer,device->address,&address,length,data))// the device is addressed
+ (*error)=device->on_write(address,*length,data);
+ break;
+ case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(rx_buffer,device->address,&address,length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ send_status=0x01;
+ (*error)=device->on_read(address,*length,data);
+ if((*error)!=DYN_NO_ERROR)
+ {
+ (*error)=DYN_INST_ERROR;
+ (*length)=0;
+ }
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ device->sync_bulk_address=address;
+ device->sync_bulk_length=*length;
+ device->sync_bulk_prev_id=prev_id;
+ device->bulk_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(rx_buffer,device->address,&address,length,data))
+ (*error)=device->on_write(address,*length,data);
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(rx_buffer)!=DYN_BROADCAST_ID)
+ {
+ send_status=0x01;
+ (*error)=DYN_CHECKSUM_ERROR;
+ (*length)=0;
+ }
+ }
+ }
+ else
+ {
+ if(device->bulk_read_pending)
+ {
+ if(id==device->sync_bulk_prev_id)
+ {
+ device->bulk_read_pending=0x00;
+ (*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=device->sync_bulk_length;
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ }
+ else if(device->sync_read_pending)
+ {
+ if(id==device->sync_bulk_prev_id)
+ {
+ device->sync_read_pending=0x00;
+ (*error)=device->on_read(device->sync_bulk_address,device->sync_bulk_length,data);
+ send_status=0x01;
+ if((*error)==DYN_NO_ERROR)
+ (*length)=device->sync_bulk_length;
+ else
+ {
+ (*length)=0;
+ (*error)=DYN_INST_ERROR;
+ }
+ }
+ }
+ }
+ }
+
+ return send_status;
+}
+
+/* public functions */
+unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char address,unsigned char eeprom_base_address1,unsigned char eeprom_base_address2)
+{
+ /* initialize the internal callbacks */
+ device->on_ping=dummy_dyn_slave_device_on_ping;
+ device->on_read=dummy_dyn_slave_device_on_read;
+ device->on_write=dummy_dyn_slave_device_on_write;
+ device->on_reset=dummy_dyn_slave_device_on_reset;
+ /* initialize internal variables */
+ device->address=address;
+ device->return_delay=0x00;
+ device->return_level=return_all;
+ device->reg_address=0xFFFF;
+ device->reg_length=0x0000;
+ device->sync_bulk_address=0x0000;
+ device->sync_bulk_length=0x0000;
+ device->sync_bulk_prev_id=0x00;
+ device->sync_read_pending=0x00;
+ device->bulk_read_pending=0x00;
+
+ /* initialize memory module */
+ mem_init(&device->memory);
+ mem_module_init(&device->mem_module);
+ device->mem_module.data=device;
+ device->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_device_write_cmd;
+ device->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_device_read_cmd;
+ if(!mem_module_add_eeprom_segment(&device->mem_module,eeprom_base_address1,EEPROM_DYN_SLAVE_LENGTH1))
+ return 0x00;
+ if(!mem_module_add_eeprom_segment(&device->mem_module,eeprom_base_address2,EEPROM_DYN_SLAVE_LENGTH2))
+ return 0x00;
+ if(!mem_add_module(&device->memory,&device->mem_module))
+ return 0x00;
+
+ return 0x01;
+}
+
+void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address)
+{
+ if(device!=0x00000000)
+ device->address=address;
+}
+
+unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
+{
+ if(device!=0x00000000)
+ return device->address;
+ else
+ return 0xFF;
+}
+
+void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay)
+{
+ if(device!=0x00000000)
+ device->return_delay=delay;
+}
+
+unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device)
+{
+ if(device!=0x00000000)
+ return device->return_delay;
+ else
+ return 0xFF;
+}
+
+void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level)
+{
+ if(device!=0x00000000)
+ device->return_level=level;
+}
+
+return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device)
+{
+ if(device!=0x00000000)
+ return device->return_level;
+ else
+ return return_all;
+}