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

Completed the implementation of the multi slave interface. Needs testing.

parent b6469f50
No related branches found
No related tags found
1 merge request!7Multi slave
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 715 additions and 544 deletions
dynamixel_base/Makefile
+ 8
− 5
View file @ 804743e4
...@@ -14,8 +14,11 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3 ...@@ -14,8 +14,11 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3
COMM_PATH = ../comm COMM_PATH = ../comm
UTILS_PATH = ../utils UTILS_PATH = ../utils
MEMORY_PATH = ../memory 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 DOC_DIR = ./doc
...@@ -53,13 +56,13 @@ DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TM ...@@ -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) all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3)
$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c $(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 $(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 $(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 $(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_build:
mkdir -p build/m4_fpu mkdir -p build/m4_fpu
mkdir -p build/m0 mkdir -p build/m0
......
dynamixel_base/include/dynamixel_master.h
+ 2
− 2
View file @ 804743e4
...@@ -6,11 +6,11 @@ ...@@ -6,11 +6,11 @@
#include "dynamixel2.h" #include "dynamixel2.h"
#ifndef MAX_DYN_MASTER_TX_BUFFER_LEN #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 #endif
#ifndef MAX_DYN_MASTER_RX_BUFFER_LEN #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 #endif
/** /**
......
dynamixel_base/include/dynamixel_slave.h
+ 17
− 107
View file @ 804743e4
...@@ -4,52 +4,47 @@ ...@@ -4,52 +4,47 @@
#include "comm.h" #include "comm.h"
#include "dynamixel.h" #include "dynamixel.h"
#include "dynamixel2.h" #include "dynamixel2.h"
#include "dynamixel_slave_registers.h" #include "scheduler.h"
#include "ram.h" #include "dynamixel_slave_device.h"
#ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN #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 #endif
#ifndef MAX_DYN_SLAVE_RX_BUFFER_LEN #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 #endif
#ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN #ifndef MAX_NUM_SLAVE_DEVICES
#define MAX_DYN_SLAVE_REG_BUFFER_LEN 256 #error "Please, specify the maximum number of slave devices with the MAX_NUM_SLAVE_DEVICES macro"
#endif #endif
/** /**
* \brief * \brief
* *
*/ */
typedef struct typedef struct TDynamixelSlave
{ {
/** /**
* \brief * \brief
* *
*/ */
TComm *comm_dev; TScheduler *scheduler;
/**
* \brief
*
*/
TDynVersion version;
/** /**
* \brief * \brief
* *
*/ */
unsigned char address; sched_channel_t sch_channel;
/** /**
* \brief * \brief
* *
*/ */
unsigned char return_delay; TComm *comm_dev;
/** /**
* \brief * \brief
* *
*/ */
return_level_t return_level; TDynVersion version;
/** /**
* \brief * \brief
* *
...@@ -84,47 +79,12 @@ typedef struct ...@@ -84,47 +79,12 @@ typedef struct
* \brief * \brief
* *
*/ */
unsigned short int reg_address; unsigned char num_slave_devices;
/**
* \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 * \brief
* *
*/ */
unsigned short int sync_bulk_length; TDynamixelSlaveDevice *slave_devices[MAX_NUM_SLAVE_DEVICES];
/**
* \brief
*
*/
unsigned char sync_bulk_prev_id;
/**
* \brief
*
*/
unsigned char sync_read_pending;
/**
* \brief
*
*/
unsigned char bulk_read_pending;
/**
* \brief
*
*/
TMemModule mem_module;
/** /**
* \brief * \brief
* *
...@@ -140,26 +100,6 @@ typedef struct ...@@ -140,26 +100,6 @@ typedef struct
* *
*/ */
void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate); 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 * \brief
* *
...@@ -172,57 +112,27 @@ typedef struct ...@@ -172,57 +112,27 @@ typedef struct
* \brief * \brief
* *
*/ */
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);
/**
* \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);
/** /**
* \brief * \brief
* *
*/ */
inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay); unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice *device);
/** /**
* \brief * \brief
* *
*/ */
inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave); void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
/**
* \brief
*
*/
inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level);
/** /**
* \brief * \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 * \brief
* *
*/ */
inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave); inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave);
/**
* \brief
*
*/
void dyn_slave_loop(TDynamixelSlave *slave);
#endif #endif
dynamixel_base/include/dynamixel_slave_device.h 0 → 100644
+ 159
− 0
View file @ 804743e4
#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
dynamixel_base/include/dynamixel_slave_registers.h
+ 16
− 14
View file @ 804743e4
#ifndef _DYNAMIXEL_SLAVE_REGISTERS_H #ifndef _DYNAMIXEL_SLAVE_REGISTERS_H
#define _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 #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 EEPROM_DYN_SLAVE_LENGTH2 1
#define DEVICE_MODEL (EEPROM_DYN_SLAVE_BASE_ADDRESS1) #define DEVICE_MODEL 0
#define FIRMWARE_VERSION (EEPROM_DYN_SLAVE_BASE_ADDRESS1+2) #define FIRMWARE_VERSION 2
#define DEVICE_ID (EEPROM_DYN_SLAVE_BASE_ADDRESS1+3) #define DEVICE_ID 3
#define BAUDRATE (EEPROM_DYN_SLAVE_BASE_ADDRESS1+4) #define BAUDRATE 4
#define RETURN_DELAY (EEPROM_DYN_SLAVE_BASE_ADDRESS1+5) #define RETURN_DELAY 5
#define RETURN_LEVEL (EEPROM_DYN_SLAVE_BASE_ADDRESS2) #define RETURN_LEVEL 0
#ifndef DEFAULT_DEVICE_MODEL #ifndef DEFAULT_DEVICE_MODEL
#define DEFAULT_DEVICE_MODEL 0x7300 #define DEFAULT_DEVICE_MODEL 0x7300
...@@ -41,5 +33,15 @@ ...@@ -41,5 +33,15 @@
#define DEFAULT_RETURN_LEVEL 0x0002 #define DEFAULT_RETURN_LEVEL 0x0002
#endif #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 #endif
dynamixel_base/src/dynamixel_slave.c
+ 63
− 416
View file @ 804743e4
This diff is collapsed.
dynamixel_base/src/dynamixel_slave_device.c 0 → 100644
+ 450
− 0
View file @ 804743e4
#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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment