diff --git a/dynamixel_base/Makefile b/dynamixel_base/Makefile index ad6b5947882c2d2a27c4e233946972836be03c20..fef5dc81ca91d8ee5c4e03d28c6f842ab9f5af02 100755 --- a/dynamixel_base/Makefile +++ b/dynamixel_base/Makefile @@ -13,8 +13,9 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3 COMM_PATH = ../comm UTILS_PATH = ../utils +MEMORY_PATH = ../memory -INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include +INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include DOC_DIR = ./doc diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h index 47af3de41b3ca73dfbef33300363ade0ec8a53e0..497678494b2f133b149cda4692506c4af707be48 100644 --- a/dynamixel_base/include/dynamixel_master.h +++ b/dynamixel_base/include/dynamixel_master.h @@ -5,7 +5,6 @@ #include "dynamixel.h" #include "dynamixel2.h" - #ifndef MAX_DYN_MASTER_TX_BUFFER_LEN #define MAX_DYN_MASTER_TX_BUFFER_LEN 1024 #endif diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h index dc6f65e82e22c3239413301a36f7aaa9bbaf19a9..08600215793138be01b2bc230fcb2a2f22b8b4d5 100644 --- a/dynamixel_base/include/dynamixel_slave.h +++ b/dynamixel_base/include/dynamixel_slave.h @@ -4,6 +4,8 @@ #include "comm.h" #include "dynamixel.h" #include "dynamixel2.h" +#include "dynamixel_slave_registers.h" +#include "ram.h" #ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN #define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024 @@ -118,6 +120,11 @@ typedef struct * */ unsigned char bulk_read_pending; + /** + * \brief + * + */ + TMemModule mem_module; /** * \brief * @@ -128,6 +135,11 @@ typedef struct * */ void (*set_rx_mode)(void); + /** + * \brief + * + */ + void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate); /** * \brief * @@ -160,7 +172,12 @@ typedef struct * \brief * */ -void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version); +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 * diff --git a/dynamixel_base/include/dynamixel_slave_registers.h b/dynamixel_base/include/dynamixel_slave_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..8a3a01bd147a1913bc031aba1c32c97a49960954 --- /dev/null +++ b/dynamixel_base/include/dynamixel_slave_registers.h @@ -0,0 +1,45 @@ +#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 RETURN_LEVEL (EEPROM_DYN_SLAVE_BASE_ADDRESS2) + +#ifndef DEFAULT_DEVICE_MODEL + #define DEFAULT_DEVICE_MODEL 0x7300 +#endif +#ifndef DEFAULT_FIRMWARE_VERSION + #define DEFAULT_FIRMWARE_VERSION 0x0001 +#endif +#ifndef DEFAULT_DEVICE_ID + #define DEFAULT_DEVICE_ID 0x0001 +#endif +#ifndef DEFAULT_BAUDRATE + #define DEFAULT_BAUDRATE 0x0010 +#endif +#ifndef DEFAULT_RETURN_DELAY + #define DEFAULT_RETURN_DELAY 0x0000 +#endif + +#ifndef DEFAULT_RETURN_LEVEL + #define DEFAULT_RETURN_LEVEL 0x0002 +#endif + +#endif + diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c index 2f67ebe2db95c447d76a088349061dea2b316529..eb18001bc2560df2627a00445cfe060daf2cc1c3 100755 --- a/dynamixel_base/src/dynamixel.c +++ b/dynamixel_base/src/dynamixel.c @@ -40,17 +40,17 @@ void dyn_copy_packet(unsigned char *source, unsigned char *destination) destination[i]=source[i]; } -inline unsigned char dyn_get_id(unsigned char *packet) +unsigned char dyn_get_id(unsigned char *packet) { return packet[DYN_ID_OFF]; } -inline unsigned char dyn_get_length(unsigned char *packet) +unsigned char dyn_get_length(unsigned char *packet) { return packet[DYN_LENGTH_OFF]; } -inline TDynInstruction dyn_get_instruction(unsigned char *packet) +TDynInstruction dyn_get_instruction(unsigned char *packet) { return packet[DYN_INST_OFF]; } @@ -99,12 +99,12 @@ void dyn_init_read_packet(unsigned char *packet,unsigned char id,unsigned char a dyn_set_checksum(packet); } -inline unsigned char dyn_get_read_length(unsigned char *packet) +unsigned char dyn_get_read_length(unsigned char *packet) { return packet[DYN_DATA_OFF+1]; } -inline unsigned char dyn_get_read_address(unsigned char *packet) +unsigned char dyn_get_read_address(unsigned char *packet) { return packet[DYN_DATA_OFF]; } @@ -125,12 +125,12 @@ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char dyn_set_checksum(packet); } -inline unsigned char dyn_get_write_address(unsigned char *packet) +unsigned char dyn_get_write_address(unsigned char *packet) { return packet[DYN_DATA_OFF]; } -inline unsigned char dyn_get_write_length(unsigned char *packet) +unsigned char dyn_get_write_length(unsigned char *packet) { return packet[DYN_LENGTH_OFF]-3; } @@ -161,12 +161,12 @@ void dyn_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned c dyn_set_checksum(packet); } -inline unsigned char dyn_get_reg_write_address(unsigned char *packet) +unsigned char dyn_get_reg_write_address(unsigned char *packet) { return packet[DYN_DATA_OFF]; } -inline unsigned char dyn_get_reg_write_length(unsigned char *packet) +unsigned char dyn_get_reg_write_length(unsigned char *packet) { return packet[DYN_LENGTH_OFF]-3; } @@ -315,7 +315,7 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err dyn_set_checksum(packet); } -inline TDynError dyn_get_status_error(unsigned char *packet) +TDynError dyn_get_status_error(unsigned char *packet) { return packet[DYN_ERROR_OFF]; } @@ -331,7 +331,7 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data return packet[DYN_LENGTH_OFF]-0x02; } -inline unsigned char dyn_get_read_status_id(unsigned char *packet) +unsigned char dyn_get_read_status_id(unsigned char *packet) { return packet[DYN_ID_OFF]; } diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c index e9e134ab5dffd14d60b407fa4bdffef26fe7b525..bc3bd0fbeea617889221a1c1fb8005e2995da977 100644 --- a/dynamixel_base/src/dynamixel2.c +++ b/dynamixel_base/src/dynamixel2.c @@ -77,17 +77,17 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination) destination[i]=source[i]; } -inline unsigned char dyn2_get_id(unsigned char *packet) +unsigned char dyn2_get_id(unsigned char *packet) { return packet[DYN2_ID_OFF]; } -inline unsigned short int dyn2_get_length(unsigned char *packet) +unsigned short int dyn2_get_length(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_LENGTH_OFF]); } -inline TDynInstruction dyn2_get_instruction(unsigned char *packet) +TDynInstruction dyn2_get_instruction(unsigned char *packet) { return packet[DYN2_INST_OFF]; } @@ -141,12 +141,12 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short dyn2_set_checksum(packet); } -inline unsigned short int dyn2_get_read_length(unsigned char *packet) +unsigned short int dyn2_get_read_length(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_DATA_OFF+2]); } -inline unsigned short int dyn2_get_read_address(unsigned char *packet) +unsigned short int dyn2_get_read_address(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_DATA_OFF]); } @@ -169,12 +169,12 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor dyn2_set_checksum(packet); } -inline unsigned short int dyn2_get_write_address(unsigned char *packet) +unsigned short int dyn2_get_write_address(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_DATA_OFF]); } -inline unsigned short int dyn2_get_write_length(unsigned char *packet) +unsigned short int dyn2_get_write_length(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5; } @@ -208,12 +208,12 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned dyn2_set_checksum(packet); } -inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet) +unsigned short int dyn2_get_reg_write_address(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_DATA_OFF]); } -inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet) +unsigned short int dyn2_get_reg_write_length(unsigned char *packet) { return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5; } @@ -468,7 +468,7 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er dyn2_set_checksum(packet); } -inline TDynError dyn2_get_status_error(unsigned char *packet) +TDynError dyn2_get_status_error(unsigned char *packet) { return packet[DYN2_DATA_OFF]; } @@ -485,7 +485,7 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char return length; } -inline unsigned char dyn2_get_read_status_id(unsigned char *packet) +unsigned char dyn2_get_read_status_id(unsigned char *packet) { return packet[DYN2_ID_OFF]; } diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index 48edf5d94d7796b9c18ee981d07ac85fed4567a6..cb3757420c2021a612d0f3acb13cf9b7b2719a63 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -743,19 +743,19 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version) master->set_rx_mode(); } -inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms) +void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms) { if(master!=0x00000000) master->rx_timeout_ms=timeout_ms; } -inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level) +void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level) { if(master!=0x00000000) master->return_level=level; } -inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master) +return_level_t dyn_master_get_return_level(TDynamixelMaster *master) { if(master!=0x00000000) return master->return_level; @@ -815,7 +815,7 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id) return error; } -inline unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data) +unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data) { return dyn_master_read_table(master,id,address,1,data); } @@ -884,7 +884,7 @@ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,un return error; } -inline unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data) +unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data) { return dyn_master_write_table(master,id,address,1,&data); } diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c index 9f92c1cccbd2ef85cf46b0c09cbcdaf8782d1f04..69a43fa2f67af55c9b46b811b6044b5cadd67fe5 100644 --- a/dynamixel_base/src/dynamixel_slave.c +++ b/dynamixel_base/src/dynamixel_slave.c @@ -1,6 +1,44 @@ #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); +} + unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte) { if(dyn_slave!=0x00000000) @@ -109,6 +147,11 @@ void dummy_dyn_slave_set_rx_mode(void) } +void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + +} + void dummy_dyn_slave_on_ping(void) { @@ -438,7 +481,7 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave) } /* public functions */ -void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version) +unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version) { /* assign communication functions */ dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb; @@ -450,6 +493,7 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn /* initialize the internal callbacks */ 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; @@ -470,24 +514,43 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn slave->sync_read_pending=0x00; slave->bulk_read_pending=0x00; + /* 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; + slave->set_rx_mode(); /* enable reception by IRQ */ comm_receive_irq(slave->comm_dev,0); + + return 0x01; +} + +void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate) +{ + slave->set_baudrate(slave->comm_dev,baudrate); } -inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms) +void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms) { if(slave!=0x00000000) slave->rx_timeout_ms=timeout_ms; } -inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address) +void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address) { if(slave!=0x00000000) slave->address=address; } -inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave) +unsigned char dyn_slave_get_address(TDynamixelSlave *slave) { if(slave!=0x00000000) return slave->address; @@ -495,13 +558,13 @@ inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave) return 0xFF; } -inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay) +void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay) { if(slave!=0x00000000) slave->return_delay=delay; } -inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave) +unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave) { if(slave!=0x00000000) return slave->return_delay; @@ -509,13 +572,13 @@ inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave) return 0xFF; } -inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level) +void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level) { if(slave!=0x00000000) slave->return_level=level; } -inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave) +return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave) { if(slave!=0x00000000) return slave->return_level; @@ -523,7 +586,7 @@ inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave) return return_all; } -inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave) +TDynVersion dyn_slave_get_version(TDynamixelSlave *slave) { if(slave!=0x00000000) return slave->version;