diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h
index 29b35c00bcd38f3de34e17a61442c7e40297be3e..2b06e605e40e14a2ec8821a79cb50d0acd84f076 100755
--- a/dynamixel_base/include/dynamixel.h
+++ b/dynamixel_base/include/dynamixel.h
@@ -23,7 +23,7 @@ void dyn_copy_packet(unsigned char *source, unsigned char *destination);
* \brief
*
*/
-inline unsigned char dyn_get_id(unsigned char *packet)
+static inline unsigned char dyn_get_id(unsigned char *packet)
{
return packet[DYN_ID_OFF];
}
@@ -31,7 +31,7 @@ inline unsigned char dyn_get_id(unsigned char *packet)
* \brief
*
*/
-inline unsigned char dyn_get_length(unsigned char *packet)
+static inline unsigned char dyn_get_length(unsigned char *packet)
{
return packet[DYN_LENGTH_OFF];
}
@@ -39,7 +39,7 @@ inline unsigned char dyn_get_length(unsigned char *packet)
* \brief
*
*/
-inline TDynInstruction dyn_get_instruction(unsigned char *packet)
+static inline TDynInstruction dyn_get_instruction(unsigned char *packet)
{
return (TDynInstruction)packet[DYN_INST_OFF];
}
@@ -81,7 +81,7 @@ void dyn_init_read_packet(unsigned char *packet,unsigned char id,unsigned char a
* \brief
*
*/
-inline unsigned char dyn_get_read_length(unsigned char *packet)
+static inline unsigned char dyn_get_read_length(unsigned char *packet)
{
return packet[DYN_DATA_OFF+1];
}
@@ -89,7 +89,7 @@ inline unsigned char dyn_get_read_length(unsigned char *packet)
* \brief
*
*/
-inline unsigned char dyn_get_read_address(unsigned char *packet)
+static inline unsigned char dyn_get_read_address(unsigned char *packet)
{
return packet[DYN_DATA_OFF];
}
@@ -103,7 +103,7 @@ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char
* \brief
*
*/
-inline unsigned char dyn_get_write_address(unsigned char *packet)
+static inline unsigned char dyn_get_write_address(unsigned char *packet)
{
return packet[DYN_DATA_OFF];
}
@@ -111,7 +111,7 @@ inline unsigned char dyn_get_write_address(unsigned char *packet)
* \brief
*
*/
-inline unsigned char dyn_get_write_length(unsigned char *packet)
+static inline unsigned char dyn_get_write_length(unsigned char *packet)
{
return packet[DYN_LENGTH_OFF]-3;
}
@@ -130,7 +130,7 @@ void dyn_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned c
* \brief
*
*/
-inline unsigned char dyn_get_reg_write_address(unsigned char *packet)
+static inline unsigned char dyn_get_reg_write_address(unsigned char *packet)
{
return packet[DYN_DATA_OFF];
}
@@ -138,7 +138,7 @@ inline unsigned char dyn_get_reg_write_address(unsigned char *packet)
* \brief
*
*/
-inline unsigned char dyn_get_reg_write_length(unsigned char *packet)
+static inline unsigned char dyn_get_reg_write_length(unsigned char *packet)
{
return packet[DYN_LENGTH_OFF]-3;
}
@@ -208,7 +208,7 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err
* \brief
*
*/
-inline TDynError dyn_get_status_error(unsigned char *packet)
+static inline TDynError dyn_get_status_error(unsigned char *packet)
{
return (TDynError)packet[DYN_ERROR_OFF];
}
@@ -222,7 +222,7 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data
* \brief
*
*/
-inline unsigned char dyn_get_read_status_id(unsigned char *packet)
+static inline unsigned char dyn_get_read_status_id(unsigned char *packet)
{
return packet[DYN_ID_OFF];
}
diff --git a/dynamixel_base/include/dynamixel2.h b/dynamixel_base/include/dynamixel2.h
index f4303061ff52d4fde5b34c664ebad18f47cadd41..8b6b3d440aecb4321f5a5d6beedc4a15841431f8 100644
--- a/dynamixel_base/include/dynamixel2.h
+++ b/dynamixel_base/include/dynamixel2.h
@@ -24,7 +24,7 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination);
* \brief
*
*/
-inline unsigned char dyn2_get_id(unsigned char *packet)
+static inline unsigned char dyn2_get_id(unsigned char *packet)
{
return packet[DYN2_ID_OFF];
}
@@ -32,7 +32,7 @@ inline unsigned char dyn2_get_id(unsigned char *packet)
* \brief
*
*/
-inline unsigned short int dyn2_get_length(unsigned char *packet)
+static inline unsigned short int dyn2_get_length(unsigned char *packet)
{
return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256);
}
@@ -40,7 +40,7 @@ inline unsigned short int dyn2_get_length(unsigned char *packet)
* \brief
*
*/
-inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
+static inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
{
return (TDynInstruction)packet[DYN2_INST_OFF];
}
@@ -82,7 +82,7 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short
* \brief
*
*/
-inline unsigned short int dyn2_get_read_length(unsigned char *packet)
+static inline unsigned short int dyn2_get_read_length(unsigned char *packet)
{
return (packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256);
}
@@ -90,7 +90,7 @@ inline unsigned short int dyn2_get_read_length(unsigned char *packet)
* \brief
*
*/
-inline unsigned short int dyn2_get_read_address(unsigned char *packet)
+static inline unsigned short int dyn2_get_read_address(unsigned char *packet)
{
return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
}
@@ -104,7 +104,7 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor
* \brief
*
*/
-inline unsigned short int dyn2_get_write_address(unsigned char *packet)
+static inline unsigned short int dyn2_get_write_address(unsigned char *packet)
{
return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
}
@@ -112,7 +112,7 @@ inline unsigned short int dyn2_get_write_address(unsigned char *packet)
* \brief
*
*/
-inline unsigned short int dyn2_get_write_length(unsigned char *packet)
+static inline unsigned short int dyn2_get_write_length(unsigned char *packet)
{
return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
}
@@ -131,7 +131,7 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned
* \brief
*
*/
-inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
+static inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
{
return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
}
@@ -139,7 +139,7 @@ inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
* \brief
*
*/
-inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
+static inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
{
return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
}
@@ -231,7 +231,7 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er
* \brief
*
*/
-inline TDynError dyn2_get_status_error(unsigned char *packet)
+static inline TDynError dyn2_get_status_error(unsigned char *packet)
{
return (TDynError)packet[DYN2_DATA_OFF];
}
@@ -245,7 +245,7 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char
* \brief
*
*/
-inline unsigned char dyn2_get_read_status_id(unsigned char *packet)
+static inline unsigned char dyn2_get_read_status_id(unsigned char *packet)
{
return packet[DYN2_ID_OFF];
}
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index f1fdf2e6fed4c8daa094bb9f2324ba1191768ff4..3c7f0470418d0303be47617347f68b0ed145fb4c 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -100,7 +100,7 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version);
* \brief
*
*/
-inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
+static inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
{
if(master!=0x00000000)
master->rx_timeout_ms=timeout_ms;
@@ -109,7 +109,7 @@ inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short in
* \brief
*
*/
-inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
+static inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
{
if(master!=0x00000000)
master->return_level=level;
@@ -118,7 +118,7 @@ inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t
* \brief
*
*/
-inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
+static inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
{
if(master!=0x00000000)
return master->return_level;
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 067ad7ee7fd164a9c212ccf8502eae5e534ccd31..c680e17aff39e41b2d9005eb3136cff4f7d4e180 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -126,7 +126,7 @@ unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice
* \brief
*
*/
-inline void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
+static inline void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
{
if(slave!=0x00000000)
slave->set_baudrate(slave->comm_dev,baudrate);
@@ -135,7 +135,7 @@ inline 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)
+static inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
{
if(slave!=0x00000000)
slave->rx_timeout_ms=timeout_ms;
@@ -144,7 +144,7 @@ inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int t
* \brief
*
*/
-inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
+static inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
{
if(slave!=0x00000000)
return slave->version;
@@ -155,7 +155,7 @@ inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
* \brief
*
*/
-inline void dyn_slave_start(TDynamixelSlave *slave)
+static inline void dyn_slave_start(TDynamixelSlave *slave)
{
scheduler_enable_channel(slave->scheduler,slave->sch_channel);
}
@@ -163,7 +163,7 @@ inline void dyn_slave_start(TDynamixelSlave *slave)
* \brief
*
*/
-inline void dyn_slave_stop(TDynamixelSlave *slave)
+static inline void dyn_slave_stop(TDynamixelSlave *slave)
{
scheduler_disable_channel(slave->scheduler,slave->sch_channel);
}
diff --git a/dynamixel_base/include/dynamixel_slave_device.h b/dynamixel_base/include/dynamixel_slave_device.h
index 34b28e1daf6fef7d317a3b7de74e66767baa7920..aa7617bd3597beff62bb39baa10c2740aa500e8b 100644
--- a/dynamixel_base/include/dynamixel_slave_device.h
+++ b/dynamixel_base/include/dynamixel_slave_device.h
@@ -132,7 +132,7 @@ unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char
* \brief
*
*/
-inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address)
+static inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address)
{
if(device!=0x00000000)
device->address=address;
@@ -141,7 +141,7 @@ inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned
* \brief
*
*/
-inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
+static inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
{
if(device!=0x00000000)
return device->address;
@@ -152,7 +152,7 @@ inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
* \brief
*
*/
-inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay)
+static inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay)
{
if(device!=0x00000000)
device->return_delay=delay;
@@ -161,7 +161,7 @@ inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsi
* \brief
*
*/
-inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device)
+static inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device)
{
if(device!=0x00000000)
return device->return_delay;
@@ -172,7 +172,7 @@ inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *de
* \brief
*
*/
-inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level)
+static inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level)
{
if(device!=0x00000000)
device->return_level=level;
@@ -181,7 +181,7 @@ inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,retu
* \brief
*
*/
-inline return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device)
+static inline return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device)
{
if(device!=0x00000000)
return device->return_level;
@@ -202,7 +202,7 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
* \brief
*
*/
-inline TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device)
+static inline TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device)
{
return &device->memory;
}
diff --git a/dynamixel_base/include/dynamixel_slave_registers.h b/dynamixel_base/include/dynamixel_slave_registers.h
index 46cef2b79342753136e2514c7ce41c79383306a1..40f60b49385636e2d818fc5562af66efefb639b8 100644
--- a/dynamixel_base/include/dynamixel_slave_registers.h
+++ b/dynamixel_base/include/dynamixel_slave_registers.h
@@ -14,13 +14,15 @@
#define RETURN_LEVEL 0
#define dyn_slave_control_eeprom_data(name,section_name,base_address1,base_address2,DEFAULT_DEVICE_MODEL,DEFAULT_FIRMWARE_VERSION,DEFAULT_DEVICE_ID,DEFAULT_BAUDRATE,DEFAULT_RETURN_DELAY,DEFAULT_RETURN_LEVEL) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_DEVICE_MODEL&0x00FF,(DEFAULT_DEVICE_ID<<8) + base_address1+DEVICE_MODEL, \
- (DEFAULT_DEVICE_MODEL>>8)&0x00FF,(DEFAULT_DEVICE_ID<<8) + base_address1+DEVICE_MODEL+1, \
- DEFAULT_FIRMWARE_VERSION,(DEFAULT_DEVICE_ID<<8) + base_address1+FIRMWARE_VERSION, \
- DEFAULT_DEVICE_ID,(DEFAULT_DEVICE_ID<<8) + base_address1+DEVICE_ID, \
- DEFAULT_BAUDRATE,(DEFAULT_DEVICE_ID<<8) + base_address1+BAUDRATE, \
- DEFAULT_RETURN_DELAY,(DEFAULT_DEVICE_ID<<8) + base_address1+RETURN_DELAY, \
- DEFAULT_RETURN_LEVEL,(DEFAULT_DEVICE_ID<<8) + base_address2+RETURN_LEVEL};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))= {\
+ 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_device.c b/dynamixel_base/src/dynamixel_slave_device.c
index 25ae4dfa993643d1b031add57f913495caace089..3d792dc043937e39ce47630f8ed16f8b8c868ee4 100644
--- a/dynamixel_base/src/dynamixel_slave_device.c
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -403,7 +403,7 @@ unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char
device->bulk_read_pending=0x00;
/* initialize memory module */
- mem_init(&device->memory,address);
+ 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;
diff --git a/dynamixel_manager/Makefile b/dynamixel_manager/Makefile
index 020c8182f704eec57c52c1efec5a4c7590ff25d3..1eb0622aba6cb3ccd78e5a4df4867c0c583003d3 100755
--- a/dynamixel_manager/Makefile
+++ b/dynamixel_manager/Makefile
@@ -15,8 +15,9 @@ COMM_PATH = ../comm
UTILS_PATH = ../utils
DYN_BASE_PATH = ../dynamixel_base
MEMORY_PATH = ../memory
+SCHEDULER_PATH = ../scheduler
-INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include
+INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include -I$(SCHEDULER_PATH)/include
MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8
@@ -35,8 +36,10 @@ DYNAMIXEL_OUT_M3 = ./lib/dynamixel_manager_m3.a
SRC_DIR=./src/
SRC=$(wildcard $(SRC_DIR)*.c)
-#SRC_DIR_MODULES=./src/modules/
-#SRC+=$(wildcard $(SRC_DIR_MODULES)*.c)
+SRC_DIR_MODULES=./src/modules/
+SRC+=$(wildcard $(SRC_DIR_MODULES)*.c)
+SRC_DIR_SCHEDULER=$(SCHEDULER_PATH)/src
+SRC+=$(wildcard $(SRC_DIR_SCHEDULER)*.c)
DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/
DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o))
diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h
index 948c040cb0bb4e0ae4dd4083bc276cc1881fd925..2860e0ceb0d8171eac3301e512d87df7c28aca57 100644
--- a/dynamixel_manager/include/dyn_manager.h
+++ b/dynamixel_manager/include/dyn_manager.h
@@ -8,6 +8,7 @@ extern "C" {
#include "dyn_manager_registers.h"
#include "dynamixel_master.h"
#include "memory.h"
+#include "scheduler.h"
#ifndef DYN_MANAGER_MAX_NUM_MASTERS
#error "Please, specify the maximum number of masters with the DYN_MANAGER_MAX_NUM_MASTERS macro"
@@ -116,24 +117,42 @@ typedef struct{
unsigned char num_devices;
OP_HANDLE op_handles[DYN_MANAGER_MAX_NUM_OP];
unsigned char num_ops;
- unsigned short int period_us;
- void (*init_timer)(void);
- void (*set_period)(unsigned short int period_us);
+ unsigned char period_ms;
+ TScheduler *scheduler;
+ sched_channel_t sch_loop_ch;
+ sched_channel_t sch_scan_ch;
TMemory *memory;
TMemModule mem_module;
+ unsigned char running;
+ unsigned int present_devices;
+ volatile unsigned char lock;
}TDynManager;
// public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned short int eeprom_base_address,unsigned short int ram_base_address);
-void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us);
-unsigned short int dyn_manager_get_period(TDynManager *manager);
+unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t loop_ch,sched_channel_t scan_ch,unsigned short int eeprom_base_address,unsigned short int ram_base_address);
+void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms);
+static inline unsigned char dyn_manager_get_period(TDynManager *manager)
+{
+ return manager->period_ms;
+}
+void dyn_manager_start(TDynManager *manager);
+void dyn_manager_stop(TDynManager *manager);
void dyn_manager_scan(TDynManager *manager);
void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master);
-unsigned char dyn_manager_get_num_masters(TDynManager *manager);
+static inline unsigned char dyn_manager_get_num_masters(TDynManager *manager)
+{
+ return manager->num_masters;
+}
TDynamixelMaster *dyn_manager_get_device_master(TDynManager *manager,unsigned char device_id);
-unsigned short int dyn_manager_get_device_model(TDynManager *manager,unsigned char device_id);
+static inline unsigned short int dyn_manager_get_device_model(TDynManager *manager,unsigned char device_id)
+{
+ return manager->devices[device_id].model;
+}
void dyn_manager_add_module(TDynManager *manager,struct TDynModule *module);
-unsigned char dyn_manager_get_num_modules(TDynManager *manager);
+static inline unsigned char dyn_manager_get_num_modules(TDynManager *manager)
+{
+ return manager->num_modules;
+}
void dyn_manager_delete_op(TDynManager *manager,OP_HANDLE *op);
void dyn_manager_set_op_period(TDynManager *manager,OP_HANDLE *op,unsigned char period_count);
unsigned char dyn_manager_get_op_period(TDynManager *manager,OP_HANDLE *op);
diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h
index c7d360be6c57731f044ba572f5d6af9b17b51455..c7e37603271f9a28d6f40e92dea1048af36cecb6 100644
--- a/dynamixel_manager/include/dyn_manager_registers.h
+++ b/dynamixel_manager/include/dyn_manager_registers.h
@@ -1,18 +1,29 @@
#ifndef _DYN_MANAGER_REGISTERS_H
#define _DYN_MANAGER_REGISTERS_H
-#define RAM_DYN_MANAGER_LENGTH 2
+#define RAM_DYN_MANAGER_LENGTH 8
-#define EEPROM_DYN_MANAGER_LENGTH 2
+#define DYN_MANAGER_NUM_MODULES_OFFSET 0
+#define DYN_MANAGER_NUM_MASTERS_OFFSET 1
+#define DYN_MANAGER_CONTROL_OFFSET 2 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // scanning | running | | | | start scan | stop | start
+#define DYN_MANAGER_START 0x01
+#define DYN_MANAGER_STOP 0x02
+#define DYN_MANAGER_START_SCAN 0x04
+#define DYN_MANAGER_RUNNING 0x40
+#define DYN_MANAGER_SCANNING 0x80
-#define DYN_MANAGER_PERIOD 0
+#define DYN_MANAGER_NUM_DEVICES_OFFSET 3
+#define DYN_MANAGER_PRESENT_DEVICES 4
-#define DYN_MANAGER_NUM_MODULES 0
-#define DYN_MANAGER_NUM_MASTERS 1
+#define EEPROM_DYN_MANAGER_LENGTH 1
+
+#define DYN_MANAGER_PERIOD_OFFSET 0
#define dyn_manager_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD, \
- (DEFAULT_PERIOD>>8)&0x00FF,base_address+DYN_MANAGER_PERIOD+1};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+ DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD_OFFSET \
+};
#endif
diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h
index 77c5ce1e462af9ce4fb367b943413377732c401a..9291e3470a0452da3802b640186d6751b44fe4fa 100644
--- a/dynamixel_manager/include/dyn_module.h
+++ b/dynamixel_manager/include/dyn_module.h
@@ -21,26 +21,47 @@ typedef struct TDynModule{
unsigned char period_count;
unsigned char enabled;
void (*add_device)(void *module_data,unsigned char id,unsigned short int model);
- void (*set_period)(void *module_data,unsigned short int period_us);
+ void (*set_period)(void *module_data,unsigned short int period_ms);
void (*setup)(void *module_data);
void (*pre_process)(void *module_data);
void (*post_process)(void *module_data);
void *data;
- TMemModule *mem_module;
+ TMemory *memory;
+ TMemModule mem_module;
+ unsigned short int ram_base_address;
+ unsigned short int eeprom_base_address;
}TDynModule;
//public functions
-unsigned char dyn_module_init(TDynModule *module);
-inline void dyn_module_enable(TDynModule *module);
-inline void dyn_module_disable(TDynModule *module);
-inline unsigned char dyn_module_is_enabled(TDynModule *module);
+unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short int ram_base_address, unsigned short int eeprom_base_address);
+static inline void dyn_module_enable(TDynModule *module)
+{
+ module->enabled=0x01;
+}
+static inline void dyn_module_disable(TDynModule *module)
+{
+ module->enabled=0x00;
+}
+static inline unsigned char dyn_module_is_enabled(TDynModule *module)
+{
+ return module->enabled;
+}
void dyn_module_add_model(TDynModule *module,unsigned short int model);
-inline unsigned char dyn_module_get_num_models(TDynModule *module);
+static inline unsigned char dyn_module_get_num_models(TDynModule *module)
+{
+ return module->num_models;
+}
void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model);
-inline unsigned char dyn_module_get_num_assigned_devices(TDynModule *module);
+static inline unsigned char dyn_module_get_num_assigned_devices(TDynModule *module)
+{
+ return module->num_assigned_ids;
+}
unsigned char dyn_module_device_is_assigned(TDynModule *module,unsigned char id);
void dyn_module_set_period(TDynModule *module,unsigned char period);
-inline unsigned char dyn_module_get_period(TDynModule *module);
+static inline unsigned char dyn_module_get_period(TDynModule *module)
+{
+ return module->period_count;
+}
#ifdef __cplusplus
}
diff --git a/dynamixel_manager/include/dyn_module_registers.h b/dynamixel_manager/include/dyn_module_registers.h
index fe27dbb309ebefbe6eb880dd716b8ce44518836e..4e1fc4477f7062f637eaf3f45850ef8670de8266 100644
--- a/dynamixel_manager/include/dyn_module_registers.h
+++ b/dynamixel_manager/include/dyn_module_registers.h
@@ -1,18 +1,19 @@
#ifndef _DYN_MODULE_REGISTERS_H
#define _DYN_MODULE_REGISTERS_H
-#define RAM_DYN_MODULE_LENGTH 3
+#define RAM_DYN_MODULE_LENGTH 2
-#define EEPROM_DYN_MODULE_LENGTH 1
+#define DYN_MODULE_NUM_MODELS_OFFSET 0
+#define DYN_MODULE_NUM_DEVICES_OFFSET 1
-#define DYN_MODULE_PERIOD 0
+#define EEPROM_DYN_MODULE_LENGTH 1
-#define DYN_MODULE_CNTRL 0
-#define DYN_MODULE_NUM_MODELS 1
-#define DYN_MODULE_NUM_DEVICES 2
+#define DYN_MODULE_PERIOD_OFFSET 0
#define dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+ DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD_OFFSET \
+};
#endif
diff --git a/dynamixel_manager/include/modules/action.h b/dynamixel_manager/include/modules/action.h
index 506b11d5ab837a58f7d08b45041c49cf0994aeb2..309c50ccca35e54327abf3c241ff714a3874ff73 100755
--- a/dynamixel_manager/include/modules/action.h
+++ b/dynamixel_manager/include/modules/action.h
@@ -7,16 +7,62 @@ extern "C" {
#include "motion_pages.h"
#include "motion_module.h"
+#include "motion_pages.h"
+#include "memory.h"
+
+typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
+
+typedef struct{
+ TMotionModule mmodule;
+ TMemory *memory;
+ TMemModule mem_module;
+ unsigned short int ram_base_address;
+ unsigned short int eeprom_base_address;
+ TPage next_page;
+ TPage current_page;
+ unsigned char current_step_index;
+ unsigned char current_page_index;
+ // angle variables
+ long long int moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // speed variables
+ long long int start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // control variables
+ unsigned char end;
+ unsigned char stop;
+ unsigned char zero_speed_finish[PAGE_MAX_NUM_SERVOS];
+ unsigned char next_index;
+ unsigned char running;
+ // time variables (in time units (7.8 ms each time unit))
+ long long int total_time;// fixed point 48|16 format
+ long long int pre_time;// fixed point 48|16 format
+ long long int main_time;// fixed point 48|16 format
+ long long int step_time;// fixed point 48|16 format
+ long long int pause_time;// fixed point 48|16 format
+ long long int current_time;// fixed point 48|16 format
+ long long int section_time;// fixed point 48|16 format
+ long long int period;
+ // angle variables
+ long long int accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // speed variables (in controller units)
+ long long int current_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ long long int delta_speed;
+ char num_repetitions;
+ // control variables
+ action_states state;
+}TActionMModule;
// public functions
-void action_init(void);
-TMotionModule *action_get_module(void);
-unsigned char action_load_page(unsigned char page_id);
-void action_start_page(void);
-void action_stop_page(void);
-unsigned char action_is_running(void);
-unsigned char action_get_current_page(void);
-unsigned char action_get_current_step(void);
+unsigned char action_init(TActionMModule *action,TMemory *memory,unsigned short int ram_base_address);
+TMotionModule *action_get_module(TActionMModule *action);
+unsigned char action_load_page(TActionMModule *action,unsigned char page_id);
+void action_start_page(TActionMModule *action);
+void action_stop_page(TActionMModule *action);
+unsigned char action_is_running(TActionMModule *action);
+unsigned char action_get_current_page(TActionMModule *action);
+unsigned char action_get_current_step(TActionMModule *action);
#ifdef __cplusplus
}
diff --git a/dynamixel_manager/include/modules/action_mm_registers.h b/dynamixel_manager/include/modules/action_mm_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..847f0534efaf38bafcf6ac2c3b8e00ee1fec57e9
--- /dev/null
+++ b/dynamixel_manager/include/modules/action_mm_registers.h
@@ -0,0 +1,13 @@
+#ifndef _ACTION_MM_REGISTERS_H
+#define _ACTION_MM_REGISTERS_H
+
+#define RAM_ACTION_MM_LENGTH 2
+
+#define ACTION_MM_CONTROL_OFFSET 0
+#define ACTION_MM_PAGE_OFFSET 1
+ #define ACTION_MM_START 0x01
+ #define ACTION_MM_STOP 0x02
+ #define ACTION_MM_RUNNING 0x80
+
+#endif
+
diff --git a/dynamixel_manager/include/modules/dyn_servos.h b/dynamixel_manager/include/modules/dyn_servos.h
index 07d4e55521b0279fda66c9fe001c91c7623ce551..f3dc02d85a8e1f20bfe293e8d8e9bfc336db1db7 100755
--- a/dynamixel_manager/include/modules/dyn_servos.h
+++ b/dynamixel_manager/include/modules/dyn_servos.h
@@ -1,125 +1,107 @@
#ifndef _DYN_SERVOS_H
#define _DYN_SERVOS_H
-// Servo register map
-typedef enum{
- P_MODEL_NUMBER_L = 0,
- P_MODEL_NUMBER_H = 1,
- P_VERSION = 2,
- P_ID = 3,
- P_BAUD_RATE = 4,
- P_RETURN_DELAY_TIME = 5,
- P_CW_ANGLE_LIMIT_L = 6,
- P_CW_ANGLE_LIMIT_H = 7,
- P_CCW_ANGLE_LIMIT_L = 8,
- P_CCW_ANGLE_LIMIT_H = 9,
- P_SYSTEM_DATA2 = 10,
- P_HIGH_LIMIT_TEMPERATURE = 11,
- P_LOW_LIMIT_VOLTAGE = 12,
- P_HIGH_LIMIT_VOLTAGE = 13,
- P_MAX_TORQUE_L = 14,
- P_MAX_TORQUE_H = 15,
- P_RETURN_LEVEL = 16,
- P_ALARM_LED = 17,
- P_ALARM_SHUTDOWN = 18,
- P_OPERATING_MODE = 19,
- P_LOW_CALIBRATION_L = 20,
- P_LOW_CALIBRATION_H = 21,
- P_HIGH_CALIBRATION_L = 22,
- P_HIGH_CALIBRATION_H = 23,
- P_TORQUE_ENABLE = 24,
- P_LED = 25,
- P_CW_COMPLIANCE_MARGIN = 26,
- P_CCW_COMPLIANCE_MARGIN = 27,
- P_CW_COMPLIANCE_SLOPE = 28,
- P_CCW_COMPLIANCE_SLOPE = 29,
- P_D_GAIN = 26,
- P_I_GAIN = 27,
- P_P_GAIN = 28,
- P_RESERVED = 29,
- P_GOAL_POSITION_L = 30,
- P_GOAL_POSITION_H = 31,
- P_MOVING_SPEED_L = 32,
- P_MOVING_SPEED_H = 33,
- P_TORQUE_LIMIT_L = 34,
- P_TORQUE_LIMIT_H = 35,
- P_PRESENT_POSITION_L = 36,
- P_PRESENT_POSITION_H = 37,
- P_PRESENT_SPEED_L = 38,
- P_PRESENT_SPEED_H = 39,
- P_PRESENT_LOAD_L = 40,
- P_PRESENT_LOAD_H = 41,
- P_PRESENT_VOLTAGE = 42,
- P_PRESENT_TEMPERATURE = 43,
- P_REGISTERED_INSTRUCTION = 44,
- P_PAUSE_TIME = 45,
- P_MOVING = 46,
- P_LOCK = 47,
- P_PUNCH_L = 48,
- P_PUNCH_H = 49,
- P_RESERVED4 = 50,
- P_RESERVED5 = 51,
- P_POT_L = 52,
- P_POT_H = 53,
- P_PWM_OUT_L = 54,
- P_PWM_OUT_H = 55,
- P_P_ERROR_L = 56,
- P_P_ERROR_H = 57,
- P_I_ERROR_L = 58,
- P_I_ERROR_H = 59,
- P_D_ERROR_L = 60,
- P_D_ERROR_H = 61,
- P_P_ERROR_OUT_L = 62,
- P_P_ERROR_OUT_H = 63,
- P_I_ERROR_OUT_L = 64,
- P_I_ERROR_OUT_H = 65,
- P_D_ERROR_OUT_L = 66,
- P_D_ERROR_OUT_H = 67}TDynServo;
+#define NUM_REG 62
-typedef enum{
- XL_MODEL_NUMBER_L = 0,
- XL_MODEL_NUMBER_H = 1,
- XL_VERSION = 2,
- XL_ID = 3,
- XL_BAUD_RATE = 4,
- XL_RETURN_DELAY_TIME = 5,
- XL_CW_ANGLE_LIMIT_L = 6,
- XL_CW_ANGLE_LIMIT_H = 7,
- XL_CCW_ANGLE_LIMIT_L = 8,
- XL_CCW_ANGLE_LIMIT_H = 9,
- XL_SYSTEM_DATA2 = 10,
- XL_CONTROL_MODE = 11,
- XL_HIGH_LIMIT_TEMPERATURE = 12,
- XL_LOW_LIMIT_VOLTAGE = 13,
- XL_HIGH_LIMIT_VOLTAGE = 14,
- XL_MAX_TORQUE_L = 15,
- XL_MAX_TORQUE_H = 16,
- XL_RETURN_LEVEL = 17,
- XL_ALARM_SHUTDOWN = 18,
- XL_TORQUE_ENABLE = 24,
- XL_LED = 25,
- XL_D_GAIN = 27,
- XL_I_GAIN = 28,
- XL_P_GAIN = 29,
- XL_GOAL_POSITION_L = 30,
- XL_GOAL_POSITION_H = 31,
- XL_MOVING_SPEED_L = 32,
- XL_MOVING_SPEED_H = 33,
- XL_TORQUE_LIMIT_L = 35,
- XL_TORQUE_LIMIT_H = 36,
- XL_PRESENT_POSITION_L = 37,
- XL_PRESENT_POSITION_H = 38,
- XL_PRESENT_SPEED_L = 39,
- XL_PRESENT_SPEED_H = 40,
- XL_PRESENT_LOAD_L = 41,
- XL_PRESENT_LOAD_H = 42,
- XL_PRESENT_VOLTAGE = 45,
- XL_PRESENT_TEMPERATURE = 46,
- XL_REGISTERED_INSTRUCTION = 47,
- XL_MOVING = 49,
- XL_HARDWARE_ERROR_STATUS = 50,
- XL_PUNCH_L = 51,
- XL_PUNCH_H = 51}TXLServo;
+typedef struct{
+ unsigned short int address;
+ unsigned char size;
+ unsigned char eeprom;
+}TDynReg;
+extern TDynReg ax_reg[NUM_REG];
+extern TDynReg mx_1_0_reg[NUM_REG];
+extern TDynReg mx_2_0_reg[NUM_REG];
+extern TDynReg xl_reg[NUM_REG];
+
+typedef enum {
+ // [Access] [Description]
+ // Info
+ model_number=0, // (R) Model Number
+ model_info, // (R)
+ firmware_version, // (R) Version of the Firmware
+ id, // (RW) ID of Dynamixel
+ baudrate, // (RW) Baud Rate of Dynamixel
+ return_delay_time, // (RW) Return Delay Time
+ second_id, // (RW)
+ protocol, // (RW)
+ return_level, // (RW) Status Return Level
+ // Configuration
+ drive_mode, // (RW) Drive mode
+ op_mode, // (RW) Joint or wheel mode
+ homming_offset, // (RW)
+ moving_threshold, // (RW)
+ min_angle_limit, // (RW) Clockwise Angle Limit
+ max_angle_limit, // (RW) Counterclockwise Angle Limit
+ temp_limit, // (RW) Internal Temperature Limit
+ min_voltage_limit, // (RW) Lowest Voltage Limit
+ max_voltage_limit, // (RW) Highest Voltage Limit
+ pwm_limit, // (RW) Maximum Torque
+ max_pwm, // (RW) Maximum Torque
+ current_limit, // (RW)
+ max_current, // (RW)
+ velocity_limit, // (RW)
+ multi_turn_off, // (RW) Multi turn offset position
+ resolution_div, // (RW) Resolution divider
+ // Status
+ alarm_led, // (RW) LED for Alarm
+ alarm_shtdwn, // (RW) Shutdown for Alarm
+ led, // (RW) LED On/Off
+ bus_watchdog, // (RW)
+ moving, // (R) Means if there is any movement
+ moving_status, // (R)
+ lock, // (RW) Locking EEPROM
+ reg_inst, // (RW) Means if Instruction is registered
+ hardware_error, // (R) Means if there is any movement
+ // control
+ torque_en, // (RW) Torque On/Off
+ vel_pid_i, // (RW)
+ vel_pid_p, // (RW)
+ pos_pid_p, // (RW)
+ pos_pid_i, // (RW)
+ pos_pid_d, // (RW)
+ feedfwd_gain_2, // (RW)
+ feedfwd_gain_1, // (RW)
+ cw_comp_margin, // (RW) CW Compliance Margin
+ ccw_comp_margin, // (RW) CCW Compliance Margin
+ cw_comp_slope, // (RW) CW Compliance Slope
+ ccw_comp_slope, // (RW) CCW Compliance Slope
+ punch, // (RW) Punch
+ // Set point
+ goal_pos, // (RW) Goal Position
+ goal_speed, // (RW) Moving Speed
+ goal_pwm, // (RW)
+ goal_current, // (RW)
+ profile_accel, // (RW)
+ profile_vel, // (RW)
+ // Feedback
+ current_pos, // (R) Current Position
+ current_speed, // (R) Current Speed
+ current_load, // (R) Current Load
+ current_voltage, // (R) Current Voltage
+ current_temp, // (R) Current Temperature
+ current_pwm, // (R)
+ realtime_tick, // (R) Realtime tick
+ velocity_traj, // (R)
+ pos_traj // (R)
+} reg_id;
+
+#define NUM_SERVO_MODELS 11
+
+typedef struct{
+ unsigned short int model;
+ unsigned short int encoder_resolution;
+ unsigned char gear_ratio;
+ unsigned short int max_angle;
+ unsigned short int center_angle;
+ unsigned short int max_speed;
+ long long int cw_angle_limit;
+ long long int ccw_angle_limit;
+ TDynReg *registers;
+ unsigned char pid;
+ unsigned char protocol_ver;
+}TServoConfig;
+
+extern TServoConfig servo_data[NUM_SERVO_MODELS];
#endif
diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h
index 7ebf98851692aa2f1eae350e0e8c9b33f4cb9db4..59ef8ffd3c0529094abdf8d2fb14ce052d0a4468 100644
--- a/dynamixel_manager/include/modules/motion_manager.h
+++ b/dynamixel_manager/include/modules/motion_manager.h
@@ -6,34 +6,25 @@ extern "C" {
#endif
#include "dyn_module.h"
+#include "dyn_servos.h"
+#include "motion_manager_registers.h"
#ifndef MM_MAX_NUM_MOTION_MODULES
#error "Please, specify the maximum number of motion modules with the MM_MAX_NUM_MOTION_MODULES macro"
#endif
-typedef enum {MM_NONE = -1,
+typedef enum {MM_NONE = 7,
MM_ACTION = 0,
MM_WALKING = 1,
MM_JOINTS = 2,
MM_HEAD = 3} TModules;
-typedef struct{
- unsigned short int model;
- unsigned short int encoder_resolution;
- unsigned char gear_ratio;
- unsigned short int max_angle;
- unsigned short int center_angle;
- unsigned short int max_speed;
- long long int cw_angle_limit;
- long long int ccw_angle_limit;
-}TServoConfig;
-
typedef struct{
unsigned char cw_compliance;
unsigned char ccw_compliance;
unsigned short int target_value;
long long int target_angle;
- unsigned short int current_value;
+ unsigned int current_value;
long long int current_angle;
TModules module;
unsigned char enabled;
@@ -46,26 +37,48 @@ typedef struct {
TDynModule dyn_module;
struct TMotionModule *modules[MM_MAX_NUM_MOTION_MODULES];
unsigned char num_modules;
- TServoConfig servo_configs[DYN_MANAGER_MAX_NUM_DEVICES];
+ TServoConfig *servo_configs[DYN_MANAGER_MAX_NUM_DEVICES];
TServoValues servo_values[DYN_MANAGER_MAX_NUM_DEVICES];
OP_HANDLE *enable_op;
- OP_HANDLE *motion_op;
- OP_HANDLE *feedback_op;
+ OP_HANDLE *motion_op[2];
+ OP_HANDLE *feedback_op[2];
unsigned char balance_enabled;
void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]);
+ TMemory *memory;
+ TMemModule mem_module;
+ unsigned short int ram_base_address;
+ unsigned short int eeprom_base_address;
+ unsigned int present_servos;
}TMotionManager;
-unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory);
-TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager);
-void mmanager_enable_balance(TMotionManager *mmanager);
-void mmanager_disable_balance(TMotionManager *mmanager);
-unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager);
+unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+static inline TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager)
+{
+ return &mmanager->dyn_module;
+}
+static inline void mmanager_enable_balance(TMotionManager *mmanager)
+{
+ if(mmanager->balance!=0x00000000)
+ mmanager->balance_enabled=0x01;
+}
+static inline void mmanager_disable_balance(TMotionManager *mmanager)
+{
+ mmanager->balance_enabled=0x00;
+}
+static inline unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager)
+{
+ return mmanager->balance_enabled;
+}
void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module);
TModules mmanager_get_module(TMotionManager *mmanager,unsigned char servo_id);
void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id);
void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id);
unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char servo_id);
-void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset);
+static inline void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset)
+{
+ if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+ mmanager->servo_values[servo_id].offset=offset;
+}
void mmanager_add_module(TMotionManager *mmanager,struct TMotionModule *module);
#ifdef __cplusplus
diff --git a/dynamixel_manager/include/modules/motion_manager_registers.h b/dynamixel_manager/include/modules/motion_manager_registers.h
index 02fbc9ed572a7d596bada1c73b517df5bb13815a..1d0bc4b1447149c7d54fc44965d395b2d0effda3 100644
--- a/dynamixel_manager/include/modules/motion_manager_registers.h
+++ b/dynamixel_manager/include/modules/motion_manager_registers.h
@@ -3,14 +3,24 @@
#include "dyn_module_registers.h"
-#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH + 0)
+#define RAM_MM_LENGTH (4+DYN_MANAGER_MAX_NUM_DEVICES/2)
+
+#define MM_PRESENT_SERVOS_OFFSET 0
+#define MM_ENABLE_MODULE_OFFSET 4
+ #define MM_EVEN_SER_EN 0x80
+ #define MM_EVEN_SER_MOD 0x70
+ #define MM_ODD_SER_EN 0x08
+ #define MM_ODD_SER_MOD 0x07
#define EEPROM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES)
-#define MM_SERVO_OFFSET 0
+#define MM_SERVO_OFFSET_OFFSET 0
-#define dyn_mm_eeprom_data(name,section_name,base_address) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={0,base_address+MM_SERVO_OFFSET};
+#define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,...) \
+dyn_module_eeprom_data(name##_module,section_name,base_address,DEFAULT_PERIOD) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+ __VA_ARGS__ \
+};
#endif
diff --git a/dynamixel_manager/include/modules/motion_module.h b/dynamixel_manager/include/modules/motion_module.h
index 9498d5411c91ed6d61991c24e623ecb7966302ae..af07a7d31d89bd8abd16f83470cb0ffad9d42905 100644
--- a/dynamixel_manager/include/modules/motion_module.h
+++ b/dynamixel_manager/include/modules/motion_module.h
@@ -10,7 +10,7 @@ extern "C" {
typedef struct TMotionModule{
TModules id;
TMotionManager *manager;
- void (*set_period)(void *module,unsigned short int period_us);
+ void (*set_period)(void *module,unsigned short int period_ms);
void (*set_module)(void *module,unsigned char servo_id);
void (*pre_process)(void *module);
void (*post_process)(void *module);
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 73db6b175604463e9a7c863b62610ea24b11530d..3e70b32b8c366ee66e1ec271ee40da58c3c4ebe9 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -18,20 +18,41 @@ extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsig
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;
// private functions
+void dyn_manager_lock(TDynManager *manager)
+{
+ while(manager->lock==1);
+ manager->lock=1;
+}
+
+void dyn_manager_unlock(TDynManager *manager)
+{
+ manager->lock=0;
+}
+
+void dyn_manager_start_scan(TDynManager *manager)
+{
+ dyn_manager_stop(manager);
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_SCANNING;
+ scheduler_enable_channel(manager->scheduler,manager->sch_scan_ch);
+}
+
void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
- unsigned short int uint16_value;
- unsigned char *data_ptr;
+ unsigned short int eeprom_offset,ram_offset;
+
+ eeprom_offset=address-module->eeprom_base_address;
+ if(ram_in_window(module->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET,1,address,length))
+ dyn_manager_set_period(module,data[DYN_MANAGER_PERIOD_OFFSET-eeprom_offset]);
- if(ram_in_window(DYN_MANAGER_PERIOD,2,address,length))
+ ram_offset=address-module->ram_base_address;
+ if(ram_in_range(module->ram_base_address+DYN_MANAGER_CONTROL_OFFSET,address,length))
{
- uint16_value=dyn_manager_get_period(module);
- data_ptr=(unsigned char *)&uint16_value;
- if(ram_in_range(DYN_MANAGER_PERIOD,address,length))
- data_ptr[0]=data[DYN_MANAGER_PERIOD-address];
- if(ram_in_range(DYN_MANAGER_PERIOD+1,address,length))
- data_ptr[1]=data[DYN_MANAGER_PERIOD+1-address];
- dyn_manager_set_period(module,uint16_value);
+ if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START)
+ dyn_manager_start(module);
+ if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_STOP)
+ dyn_manager_stop(module);
+ if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START_SCAN)
+ dyn_manager_start_scan(module);
}
}
@@ -62,7 +83,8 @@ TDynManagerOpCommon *dyn_manager_get_op_common(TDynManager *manager,unsigned cha
default: return 0x00000000;
}
}
- else return 0x00000000;
+ else
+ return 0x00000000;
}
OP_HANDLE *dyn_manager_get_free_op_handle(TDynManager *manager)
@@ -91,7 +113,7 @@ unsigned char dyn_manager_id_present(unsigned char num,unsigned char *ids,unsign
void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char master_index,OP_HANDLE *op)
{
- unsigned char j,k,delete;
+ unsigned char j,k,del;
if(op->op_index[master_index]!=0xFF)
{
@@ -126,7 +148,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
break;
case DYN_MANAGER_SYNC: if(op->op_index[master_index]<(DYN_MANAGER_MAX_NUM_SYNC_OP-1))
{
- for(j=op->op_index[master_index];j<manager->operations[master_index].num_sync_op;j++)
+ for(j=op->op_index[master_index];j<=manager->operations[master_index].num_sync_op;j++)
{
manager->operations[master_index].sync_op[j].common.operation=manager->operations[master_index].sync_op[j+1].common.operation;
manager->operations[master_index].sync_op[j].common.repetitions=manager->operations[master_index].sync_op[j+1].common.repetitions;
@@ -134,12 +156,12 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
manager->operations[master_index].sync_op[j].common.op_handle=manager->operations[master_index].sync_op[j+1].common.op_handle;
manager->operations[master_index].sync_op[j].address=manager->operations[master_index].sync_op[j+1].address;
manager->operations[master_index].sync_op[j].length=manager->operations[master_index].sync_op[j+1].length;
- manager->operations[master_index].sync_op[j].num_devices=manager->operations[master_index].sync_op[j+1].num_devices;
for(k=0;k<manager->operations[master_index].sync_op[j].num_devices;k++)
{
manager->operations[master_index].sync_op[j].ids[k]=manager->operations[master_index].sync_op[j+1].ids[k];
manager->operations[master_index].sync_op[j].data[k]=manager->operations[master_index].sync_op[j+1].data[k];
}
+ manager->operations[master_index].sync_op[j].num_devices=manager->operations[master_index].sync_op[j+1].num_devices;
}
}
else
@@ -150,12 +172,12 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
manager->operations[master_index].sync_op[op->op_index[master_index]].common.op_handle=0x00000000;
manager->operations[master_index].sync_op[op->op_index[master_index]].address=0x0000;
manager->operations[master_index].sync_op[op->op_index[master_index]].length=0x0000;
- manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices=0x00;
for(k=0;k<manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices;k++)
{
manager->operations[master_index].sync_op[op->op_index[master_index]].ids[k]=0xFF;
manager->operations[master_index].sync_op[op->op_index[master_index]].data[k]=0x00000000;
}
+ manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices=0x00;
}
manager->operations[master_index].num_sync_op--;
break;
@@ -167,7 +189,6 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
manager->operations[master_index].bulk_op[j].common.repetitions=manager->operations[master_index].bulk_op[j+1].common.repetitions;
manager->operations[master_index].bulk_op[j].common.period_count=manager->operations[master_index].bulk_op[j+1].common.period_count;
manager->operations[master_index].bulk_op[j].common.op_handle=manager->operations[master_index].bulk_op[j+1].common.op_handle;
- manager->operations[master_index].bulk_op[j].num_devices=manager->operations[master_index].bulk_op[j+1].num_devices;
for(k=0;k<manager->operations[master_index].bulk_op[j].num_devices;k++)
{
manager->operations[master_index].bulk_op[j].address[k]=manager->operations[master_index].bulk_op[j+1].address[k];
@@ -175,6 +196,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
manager->operations[master_index].bulk_op[j].ids[k]=manager->operations[master_index].bulk_op[j+1].ids[k];
manager->operations[master_index].bulk_op[j].data[k]=manager->operations[master_index].bulk_op[j+1].data[k];
}
+ manager->operations[master_index].bulk_op[j].num_devices=manager->operations[master_index].bulk_op[j+1].num_devices;
}
}
else
@@ -183,7 +205,6 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
manager->operations[master_index].bulk_op[op->op_index[master_index]].common.repetitions=0x01;
manager->operations[master_index].bulk_op[op->op_index[master_index]].common.period_count=0x01;
manager->operations[master_index].bulk_op[op->op_index[master_index]].common.op_handle=0x00000000;
- manager->operations[master_index].bulk_op[op->op_index[master_index]].num_devices=0x00;
for(k=0;k<manager->operations[master_index].bulk_op[op->op_index[master_index]].num_devices;k++)
{
manager->operations[master_index].bulk_op[op->op_index[master_index]].address[k]=0x0000;
@@ -191,6 +212,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
manager->operations[master_index].bulk_op[op->op_index[master_index]].ids[k]=0xFF;
manager->operations[master_index].bulk_op[op->op_index[master_index]].data[k]=0x00000000;
}
+ manager->operations[master_index].bulk_op[op->op_index[master_index]].num_devices=0x00;
}
manager->operations[master_index].num_bulk_op--;
break;
@@ -199,11 +221,11 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
else
op->op_index[master_index]=0xFF;
// remove handles
- delete=0x01;
+ del=0x01;
for(j=0;j<manager->num_masters;j++)
if(op->op_index[j]!=0xFF)
- delete=0x00;
- if(delete==0x01)
+ del=0x00;
+ if(del==0x01)
{
op->op_type=no_op;
manager->num_ops--;
@@ -211,7 +233,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
}
// public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned short int eeprom_base_address,unsigned short int ram_base_address)
+unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t loop_ch,sched_channel_t scan_ch,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
{
unsigned char i,j,k;
@@ -281,9 +303,14 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned sho
manager->op_handles[i].handle_index=i;
}
manager->num_ops=0x00;
- /* initialize timer */
- if(manager->init_timer!=0x00000000)
- manager->init_timer();
+ manager->present_devices=0x00000000;
+ manager->lock=0;
+ /* initialize scheduler */
+ scheduler_set_channel(scheduler,loop_ch,(void(*)(void *))dyn_manager_loop,100,manager);
+ scheduler_set_channel_one_shot(scheduler,scan_ch,(void(*)(void *))dyn_manager_scan,100,manager);
+ manager->scheduler=scheduler;
+ manager->sch_loop_ch=loop_ch;
+ manager->sch_scan_ch=scan_ch;
/* initialize memory module */
mem_module_init(&manager->mem_module);
@@ -298,29 +325,36 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned sho
return 0x00;
manager->eeprom_base_address=eeprom_base_address;
manager->ram_base_address=ram_base_address;
+ manager->memory=memory;
+ manager->running=0x00;
return 0x01;
}
-void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us)
+void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms)
{
unsigned char i;
- if(manager->set_period!=0x00000000)
- {
- manager->set_period(period_us);
- for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
- if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000)
- manager->modules[i]->set_period(manager->modules[i]->data,period_us);
- manager->period_us=period_us;
- manager->memory->data[DYN_MANAGER_PERIOD]=period_us%256;
- manager->memory->data[DYN_MANAGER_PERIOD+1]=period_us/256;
- }
+ scheduler_change_period(manager->scheduler,manager->sch_loop_ch,period_ms);
+ for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
+ if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000)
+ manager->modules[i]->set_period(manager->modules[i]->data,period_ms*manager->modules[i]->period_count);
+ manager->period_ms=period_ms;
+ manager->memory->data[manager->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET]=period_ms;
+}
+
+void dyn_manager_start(TDynManager *manager)
+{
+ scheduler_enable_channel(manager->scheduler,manager->sch_loop_ch);
+ manager->running=0x01;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_RUNNING;
}
-unsigned short int dyn_manager_get_period(TDynManager *manager)
+void dyn_manager_stop(TDynManager *manager)
{
- return manager->period_us;
+ scheduler_disable_channel(manager->scheduler,manager->sch_loop_ch);
+ manager->running=0x00;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_RUNNING);
}
void dyn_manager_scan(TDynManager *manager)
@@ -329,6 +363,8 @@ void dyn_manager_scan(TDynManager *manager)
unsigned char i,j,k,num=0;
unsigned short int model;
+ manager->present_devices=0x00000000;
+ manager->num_devices=0;
// scan all dynamixel buses
for(i=0;i<manager->num_masters;i++)
{
@@ -344,6 +380,7 @@ void dyn_manager_scan(TDynManager *manager)
manager->devices[servo_ids[j]].model=model;
manager->devices[servo_ids[j]].master_index=i;
manager->num_devices++;
+ manager->present_devices|=(0x00000001<<servo_ids[j]);
// pass the infor to each of the modules
for(k=0;k<DYN_MANAGER_MAX_NUM_MODULES;k++)
if(manager->modules[k]!=0x00000000)
@@ -352,10 +389,17 @@ void dyn_manager_scan(TDynManager *manager)
// else ignore device
}
}
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_DEVICES_OFFSET]=manager->num_devices;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING);
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES]=manager->present_devices&0x000000FF;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES+1]=(manager->present_devices>>8)&0x000000FF;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES+2]=(manager->present_devices>>16)&0x000000FF;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES+3]=(manager->present_devices>>24)&0x000000FF;
// configure the operations
for(i=0;i<manager->num_modules;i++)
if(manager->modules[i]->setup!=0x00000000)
manager->modules[i]->setup(manager->modules[i]->data);
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING);
}
void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master)
@@ -369,15 +413,10 @@ void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master)
{
manager->masters[i]=master;
manager->num_masters++;
- manager->memory->data[DYN_MANAGER_NUM_MASTERS]++;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_MASTERS_OFFSET]++;
}
}
-unsigned char dyn_manager_get_num_masters(TDynManager *manager)
-{
- return manager->num_masters;
-}
-
TDynamixelMaster *dyn_manager_get_device_master(TDynManager *manager,unsigned char device_id)
{
if(dyn_manager_check_id(manager,device_id)!=0xFF)
@@ -386,11 +425,6 @@ TDynamixelMaster *dyn_manager_get_device_master(TDynManager *manager,unsigned ch
return 0x00000000;
}
-unsigned short int dyn_manager_get_device_model(TDynManager *manager,unsigned char device_id)
-{
- return manager->devices[device_id].model;
-}
-
void dyn_manager_add_module(TDynManager *manager,TDynModule *module)
{
unsigned char i;
@@ -403,15 +437,11 @@ void dyn_manager_add_module(TDynManager *manager,TDynModule *module)
manager->modules[i]=module;
manager->num_modules++;
module->manager=manager;
- manager->memory->data[DYN_MANAGER_NUM_MODULES]++;
+ manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_MODULES_OFFSET]++;
+ manager->modules[i]->set_period(manager->modules[i]->data,manager->period_ms*manager->modules[i]->period_count);
}
}
-unsigned char dyn_manager_get_num_modules(TDynManager *manager)
-{
- return manager->num_modules;
-}
-
void dyn_manager_delete_op(TDynManager *manager,OP_HANDLE *op)
{
unsigned char i;
@@ -488,19 +518,29 @@ OP_HANDLE *dyn_manager_single_op_new(TDynManager *manager,unsigned char mode,uns
TDynManagerSingleOp *current_op;
OP_HANDLE *op_handle;
+ dyn_manager_lock(manager);
if((master_index=dyn_manager_check_id(manager,id))==0xFF)
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
if((op_handle=dyn_manager_get_free_op_handle(manager))==0x00000000)
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
if(mode==DYN_MANAGER_READ || mode==DYN_MANAGER_WRITE)
- op_handle->op_type=DYN_MANAGER_SINGLE|mode;
+ op_handle->op_type=(op_type_t)(DYN_MANAGER_SINGLE|mode);
else
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
current_op_index=manager->operations[master_index].num_single_op;
if(current_op_index<DYN_MANAGER_MAX_NUM_SINGLE_OP)
{
current_op=&manager->operations[master_index].single_op[current_op_index];
- current_op->common.operation=DYN_MANAGER_SINGLE|mode;
+ current_op->common.operation=(op_type_t)(DYN_MANAGER_SINGLE|mode);
current_op->common.op_handle=op_handle;
current_op->id=id;
current_op->address=address;
@@ -513,8 +553,11 @@ OP_HANDLE *dyn_manager_single_op_new(TDynManager *manager,unsigned char mode,uns
else
{
op_handle->op_type=no_op;
+ dyn_manager_unlock(manager);
return 0x00000000;
}
+ dyn_manager_unlock(manager);
+
return op_handle;
}
@@ -522,44 +565,52 @@ void dyn_manager_single_op_change_id(TDynManager *manager,OP_HANDLE *op,unsigned
{
unsigned char i;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
manager->operations[i].single_op[op->op_index[i]].id=id;
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_single_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned short int address)
{
unsigned char i;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
manager->operations[i].single_op[op->op_index[i]].address=address;
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_single_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length)
{
unsigned char i;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
manager->operations[i].single_op[op->op_index[i]].length=length;
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_single_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char *data)
{
unsigned char i;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
manager->operations[i].single_op[op->op_index[i]].data=data;
}
+ dyn_manager_unlock(manager);
}
// sync operation functions
@@ -569,17 +620,25 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
TDynManagerSyncOp *current_op;
OP_HANDLE *op_handle;
+ dyn_manager_lock(manager);
if((op_handle=dyn_manager_get_free_op_handle(manager))==0x00000000)
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
if(mode==DYN_MANAGER_READ || mode==DYN_MANAGER_WRITE)
- op_handle->op_type=DYN_MANAGER_SYNC|mode;
+ op_handle->op_type=(op_type_t)(DYN_MANAGER_SYNC|mode);
else
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
for(i=0;i<num;i++)
{
if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF)
{
op_handle->op_type=no_op;
+ dyn_manager_unlock(manager);
return 0x00000000;
}
if(op_handle->op_index[master_index]==0xFF)
@@ -591,7 +650,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00)
{
op_handle->op_index[master_index]=current_op_index;
- current_op->common.operation=DYN_MANAGER_SYNC|mode;
+ current_op->common.operation=(op_type_t)(DYN_MANAGER_SYNC|mode);
current_op->common.op_handle=op_handle;
current_op->address=address;
current_op->length=length;
@@ -604,6 +663,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
else
{
op_handle->op_type=no_op;
+ dyn_manager_unlock(manager);
return 0x00000000;
}
}
@@ -620,6 +680,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
}
}
manager->num_ops++;
+ dyn_manager_unlock(manager);
return op_handle;
}
@@ -628,6 +689,7 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned
TDynManagerSyncOp *current_op;
unsigned char i,j;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -647,6 +709,7 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned
}
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids)
@@ -654,6 +717,7 @@ void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig
TDynManagerSyncOp *current_op;
unsigned char i,j,k,device_index;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -683,28 +747,33 @@ void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig
dyn_manager_delete_op_single_master(manager,i,op);
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_sync_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned short int address)
{
unsigned char i;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
manager->operations[i].sync_op[op->op_index[i]].address=address;
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_sync_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length)
{
unsigned char i;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
manager->operations[i].sync_op[op->op_index[i]].length=length;
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[])
@@ -712,6 +781,7 @@ void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned
TDynManagerSyncOp *current_op;
unsigned char i,j,device_index;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -724,6 +794,7 @@ void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned
}
}
}
+ dyn_manager_unlock(manager);
}
// bulk operation functions
@@ -733,17 +804,25 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig
TDynManagerBulkOp *current_op;
OP_HANDLE *op_handle;
+ dyn_manager_lock(manager);
if((op_handle=dyn_manager_get_free_op_handle(manager))==0x00000000)
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
if(mode==DYN_MANAGER_READ || mode==DYN_MANAGER_WRITE)
- op_handle->op_type=DYN_MANAGER_BULK|mode;
+ op_handle->op_type=(op_type_t)(DYN_MANAGER_BULK|mode);
else
+ {
+ dyn_manager_unlock(manager);
return 0x00000000;
+ }
for(i=0;i<num;i++)
{
if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF)
{
op_handle->op_type=no_op;
+ dyn_manager_unlock(manager);
return 0x00000000;
}
if(op_handle->op_index[master_index]==0xFF)
@@ -755,7 +834,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig
if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00)
{
op_handle->op_index[master_index]=current_op_index;
- current_op->common.operation=DYN_MANAGER_BULK|mode;
+ current_op->common.operation=(op_type_t)(DYN_MANAGER_BULK|mode);
current_op->common.op_handle=op_handle;
current_op->address[current_op->num_devices]=address[i];
current_op->length[current_op->num_devices]=length[i];
@@ -768,6 +847,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig
else
{
op_handle->op_type=no_op;
+ dyn_manager_unlock(manager);
return 0x00000000;
}
}
@@ -786,6 +866,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig
}
}
manager->num_ops++;
+ dyn_manager_unlock(manager);
return op_handle;
}
@@ -794,6 +875,7 @@ void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned
TDynManagerBulkOp *current_op;
unsigned char i,j;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -815,6 +897,7 @@ void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned
}
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids)
@@ -822,6 +905,7 @@ void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig
TDynManagerBulkOp *current_op;
unsigned char i,j,k,device_index;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -855,6 +939,7 @@ void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig
dyn_manager_delete_op_single_master(manager,i,op);
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address)
@@ -862,6 +947,7 @@ void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsig
TDynManagerBulkOp *current_op;
unsigned char i,j,device_index;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -874,6 +960,7 @@ void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsig
}
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *length)
@@ -881,6 +968,7 @@ void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsign
TDynManagerBulkOp *current_op;
unsigned char i,j,device_index;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -893,6 +981,7 @@ void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsign
}
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[])
@@ -900,6 +989,7 @@ void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned
TDynManagerBulkOp *current_op;
unsigned char i,j,device_index;
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
if(op->op_index[i]!=0xFF)
@@ -912,6 +1002,7 @@ void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned
}
}
}
+ dyn_manager_unlock(manager);
}
void dyn_manager_loop(TDynManager *manager)
@@ -924,7 +1015,7 @@ void dyn_manager_loop(TDynManager *manager)
TDynManagerSingleOp *single_op[DYN_MANAGER_MAX_NUM_MASTERS];
TDynManagerSyncOp *sync_op[DYN_MANAGER_MAX_NUM_MASTERS];
TDynManagerBulkOp *bulk_op[DYN_MANAGER_MAX_NUM_MASTERS];
- static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MASTERS]={0};
+ static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MODULES]={0};
static unsigned char single_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
static unsigned char sync_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
static unsigned char bulk_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
@@ -948,11 +1039,13 @@ void dyn_manager_loop(TDynManager *manager)
{
if(manager->modules[i]->pre_process!=0x00000000)
manager->modules[i]->pre_process(manager->modules[i]->data);
+ modules_period_count[i]=0;
}
}
}
// send all the commands
+ dyn_manager_lock(manager);
do{
for(i=0;i<manager->num_masters;i++)
{
@@ -1083,6 +1176,7 @@ void dyn_manager_loop(TDynManager *manager)
}
}
}while(loop_done>0);
+ dyn_manager_unlock(manager);
// execute the pre_process functions for all modules
for(i=0;i<manager->num_modules;i++)
diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c
index 4207dee7a4b61ba1383b94fe7d4d4b9441326fe2..949fe66af2017bf01bc7ed029dfadd6953da41f3 100644
--- a/dynamixel_manager/src/dyn_module.c
+++ b/dynamixel_manager/src/dyn_module.c
@@ -1,6 +1,25 @@
#include "dyn_module.h"
+#include "dyn_module_registers.h"
+#include "ram.h"
-unsigned char dyn_module_init(TDynModule *module)
+// private functions
+void dyn_module_write_cmd(TDynModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned short int eeprom_offset;
+
+ eeprom_offset=address-module->eeprom_base_address;
+ if(ram_in_window(module->eeprom_base_address+DYN_MODULE_PERIOD_OFFSET,1,address,length))
+ dyn_module_set_period(module,data[DYN_MODULE_PERIOD_OFFSET-eeprom_offset]);
+ ram_write_table(module->memory,address,length,data);
+}
+
+void dyn_module_read_cmd(TDynModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(module->memory,address,length,data);
+}
+
+// public functions
+unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
{
unsigned char i;
@@ -11,31 +30,31 @@ unsigned char dyn_module_init(TDynModule *module)
module->assigned_ids[i]=0x00;
module->num_assigned_ids=0x00;
module->period_count=0x00;
- module->enabled=0x00;
+ module->enabled=0x01;
module->add_device=0x00000000;
+ module->set_period=0x00000000;
module->setup=0x00000000;
module->pre_process=0x00000000;
module->post_process=0x00000000;
module->data=0x00000000;
+ /* initialize memory module */
+ mem_module_init(&module->mem_module);
+ module->mem_module.data=module;
+ module->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_module_write_cmd;
+ module->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_module_read_cmd;
+ if(!mem_module_add_ram_segment(&module->mem_module,ram_base_address,RAM_DYN_MODULE_LENGTH))
+ return 0x00;
+ if(!mem_module_add_eeprom_segment(&module->mem_module,eeprom_base_address,EEPROM_DYN_MODULE_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&module->mem_module))
+ return 0x00;
+ module->eeprom_base_address=eeprom_base_address;
+ module->ram_base_address=ram_base_address;
+ module->memory=memory;
return 0x01;
}
-void dyn_module_enable(TDynModule *module)
-{
- module->enabled=0x01;
-}
-
-void dyn_module_disable(TDynModule *module)
-{
- module->enabled=0x00;
-}
-
-unsigned char dyn_module_is_enabled(TDynModule *module)
-{
- return module->enabled;
-}
-
void dyn_module_add_model(TDynModule *module,unsigned short int model)
{
unsigned char i;
@@ -50,11 +69,6 @@ void dyn_module_add_model(TDynModule *module,unsigned short int model)
}
}
-unsigned char dyn_module_get_num_models(TDynModule *module)
-{
- return module->num_models;
-}
-
void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model)
{
unsigned char i;
@@ -72,12 +86,7 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i
}
}
-unsigned char dyn_module_get_num_assigned_devices(TDynModule *module)
-{
- return module->num_assigned_ids;
-}
-
-unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id)
+unsigned char dyn_module_device_is_assigned(TDynModule *module,unsigned char id)
{
if(id<DYN_MANAGER_MAX_NUM_DEVICES)
return module->assigned_ids[id];
@@ -88,14 +97,8 @@ unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id)
void dyn_module_set_period(TDynModule *module,unsigned char period)
{
if(period==0x00)
- module->period_count=0x01;
- else
- module->period_count=period;
-}
-
-unsigned char dyn_module_get_period(TDynModule *module)
-{
- return module->period_count;
+ period=0x01;
+ module->period_count=period;
+ if(module->set_period!=0x00000000)
+ module->set_period(module->data,period*module->manager->period_ms);
}
-
-
diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c
index ea816ab70fd8b5fb941a6d98c1ecd37825016141..e901e01264cd3153a919c00899412317c73b4373 100755
--- a/dynamixel_manager/src/modules/action.c
+++ b/dynamixel_manager/src/modules/action.c
@@ -1,43 +1,34 @@
#include "action.h"
-#include "motion_pages.h"
+#include "action_mm_registers.h"
+#include "ram.h"
#define SPEED_BASE_SCHEDULE 0x00
#define TIME_BASE_SCHEDULE 0x0A
-// private variables
-typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
-TMotionModule action_module;
+// private functions
+void action_write_cmd(TActionMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned int ram_offset;
-TPage action_next_page;
-TPage action_current_page;
-unsigned char action_current_step_index;
-unsigned char action_current_page_index;
-// angle variables
-long long int action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-long long int action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-// speed variables
-long long int action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-long long int action_main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-// control variables
-unsigned char action_end,action_stop;
-unsigned char action_zero_speed_finish[PAGE_MAX_NUM_SERVOS];
-unsigned char action_next_index;
-unsigned char action_running;
-// time variables (in time units (7.8 ms each time unit))
-long long int action_total_time;// fixed point 48|16 format
-long long int action_pre_time;// fixed point 48|16 format
-long long int action_main_time;// fixed point 48|16 format
-long long int action_step_time;// fixed point 48|16 format
-long long int action_pause_time;// fixed point 48|16 format
-long long int action_current_time;// fixed point 48|16 format
-long long int action_section_time;// fixed point 48|16 format
-long long int action_period;
+ ram_offset=address-module->ram_base_address;
+ if(ram_in_range(module->ram_base_address+ACTION_MM_CONTROL_OFFSET,address,length))
+ {
+ if(data[ACTION_MM_CONTROL_OFFSET-ram_offset]&ACTION_MM_START)
+ action_start_page(module);
+ if(data[ACTION_MM_CONTROL_OFFSET-ram_offset]&ACTION_MM_STOP)
+ action_stop_page(module);
+ }
+ if(ram_in_range(module->ram_base_address+ACTION_MM_PAGE_OFFSET,address,length))// load the page identifier
+ action_load_page(module,data[ACTION_MM_PAGE_OFFSET-ram_offset]);
+}
-// private functions
-void action_load_next_step(void)
+void action_read_cmd(TActionMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(module->memory,address,length,data);
+}
+
+void action_load_next_step(TActionMModule *action)
{
- // page and step information
- static char num_repetitions=0;
// angle variables
short int next_angle;// information from the flash memory (fixed point 9|7 format)
long long int max_angle;// fixed point format 48|16 format
@@ -56,178 +47,181 @@ void action_load_next_step(void)
// control information
unsigned char i=0;
- action_current_step_index++;
- if(action_current_step_index>=action_current_page.header.stepnum)// the last step has been executed
+ action->current_step_index++;
+ if(action->current_step_index>=action->current_page.header.stepnum)// the last step has been executed
{
- if(action_current_page.header.stepnum==1)
+ if(action->current_page.header.stepnum==1)
{
- if(action_stop)
- action_next_index=action_current_page.header.exit;
+ if(action->stop)
+ action->next_index=action->current_page.header.exit;
else
{
- num_repetitions--;
- if(num_repetitions>0)
- action_next_index=action_current_page_index;
+ action->num_repetitions--;
+ if(action->num_repetitions>0)
+ action->next_index=action->current_page_index;
else
- action_next_index=action_current_page.header.next;
+ action->next_index=action->current_page.header.next;
}
- if(action_next_index==0)
- action_end=0x01;
+ if(action->next_index==0)
+ action->end=0x01;
else
{
- if(action_next_index!=action_current_page_index)
+ if(action->next_index!=action->current_page_index)
{
- pages_get_page(action_next_index,&action_next_page);
- if(!pages_check_checksum(&action_next_page))
- action_end=0x01;
+ pages_get_page(action->next_index,&action->next_page);
+ if(!pages_check_checksum(&action->next_page))
+ action->end=0x01;
}
- if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
- action_end=0x01;
+ if(action->next_page.header.repeat==0 || action->next_page.header.stepnum==0)
+ action->end=0x01;
}
}
- pages_copy_page(&action_next_page,&action_current_page);// make the next page active
- if(action_current_page_index!=action_next_index)
- num_repetitions=action_current_page.header.repeat;
- action_current_step_index=0;
- action_current_page_index=action_next_index;
+ pages_copy_page(&action->next_page,&action->current_page);// make the next page active
+ if(action->current_page_index!=action->next_index)
+ action->num_repetitions=action->current_page.header.repeat;
+ action->current_step_index=0;
+ action->current_page_index=action->next_index;
}
- else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step
+ else if(action->current_step_index==action->current_page.header.stepnum-1)// it is the last step
{
- if(action_stop)
- action_next_index=action_current_page.header.exit;
+ if(action->stop)
+ action->next_index=action->current_page.header.exit;
else
{
- num_repetitions--;
- if(num_repetitions>0)
- action_next_index=action_current_page_index;
+ action->num_repetitions--;
+ if(action->num_repetitions>0)
+ action->next_index=action->current_page_index;
else
- action_next_index=action_current_page.header.next;
+ action->next_index=action->current_page.header.next;
}
- if(action_next_index==0)
- action_end=0x01;
+ if(action->next_index==0)
+ action->end=0x01;
else
{
- if(action_next_index!=action_current_page_index)
+ if(action->next_index!=action->current_page_index)
{
- pages_get_page(action_next_index,&action_next_page);
- if(!pages_check_checksum(&action_next_page))
- action_end=0x01;
+ pages_get_page(action->next_index,&action->next_page);
+ if(!pages_check_checksum(&action->next_page))
+ action->end=0x01;
}
- if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
- action_end=0x01;
+ if(action->next_page.header.repeat==0 || action->next_page.header.stepnum==0)
+ action->end=0x01;
}
}
// compute trajectory
- action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5;
- action_pause_time=action_pause_time<<9;
- action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5;
- action_step_time=action_step_time<<9;
- if(action_step_time<action_period)
- action_step_time=action_period;// 0.078 in 16|16 format
+ action->pause_time=(action->current_page.steps[action->current_step_index].pause*action->current_page.header.speed)>>5;
+ action->pause_time=action->pause_time<<9;
+ action->step_time=(action->current_page.steps[action->current_step_index].time*action->current_page.header.speed)>>5;
+ action->step_time=action->step_time<<9;
+ if(action->step_time<action->period)
+ action->step_time=action->period;// 0.078 in 16|16 format
max_angle=0;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- angle=action_current_page.steps[action_current_step_index].position[i];
+ angle=action->current_page.steps[action->current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
- target_angles=action_module.manager->servo_values[i].target_angle;
+ target_angles=action->mmodule.manager->servo_values[i].target_angle;
else
target_angles=angle<<9;
- action_moving_angles[i]=target_angles-action_module.manager->servo_values[i].target_angle;
- if(action_end)
+ action->moving_angles[i]=target_angles-action->mmodule.manager->servo_values[i].target_angle;
+ if(action->end)
next_angles=target_angles;
else
{
- if(action_current_step_index==action_current_page.header.stepnum-1)
- next_angle=action_next_page.steps[0].position[i];
+ if(action->current_step_index==action->current_page.header.stepnum-1)
+ next_angle=action->next_page.steps[0].position[i];
else
- next_angle=action_current_page.steps[action_current_step_index+1].position[i];
+ next_angle=action->current_page.steps[action->current_step_index+1].position[i];
if(next_angle==0x5A00)
next_angles=target_angles;
else
next_angles=next_angle<<9;
}
// check changes in direction
- if(((action_module.manager->servo_values[i].target_angle < target_angles) && (target_angles < next_angles)) ||
- ((action_module.manager->servo_values[i].target_angle > target_angles) && (target_angles > next_angles)))
+ if(((action->mmodule.manager->servo_values[i].target_angle < target_angles) && (target_angles < next_angles)) ||
+ ((action->mmodule.manager->servo_values[i].target_angle > target_angles) && (target_angles > next_angles)))
dir_change=0x00;
else
dir_change=0x01;
// check the type of ending
- if(dir_change || action_pause_time>0 || action_end)
- action_zero_speed_finish[i]=0x01;
+ if(dir_change || action->pause_time>0 || action->end)
+ action->zero_speed_finish[i]=0x01;
else
- action_zero_speed_finish[i]=0x00;
+ action->zero_speed_finish[i]=0x00;
// find the maximum angle of motion in speed base schedule
- if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
+ if(action->current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
{
// fer el valor absolut
- if(action_moving_angles[i]<0)
- tmp_angle=-action_moving_angles[i];
+ if(action->moving_angles[i]<0)
+ tmp_angle=-action->moving_angles[i];
else
- tmp_angle=action_moving_angles[i];
+ tmp_angle=action->moving_angles[i];
if(tmp_angle>max_angle)
max_angle=tmp_angle;
}
- action_module.manager->servo_values[i].cw_compliance=(1<<(action_current_page.header.slope[i]&0x0F));
- action_module.manager->servo_values[i].ccw_compliance=(1<<((action_current_page.header.slope[i]&0xF0)>>4));
+ action->mmodule.manager->servo_values[i].cw_compliance=(1<<(action->current_page.header.slope[i]&0x0F));
+ action->mmodule.manager->servo_values[i].ccw_compliance=(1<<((action->current_page.header.slope[i]&0xF0)>>4));
}
}
- if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
- action_step_time=(max_angle*256)/(action_current_page.steps[action_current_step_index].time*720);
- accel_time_num=action_current_page.header.accel;
+ if(action->current_page.header.schedule==SPEED_BASE_SCHEDULE)
+ action->step_time=(max_angle*256)/(action->current_page.steps[action->current_step_index].time*720);
+ accel_time_num=action->current_page.header.accel;
accel_time_num=accel_time_num<<9;
- if(action_step_time<=(accel_time_num<<1))
+ if(action->step_time<=(accel_time_num<<1))
{
- if(action_step_time==0)
+ if(action->step_time==0)
accel_time_num=0;
else
{
- accel_time_num=(action_step_time-action_period)>>1;
+ accel_time_num=(action->step_time-action->period)>>1;
if(accel_time_num==0)
- action_step_time=0;
+ action->step_time=0;
}
}
- action_total_time=action_step_time;
- action_pre_time=accel_time_num;
- action_main_time=action_total_time-action_pre_time;
- non_zero_speed_divider=(action_pre_time>>1)+action_main_time;
- if(non_zero_speed_divider<action_period)
- non_zero_speed_divider=action_period;
- zero_speed_divider=action_main_time;
- if(zero_speed_divider<action_period)
- zero_speed_divider=action_period;
+ action->total_time=action->step_time;
+ action->pre_time=accel_time_num;
+ action->main_time=action->total_time-action->pre_time;
+ non_zero_speed_divider=(action->pre_time>>1)+action->main_time;
+ if(non_zero_speed_divider<action->period)
+ non_zero_speed_divider=action->period;
+ zero_speed_divider=action->main_time;
+ if(zero_speed_divider<action->period)
+ zero_speed_divider=action->period;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
- moving_angle=action_moving_angles[i]<<16;
- if(action_zero_speed_finish[i])
- action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
+ action_pre_time_initial_speed_angle=(action->start_speed[i]*action->pre_time)>>1;
+ moving_angle=action->moving_angles[i]<<16;
+ if(action->zero_speed_finish[i])
+ action->main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
else
- action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
-// if((action_main_speed[i]>>16)>info.max_speed)
-// action_main_speed[i]=(info.max_speed*65536);
-// else if((action_main_speed[i]>>16)<-info.max_speed)
-// action_main_speed[i]=-(info.max_speed*65536);
+ action->main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
+// if((action->main_speed[i]>>16)>info.max_speed)
+// action->main_speed[i]=(info.max_speed*65536);
+// else if((action->main_speed[i]>>16)<-info.max_speed)
+// action->main_speed[i]=-(info.max_speed*65536);
}
}
}
-void action_finish(void)
+void action_finish(TActionMModule *action)
{
// set the internal state machine to the idle state
- action_end=0x00;
+ action->end=0x00;
// change the internal state
- action_running=0x00;
+ action->running=0x00;
+ action->memory->data[action->ram_base_address+ACTION_MM_CONTROL_OFFSET]&=(~ACTION_MM_RUNNING);
}
-void action_set_period(void *module,unsigned short int period_us)
+void action_set_period(void *module,unsigned short int period_ms)
{
+ TActionMModule *action=(TActionMModule *)module;
+
// load the current period
- action_period=(period_us<<16)/1000000;
+ action->period=(period_ms<<16)/1000;
}
void action_set_module(void *module,unsigned char servo_id)
@@ -237,221 +231,214 @@ void action_set_module(void *module,unsigned char servo_id)
void action_pre_process(void *module)
{
+ TActionMModule *action=(TActionMModule *)module;
unsigned char i;
- // angle variables
- static long long int accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
- static long long int main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
- // speed variables (in controller units)
- static long long int current_speed[PAGE_MAX_NUM_SERVOS]={0};// fixed point 48|16 format
- long long int delta_speed;
- // control variables
- static action_states state=ACTION_PAUSE;
- if(action_running==0x01)
+ if(action->running==0x01)
{
- action_current_time+=action_period;
- switch(state)
+ action->current_time+=action->period;
+ switch(action->state)
{
- case ACTION_PRE: if(action_current_time>action_section_time)
+ case ACTION_PRE: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* the last segment of the pre section */
- delta_speed=(action_main_speed[i]-action_start_speed[i]);
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=(action->main_speed[i]-action->start_speed[i]);
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=(action->start_speed[i]*action->section_time+((action->delta_speed*action->section_time)>>1))>>16;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
/* update of the state */
- if(!action_zero_speed_finish[i])
+ if(!action->zero_speed_finish[i])
{
- if((action_step_time-action_pre_time)==0)
- main_angles[i]=0;
+ if((action->step_time-action->pre_time)==0)
+ action->main_angles[i]=0;
else
- main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
+ action->main_angles[i]=((action->moving_angles[i]-action->accel_angles[i])*(action->step_time-(action->pre_time<<1)))/(action->step_time-action->pre_time);
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
}
else
{
- main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
+ action->main_angles[i]=action->moving_angles[i]-action->accel_angles[i]-(((action->main_speed[i]*action->pre_time)>>1)>>16);
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
}
/* the first step of the main section */
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(action->main_angles[i]*(action->current_time-action->section_time))/(action->step_time-(action->pre_time<<1));
+ action->current_speed[i]=action->main_speed[i];
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_step_time-(action_pre_time<<1);
- state=ACTION_MAIN;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->step_time-(action->pre_time<<1);
+ action->state=ACTION_MAIN;
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=((action->main_speed[i]-action->start_speed[i])*action->current_time)/action->section_time;
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=((action->start_speed[i]*action->current_time)+((action->delta_speed*action->current_time)>>1))>>16;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
}
}
- state=ACTION_PRE;
+ action->state=ACTION_PRE;
}
break;
- case ACTION_MAIN: if(action_current_time>action_section_time)
+ case ACTION_MAIN: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* last segment of the main section */
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+main_angles[i];
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->main_angles[i];
+ action->current_speed[i]=action->main_speed[i];
/* update state */
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
- main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
+ action->main_angles[i]=action->moving_angles[i]-action->main_angles[i]-action->accel_angles[i];
/* first segment of the post section */
- if(action_zero_speed_finish[i])
+ if(action->zero_speed_finish[i])
{
- delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
- current_speed[i]=action_main_speed[i]+delta_speed;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ action->delta_speed=((0.0-action->main_speed[i])*(action->current_time-action->section_time))/action->pre_time;
+ action->current_speed[i]=action->main_speed[i]+action->delta_speed;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(((action->main_speed[i]*(action->current_time-action->section_time))+((action->delta_speed*(action->current_time-action->section_time))>>1))>>16);
}
else
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+((action->main_angles[i]*(action->current_time-action->section_time))/action->pre_time);
+ action->current_speed[i]=action->main_speed[i];
}
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pre_time;
- state=ACTION_POST;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pre_time;
+ action->state=ACTION_POST;
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(action->main_angles[i]*action->current_time)/action->section_time;
+ action->current_speed[i]=action->main_speed[i];
}
}
- state=ACTION_MAIN;
+ action->state=ACTION_MAIN;
}
break;
- case ACTION_POST: if(action_current_time>action_section_time)
+ case ACTION_POST: if(action->current_time>action->section_time)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
/* last segment of the post section */
- if(action_zero_speed_finish[i])
+ if(action->zero_speed_finish[i])
{
- delta_speed=-action_main_speed[i];
- current_speed[i]=action_main_speed[i]+delta_speed;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+ action->delta_speed=-action->main_speed[i];
+ action->current_speed[i]=action->main_speed[i]+action->delta_speed;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(((action->main_speed[i]*action->section_time)+((action->delta_speed*action->section_time)>>1))>>16);
}
else
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+main_angles[i];
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->main_angles[i];
+ action->current_speed[i]=action->main_speed[i];
}
/* update state */
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
- action_start_speed[i]=current_speed[i];
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
+ action->start_speed[i]=action->current_speed[i];
}
}
/* load the next step */
- if(action_pause_time==0)
+ if(action->pause_time==0)
{
- if(action_end)
+ if(action->end)
{
- state=ACTION_PAUSE;
- action_finish();
+ action->state=ACTION_PAUSE;
+ action_finish(action);
}
else
{
- action_load_next_step();
+ action_load_next_step(action);
/* first step of the next section */
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=((action->main_speed[i]-action->start_speed[i])*(action->current_time-action->section_time))/action->pre_time;
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=(((action->start_speed[i]*(action->current_time-action->section_time))+((action->delta_speed*(action->current_time-action->section_time))>>1))>>16);
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pre_time;
- state=ACTION_PRE;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pre_time;
+ action->state=ACTION_PRE;
}
}
else
{
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pause_time;
- state=ACTION_PAUSE;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pause_time;
+ action->state=ACTION_PAUSE;
}
}
else
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- if(action_zero_speed_finish[i])
+ if(action->zero_speed_finish[i])
{
- delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i]+delta_speed;
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+ action->delta_speed=((0.0-action->main_speed[i])*action->current_time)/action->section_time;
+ action->current_speed[i]=action->main_speed[i]+action->delta_speed;
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(((action->main_speed[i]*action->current_time)+((action->delta_speed*action->current_time)>>1))>>16);
}
else
{
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i];
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+(action->main_angles[i]*action->current_time)/action->section_time;
+ action->current_speed[i]=action->main_speed[i];
}
}
}
- state=ACTION_POST;
+ action->state=ACTION_POST;
}
break;
- case ACTION_PAUSE: if(action_current_time>action_section_time)
+ case ACTION_PAUSE: if(action->current_time>action->section_time)
{
- if(action_end)
+ if(action->end)
{
- state=ACTION_PAUSE;
- action_finish();
+ action->state=ACTION_PAUSE;
+ action_finish(action);
}
else
{
// find next page to execute
- action_load_next_step();
+ action_load_next_step(action);
/* first step of the next section */
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(mmanager_get_module(action_module.manager,i)==MM_ACTION)
+ if(mmanager_get_module(action->mmodule.manager,i)==MM_ACTION)
{
- delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i];
+ action->delta_speed=((action->main_speed[i]-action->start_speed[i])*(action->current_time-action->section_time))/action->pre_time;
+ action->current_speed[i]=action->start_speed[i]+action->delta_speed;
+ action->accel_angles[i]=(((action->start_speed[i]*(action->current_time-action->section_time))+((action->delta_speed*(action->current_time-action->section_time))>>1))>>16);
+ action->mmodule.manager->servo_values[i].target_angle=action->start_angles[i]+action->accel_angles[i];
}
}
- action_current_time=action_current_time-action_section_time;
- action_section_time=action_pre_time;
- state=ACTION_PRE;
+ action->current_time=action->current_time-action->section_time;
+ action->section_time=action->pre_time;
+ action->state=ACTION_PRE;
}
}
else// pause section still active
- state=ACTION_PAUSE;
+ action->state=ACTION_PAUSE;
break;
default: break;
}
@@ -459,98 +446,116 @@ void action_pre_process(void *module)
}
// public functions
-void action_init(void)
+unsigned char action_init(TActionMModule *action,TMemory *memory,unsigned short int ram_base_address)
{
unsigned char i;
/* initialize the motion module */
- mmodule_init(&action_module);
- action_module.id=MM_ACTION;
- action_module.set_period=action_set_period;
- action_module.set_module=action_set_module;
- action_module.pre_process=action_pre_process;
+ mmodule_init(&action->mmodule);
+ action->mmodule.id=MM_ACTION;
+ action->mmodule.set_period=action_set_period;
+ action->mmodule.set_module=action_set_module;
+ action->mmodule.pre_process=action_pre_process;
+ action->mmodule.data=action;
// init manager_current_angles with the current position of the servos
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
// angle variables
- action_moving_angles[i]=0;// fixed point 48|16 format
- action_start_angles[i]=0;
+ action->moving_angles[i]=0;// fixed point 48|16 format
+ action->start_angles[i]=0;
// speed variables
- action_start_speed[i]=0;// fixed point 48|16 format
- action_main_speed[i]=0;// fixed point 48|16 format
+ action->start_speed[i]=0;// fixed point 48|16 format
+ action->main_speed[i]=0;// fixed point 48|16 format
// control variables
- action_zero_speed_finish[i]=0x00;
+ action->zero_speed_finish[i]=0x00;
+ action->current_speed[i]=0;
}
// clear the contents of the next page
- pages_clear_page(&action_next_page);
- pages_clear_page(&action_current_page);
- action_current_page_index=0;
- action_current_step_index=-1;
+ pages_clear_page(&action->next_page);
+ pages_clear_page(&action->current_page);
+ action->current_page_index=0;
+ action->current_step_index=-1;
// control variables
- action_end=0x00;
- action_stop=0x00;
- action_running=0x00;
- action_next_index=0;
+ action->end=0x00;
+ action->stop=0x00;
+ action->running=0x00;
+ action->next_index=0;
// time variables (in time units (7.8 ms each time unit))
- action_total_time=0;// fixed point 48|16 format
- action_pre_time=0;// fixed point 48|16 format
- action_main_time=0;// fixed point 48|16 format
- action_step_time=0;// fixed point 48|16 format
- action_pause_time=0;// fixed point 48|16 format
- action_current_time=0;// fixed point 48|16 format
- action_section_time=0;// fixed point 48|16 format
- action_period=(DYN_MANAGER_DEFAULT_PERIOD_US<<16)/1000000;
+ action->total_time=0;// fixed point 48|16 format
+ action->pre_time=0;// fixed point 48|16 format
+ action->main_time=0;// fixed point 48|16 format
+ action->step_time=0;// fixed point 48|16 format
+ action->pause_time=0;// fixed point 48|16 format
+ action->current_time=0;// fixed point 48|16 format
+ action->section_time=0;// fixed point 48|16 format
+ action->state=ACTION_PAUSE;
+ action->num_repetitions=0;
+ //action_period=(DYN_MANAGER_DEFAULT_PERIOD_US<<16)/1000000;
+ mem_module_init(&action->mem_module);
+ action->mem_module.data=action;
+ action->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))action_write_cmd;
+ action->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))action_read_cmd;
+ if(!mem_module_add_ram_segment(&action->mem_module,ram_base_address,RAM_ACTION_MM_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&action->mem_module))
+ return 0x00;
+ action->ram_base_address=ram_base_address;
+ action->memory=memory;
+
+ return 0x01;
}
-TMotionModule *action_get_module(void)
+TMotionModule *action_get_module(TActionMModule *action)
{
- return &action_module;
+ return &action->mmodule;
}
-unsigned char action_load_page(unsigned char page_id)
+unsigned char action_load_page(TActionMModule *action,unsigned char page_id)
{
- action_next_index=page_id;
- pages_get_page(action_next_index,&action_current_page);
- pages_get_page(action_current_page.header.next,&action_next_page);
- if(!pages_check_checksum(&action_current_page))
+ action->next_index=page_id;
+ pages_get_page(action->next_index,&action->current_page);
+ pages_get_page(action->current_page.header.next,&action->next_page);
+ if(!pages_check_checksum(&action->current_page))
return 0x00;
- if(!pages_check_checksum(&action_next_page))
+ if(!pages_check_checksum(&action->next_page))
return 0x00;
+ action->memory->data[action->ram_base_address+ACTION_MM_PAGE_OFFSET]=page_id;
return 0x01;
}
-void action_start_page(void)
+void action_start_page(TActionMModule *action)
{
unsigned char i;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
- action_start_angles[i]=action_module.manager->servo_values[i].target_angle;
- action_stop=0x00;
- action_current_time=0;
- action_section_time=0;
- action_current_step_index=-1;
+ action->start_angles[i]=action->mmodule.manager->servo_values[i].target_angle;
+ action->stop=0x00;
+ action->current_time=0;
+ action->section_time=0;
+ action->current_step_index=-1;
/* clear the interrupt flag */
- action_running=0x01;
+ action->running=0x01;
+ action->memory->data[action->ram_base_address+ACTION_MM_CONTROL_OFFSET]|=ACTION_MM_RUNNING;
}
-void action_stop_page(void)
+void action_stop_page(TActionMModule *action)
{
- action_stop=0x01;
+ action->stop=0x01;
}
-unsigned char action_is_running(void)
+unsigned char action_is_running(TActionMModule *action)
{
- return action_running;
+ return action->running;
}
-unsigned char action_get_current_page(void)
+unsigned char action_get_current_page(TActionMModule *action)
{
- return action_current_page_index;
+ return action->current_page_index;
}
-unsigned char action_get_current_step(void)
+unsigned char action_get_current_step(TActionMModule *action)
{
- return action_current_step_index;
+ return action->current_step_index;
}
diff --git a/dynamixel_manager/src/modules/dyn_servos.c b/dynamixel_manager/src/modules/dyn_servos.c
new file mode 100644
index 0000000000000000000000000000000000000000..4a7ba4a90f29903fa52bbd3bf4fb2dce2b9396ff
--- /dev/null
+++ b/dynamixel_manager/src/modules/dyn_servos.c
@@ -0,0 +1,291 @@
+#include "dyn_servos.h"
+
+TDynReg ax_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0xFFFF,0,0},
+ {0x0002,1,1},
+ {0x0003,1,1},
+ {0x0004,1,1},
+ {0x0005,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0010,1,1},
+ // Configuration
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0006,2,1},
+ {0x0008,2,1},
+ {0x000B,1,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x000E,2,1},
+ {0x0022,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Status
+ {0x0011,1,1},
+ {0x0012,1,1},
+ {0x0019,1,0},
+ {0xFFFF,0,0},
+ {0x002E,1,0},
+ {0xFFFF,0,0},
+ {0x002F,1,0},
+ {0x002C,1,0},
+ {0xFFFF,0,0},
+ // Control
+ {0x0018,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x001A,1,0},
+ {0x001B,1,0},
+ {0x001C,1,0},
+ {0x001D,1,0},
+ {0x0030,2,0},
+ // Set point
+ {0x001E,2,0},
+ {0x0020,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Feedback
+ {0x0024,2,0},
+ {0x0026,2,0},
+ {0x0028,2,0},
+ {0x002A,1,0},
+ {0x002B,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0}};
+
+TDynReg mx_1_0_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0xFFFF,0,0},
+ {0x0002,1,1},
+ {0x0003,1,1},
+ {0x0004,1,1},
+ {0x0005,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0010,1,1},
+ // Configuration
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0006,2,1},
+ {0x0008,2,1},
+ {0x000B,1,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x000E,2,1},
+ {0x0022,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0020,2,0},
+ {0x0014,2,1},
+ {0x0016,1,1},
+ // Status
+ {0x0011,1,1},
+ {0x0012,1,1},
+ {0x0019,1,0},
+ {0xFFFF,0,0},
+ {0x002E,1,0},
+ {0xFFFF,0,0},
+ {0x002F,1,0},
+ {0x002C,1,0},
+ {0xFFFF,0,0},
+ // Control
+ {0x0018,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x001C,1,0},
+ {0x001B,1,0},
+ {0x001A,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0030,2,0},
+ // Set point
+ {0x001E,2,0},
+ {0x0020,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0049,1,0},
+ {0xFFFF,0,0},
+ // Feedback
+ {0x0024,2,0},
+ {0x0026,2,0},
+ {0x0028,2,0},
+ {0x002A,1,0},
+ {0x002B,1,0},
+ {0xFFFF,0,0},
+ {0x0032,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0}};
+
+TDynReg xl_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0xFFFF,0,0},
+ {0x0002,1,1},
+ {0x0003,1,1},
+ {0x0004,1,1},
+ {0x0005,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0011,1,1},
+ // Configuration
+ {0xFFFF,0,0},
+ {0x000B,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0006,2,1},
+ {0x0008,2,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x000E,1,1},
+ {0x000F,2,1},
+ {0x0023,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Status
+ {0x0012,1,1},
+ {0x0012,1,1},
+ {0x0019,1,0},
+ {0xFFFF,0,0},
+ {0x0031,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x002F,1,0},
+ {0x0032,1,0},
+ // Control
+ {0x0018,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x001D,1,0},
+ {0x001C,1,0},
+ {0x001B,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0033,2,0},
+ // Set point
+ {0x001E,2,0},
+ {0x0020,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0049,1,0},
+ {0xFFFF,0,0},
+ // Feedback
+ {0x0025,2,0},
+ {0x0027,2,0},
+ {0x0029,2,0},
+ {0x002D,1,0},
+ {0x002E,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0}};
+
+TDynReg mx_2_0_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0x0002,4,1},
+ {0x0006,1,1},
+ {0x0007,1,1},
+ {0x0008,1,1},
+ {0x0009,1,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x0044,1,0},
+ // Configuration
+ {0x000A,1,1},
+ {0x000B,1,1},
+ {0x0014,4,1},
+ {0x0018,4,1},
+ {0x0034,4,1},
+ {0x0030,4,1},
+ {0x001F,1,1},
+ {0x0022,2,1},
+ {0x0020,2,1},
+ {0x0024,2,1},
+ {0xFFFF,0,0},
+ {0x0026,2,1},
+ {0xFFFF,0,0},
+ {0x002C,4,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Status
+ {0x003F,1,1},
+ {0x003F,1,1},
+ {0x0041,1,0},
+ {0x0062,1,0},
+ {0x007A,1,0},
+ {0x007B,1,0},
+ {0xFFFF,0,0},
+ {0x0045,1,0},
+ {0x0046,1,0},
+ // Control
+ {0x0040,1,0},
+ {0x004C,2,0},
+ {0x004E,2,0},
+ {0x0054,2,0},
+ {0x0052,2,0},
+ {0x0050,2,0},
+ {0x0058,2,0},
+ {0x005A,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Set point
+ {0x0074,4,0},
+ {0x0068,4,0},
+ {0x0064,2,0},
+ {0x0066,2,0},
+ {0x006C,4,0},
+ {0x0070,4,0},
+ // Feedback
+ {0x0084,4,0},
+ {0x0080,4,0},
+ {0x007E,2,0},
+ {0x0090,2,0},
+ {0x0092,1,0},
+ {0x007C,2,0},
+ {0x0078,2,0},
+ {0x0088,5,0},
+ {0x008C,4,0}};
+
+TServoConfig servo_data[NUM_SERVO_MODELS]={
+//model | enc_res | gear | max_angle | center_angle | max_speed | cw_limit | ccw_limit | registers | pid | protocol
+{300 , 1023 , 32 , 300<<7 , 150<<7 , 324 , 0 , 300<<7 , ax_reg , 0 , 1},
+{12 , 1023 , 254 , 300<<7 , 150<<7 , 354 , 0 , 300<<7 , ax_reg , 0 , 1},
+{18 , 1023 , 254 , 300<<7 , 150<<7 , 582 , 0 , 300<<7 , ax_reg , 0 , 1},
+{360 , 4095 , 32 , 360<<7 , 180<<7 , 2820 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{29 , 4095 , 193 , 360<<7 , 180<<7 , 330 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{310 , 4095 , 200 , 360<<7 , 180<<7 , 378 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{320 , 4095 , 225 , 360<<7 , 180<<7 , 270 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{30 , 4095 , 193 , 360<<7 , 180<<7 , 378 , 0 , 360<<7 , mx_2_0_reg, 1 , 2},
+{311 , 4095 , 200 , 360<<7 , 180<<7 , 378 , 0 , 360<<7 , mx_2_0_reg, 1 , 2},
+{321 , 4095 , 225 , 360<<7 , 180<<7 , 270 , 0 , 360<<7 , mx_2_0_reg, 1 , 2},
+{350 , 1023 , 238 , 300<<7 , 150<<7 , 684 , 0 , 300<<7 , xl_reg , 1 , 2}};
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index 9c8c7dc992e7f8098e6ab3ba4c4c4fcedf20625f..3e5a676b5384bb71abd3ce4386a210ebcd3c3742 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -1,32 +1,83 @@
#include "motion_manager.h"
+#include "motion_manager_registers.h"
#include "motion_module.h"
-#include "dyn_devices.h"
-#include "dyn_servos.h"
-
-/* private variables */
-uint16_t mm_eeprom_data[] __attribute__ ((section (".eeprom")))={DEFAULT_ADC_PERIOD,ADC_PERIOD};
+#include "ram.h"
/* private functions */
+void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
+{
+ unsigned char reg_value[4]={0};
+
+ if(reg->address!=0xFFFF)
+ {
+ dyn_master_read_table(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,reg->address,reg->size,reg_value);
+ if(reg->size==1)
+ (*value)=reg_value[0];
+ else if(reg->size==2)
+ (*value)=reg_value[0]+(reg_value[1]<<8);
+ else if(reg->size==4)
+ (*value)=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
+ }
+}
+
+void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int value)
+{
+}
+
void mmanager_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
+ TMotionManager *manager=(TMotionManager *)module;
+ ram_read_table(manager->memory,address,length,data);
}
void mmanager_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
+ TMotionManager *manager=(TMotionManager *)module;
+ unsigned short int eeprom_offset,ram_offset,i,j;
+ unsigned char byte_value,mod;
+ eeprom_offset=address-manager->eeprom_base_address;
+ if(ram_in_window(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES,address,length))
+ {
+ for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+ {
+ if(ram_in_range(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET+i,address,length))
+ mmanager_set_offset(manager,i,(signed char)(data[MM_SERVO_OFFSET_OFFSET-eeprom_offset+i]));
+ }
+ }
+ ram_offset=address-manager->ram_base_address;
+ if(ram_in_window(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES/2,address,length))
+ {
+ for(i=0,j=0;i<DYN_MANAGER_MAX_NUM_DEVICES/2;i++,j+=2)
+ {
+ if(ram_in_range(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET+i,address,length))
+ {
+ byte_value=data[MM_ENABLE_MODULE_OFFSET-ram_offset+i];
+ if(byte_value&MM_EVEN_SER_EN) mmanager_enable_servo(manager,j);
+ else mmanager_disable_servo(manager,j);
+ mod=(byte_value&MM_EVEN_SER_MOD)>>4;
+ mmanager_set_module(manager,j,(TModules)mod);
+ if(byte_value&MM_ODD_SER_EN) mmanager_enable_servo(manager,j+1);
+ else mmanager_disable_servo(manager,j+1);
+ mod=byte_value&MM_ODD_SER_MOD;
+ mmanager_set_module(manager,j+1,(TModules)mod);
+ }
+ }
+ }
+ ram_write_table(manager->memory,address,length,data);
}
unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
inline unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle)
{
- return ((angle+mmanager->servo_configs[servo_id].center_angle)*mmanager->servo_configs[servo_id].encoder_resolution)/mmanager->servo_configs[servo_id].max_angle;
+ return ((angle+mmanager->servo_configs[servo_id]->center_angle)*mmanager->servo_configs[servo_id]->encoder_resolution)/mmanager->servo_configs[servo_id]->max_angle;
}
short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value);
inline short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value)
{
- return (((short int)((value*mmanager->servo_configs[servo_id].max_angle)/mmanager->servo_configs[servo_id].encoder_resolution))-mmanager->servo_configs[servo_id].center_angle);
+ return (((short int)((value*mmanager->servo_configs[servo_id]->max_angle)/mmanager->servo_configs[servo_id]->encoder_resolution))-mmanager->servo_configs[servo_id]->center_angle);
}
void mmanager_compute_targets(TMotionManager *mmanager)
@@ -64,129 +115,29 @@ void mmanager_compute_angles(TMotionManager *mmanager)
void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned short int model)
{
- unsigned short int cw_value,ccw_value;
+ unsigned int cw_value,ccw_value;
+ unsigned char i;
if(id<DYN_MANAGER_MAX_NUM_DEVICES)
{
- switch(model)
- {
- case SERVO_DX113: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=193;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=324;
- break;
- case SERVO_DX116: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=143;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=354;
- break;
- case SERVO_DX117: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=193;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=510;
- break;
- case SERVO_AX12A: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=254;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=354;
- break;
- case SERVO_AX12W: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=32;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=2830;
- break;
- case SERVO_AX18A: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=254;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=582;
- break;
- case SERVO_RX10: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=193;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=324;
- break;
- case SERVO_MX12W: mmanager->servo_configs[id].encoder_resolution=4095;
- mmanager->servo_configs[id].gear_ratio=32;
- mmanager->servo_configs[id].max_angle=360<<7;
- mmanager->servo_configs[id].center_angle=180<<7;
- mmanager->servo_configs[id].max_speed=2820;
- break;
- case SERVO_MX28: mmanager->servo_configs[id].encoder_resolution=4095;
- mmanager->servo_configs[id].gear_ratio=193;
- mmanager->servo_configs[id].max_angle=360<<7;
- mmanager->servo_configs[id].center_angle=180<<7;
- mmanager->servo_configs[id].max_speed=330;
- break;
- case SERVO_RX24F: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=193;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=756;
- break;
- case SERVO_RX28: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=193;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=402;
- break;
- case SERVO_RX64: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=200;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=294;
- break;
- case SERVO_MX64: mmanager->servo_configs[id].encoder_resolution=4095;
- mmanager->servo_configs[id].gear_ratio=200;
- mmanager->servo_configs[id].max_angle=360<<7;
- mmanager->servo_configs[id].center_angle=180<<7;
- mmanager->servo_configs[id].max_speed=378;
- break;
- case SERVO_EX106: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=184;
- mmanager->servo_configs[id].max_angle=250<<7;
- mmanager->servo_configs[id].center_angle=125<<7;
- mmanager->servo_configs[id].max_speed=414;
- break;
- case SERVO_MX106: mmanager->servo_configs[id].encoder_resolution=4095;
- mmanager->servo_configs[id].gear_ratio=225;
- mmanager->servo_configs[id].max_angle=360<<7;
- mmanager->servo_configs[id].center_angle=180<<7;
- mmanager->servo_configs[id].max_speed=270;
- break;
- case SERVO_XL320: mmanager->servo_configs[id].encoder_resolution=1023;
- mmanager->servo_configs[id].gear_ratio=238;
- mmanager->servo_configs[id].max_angle=300<<7;
- mmanager->servo_configs[id].center_angle=150<<7;
- mmanager->servo_configs[id].max_speed=684;
- break;
- default: break;
- }
- // get the servo's current position
- if(model==SERVO_XL320)
- {
- dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_PRESENT_POSITION_L,&mmanager->servo_values[id].current_value);
- dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_CW_ANGLE_LIMIT_L,&cw_value);
- dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_CCW_ANGLE_LIMIT_L,&ccw_value);
- }
- else
- {
- dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_PRESENT_POSITION_L,&mmanager->servo_values[id].current_value);
- dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_CW_ANGLE_LIMIT_L,&cw_value);
- dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_CCW_ANGLE_LIMIT_L,&ccw_value);
- }
+ for(i=0;i<NUM_SERVO_MODELS;i++)
+ if(model==servo_data[i].model)
+ mmanager->servo_configs[id]=&servo_data[i];
+
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
mmanager->servo_values[id].target_value=mmanager->servo_values[id].current_value;
mmanager->servo_values[id].current_angle=(mmanager_value_to_angle(mmanager,id,mmanager->servo_values[id].current_value)<<9);
mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle;
// read the servo limits
- mmanager->servo_configs[id].cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9);
- mmanager->servo_configs[id].ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9);
+ mmanager->servo_configs[id]->cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9);
+ mmanager->servo_configs[id]->ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9);
+ mmanager->present_servos|=(0x00000001<<id);
+ mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET]=mmanager->present_servos&0x000000FF;
+ mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+1]=(mmanager->present_servos>>8)&0x000000FF;
+ mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+2]=(mmanager->present_servos>>16)&0x000000FF;
+ mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+3]=(mmanager->present_servos>>24)&0x000000FF;
}
}
@@ -196,55 +147,98 @@ void mmanager_setup(TMotionManager *mmanager)
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];
- unsigned char i,num=0;
+ unsigned char i,num=0,first=1;
+ unsigned short int start_address=0;
// initialize the motion operation
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{
if(mmanager->dyn_module.assigned_ids[i]==0x01)
{
- data[num]=&mmanager->servo_values[i].cw_compliance;
- ids[num]=i;
- num++;
+ if(mmanager->servo_configs[i]->protocol_ver==1)
+ {
+ data[num]=&mmanager->servo_values[i].cw_compliance;
+ ids[num]=i;
+ num++;
+ if(first==1)
+ {
+ first=0;
+ if(mmanager->servo_configs[i]->pid==1)
+ start_address=mmanager->servo_configs[i]->registers[pos_pid_p].address;
+ else
+ start_address=mmanager->servo_configs[i]->registers[cw_comp_slope].address;
+ }
+ }
+ }
+ }
+ if(num>0)
+ {
+ if(mmanager->motion_op[0]!=0x00000000)
+ dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[0]);
+ mmanager->motion_op[0]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,4,(unsigned char * const*)&data);
+ }
+ num=0;
+ for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+ {
+ if(mmanager->dyn_module.assigned_ids[i]==0x01)
+ {
+ if(mmanager->servo_configs[i]->protocol_ver==2)
+ {
+ data[num]=(unsigned char *)&mmanager->servo_values[i].target_value;
+ ids[num]=i;
+ num++;
+ if(first==1)
+ {
+ first=0;
+ start_address=mmanager->servo_configs[i]->registers[goal_pos].address;
+ }
+ }
}
}
- mmanager->motion_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,P_CW_COMPLIANCE_SLOPE,4,(unsigned char * const*)&data);
+ if(num>0)
+ {
+ if(mmanager->motion_op[1]!=0x00000000)
+ dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[1]);
+ mmanager->motion_op[1]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,2,(unsigned char * const*)&data);
+ }
/* create a feedback operation with the servos that support bulk read */
num=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{
if(mmanager->dyn_module.assigned_ids[i]==0x01)
{
- if(HAS_BULK_READ(dyn_manager_get_device_model(mmanager->dyn_module.manager,i)))
+ if(mmanager->servo_configs[i]->protocol_ver==2)
{
- if(dyn_manager_get_device_model(mmanager->dyn_module.manager,i)==SERVO_XL320)
- address[num]=XL_PRESENT_POSITION_L;
- else
- address[num]=P_PRESENT_POSITION_L;
+ address[num]=mmanager->servo_configs[i]->registers[current_pos].address;
data[num]=(unsigned char *)&mmanager->servo_values[i].current_value;
- length[num]=2;
+ length[num]=mmanager->servo_configs[i]->registers[current_pos].size;
ids[num]=i;
num++;
}
}
}
- mmanager->feedback_op=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
+ if(num>0)
+ {
+ if(mmanager->feedback_op[0]!=0x00000000)
+ dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->feedback_op[0]);
+ mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
+ }
}
-void mmanager_set_period(TMotionManager *mmanager,unsigned short int period_us)
+void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms)
{
unsigned char i;
- for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
- if(mmanager->modules[i]->set_period!=0x00000000)
- mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_us*mmanager->dyn_module.period_count);
+ for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
+ if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->set_period!=0x00000000)
+ mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_ms);
}
void mmanager_pre_process(TMotionManager *mmanager)
{
unsigned char i;
- for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
+ for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
{
if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->pre_process!=0x00000000)
mmanager->modules[i]->pre_process(mmanager->modules[i]->data);
@@ -259,7 +253,7 @@ void mmanager_post_process(TMotionManager *mmanager)
/* transform the current values to angles */
mmanager_compute_angles(mmanager);
- for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
+ for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
{
if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->post_process!=0x00000000)
mmanager->modules[i]->post_process(mmanager->modules[i]->data);
@@ -267,29 +261,15 @@ void mmanager_post_process(TMotionManager *mmanager)
}
/* public functions */
-unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory)
+unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
{
unsigned char i;
/* initialize the base module */
- dyn_module_init(&mmanager->dyn_module);
+ dyn_module_init(&mmanager->dyn_module,memory,ram_base_address,eeprom_base_address);
/* add all known servo models */
- dyn_module_add_model(&mmanager->dyn_module,SERVO_DX113);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_DX116);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_DX117);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_AX12A);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_AX12W);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_AX18A);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_RX10);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_MX12W);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_MX28);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_RX24F);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_RX28);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_RX64);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_MX64);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_EX106);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_MX106);
- dyn_module_add_model(&mmanager->dyn_module,SERVO_XL320);
+ for(i=0;i<NUM_SERVO_MODELS;i++)
+ dyn_module_add_model(&mmanager->dyn_module,servo_data[i].model);
mmanager->dyn_module.data=mmanager;
mmanager->dyn_module.add_device=(void (*)(void *,unsigned char,unsigned short int))mmanager_add_device;
mmanager->dyn_module.setup=(void (*)(void *))mmanager_setup;
@@ -300,19 +280,12 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory)
mmanager->balance_enabled=0x00;
mmanager->num_modules=0x00;
/* initialize motion module base attributes */
- for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
+ for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
mmanager->modules[i]=0x00000000;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{
/* servo configuration */
- mmanager->servo_configs[i].model=0x0000;
- mmanager->servo_configs[i].encoder_resolution=0x0000;
- mmanager->servo_configs[i].gear_ratio=0x00;
- mmanager->servo_configs[i].max_angle=0x0000;
- mmanager->servo_configs[i].center_angle=0x0000;
- mmanager->servo_configs[i].max_speed=0x0000;
- mmanager->servo_configs[i].cw_angle_limit=0;
- mmanager->servo_configs[i].ccw_angle_limit=0;
+ mmanager->servo_configs[i]=0x00000000;
/* servo values */
mmanager->servo_values[i].cw_compliance=0x00;
mmanager->servo_values[i].ccw_compliance=0x00;
@@ -325,46 +298,33 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory)
mmanager->servo_values[i].offset=0x00;
}
mmanager->enable_op=0x00000000;
- mmanager->motion_op=0x00000000;
- mmanager->feedback_op=0x00000000;
+ mmanager->motion_op[0]=0x00000000;
+ mmanager->motion_op[1]=0x00000000;
+ mmanager->feedback_op[0]=0x00000000;
+ mmanager->feedback_op[1]=0x00000000;
mmanager->balance=0x00000000;
+ mmanager->present_servos=0x00000000;
/* initialize memory module */
- mem_module_init(&manager->mem_module);
- manager->mem_module.data=manager;
- manager->mem_module.write_cmd=mmanager_write_cmd;
- manager->mem_module.read_cmd=mmanager_read_cmd;
- if(!mem_module_add_ram_segment(&manager->mem_module,RAM_MM_BASE_ADDRESS,RAM_MM_LENGTH))
+ mem_module_init(&mmanager->mem_module);
+ mmanager->mem_module.data=mmanager;
+ mmanager->mem_module.write_cmd=mmanager_write_cmd;
+ mmanager->mem_module.read_cmd=mmanager_read_cmd;
+ if(!mem_module_add_ram_segment(&mmanager->mem_module,ram_base_address+RAM_DYN_MODULE_LENGTH,RAM_MM_LENGTH))
return 0x00;
- if(!mem_module_add_eeprom_segment(&manager->mem_module,EEPROM_MM_BASE_ADDRESS,EEPROM_MM_LENGTH))
+ mmanager->ram_base_address=ram_base_address+RAM_DYN_MODULE_LENGTH;
+ if(!mem_module_add_eeprom_segment(&mmanager->mem_module,eeprom_base_address+EEPROM_DYN_MODULE_LENGTH,EEPROM_MM_LENGTH))
return 0x00;
- if(!mem_add_module(memory,&manager->mem_module))
+ mmanager->eeprom_base_address=eeprom_base_address+EEPROM_DYN_MODULE_LENGTH;
+ if(!mem_add_module(memory,&mmanager->mem_module))
return 0x00;
+ mmanager->memory=memory;
+ for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES/2;i++)
+ mmanager->memory->data[ram_base_address+MM_ENABLE_MODULE_OFFSET+i]=0x77;
return 0x01;
}
-TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager)
-{
- return &mmanager->dyn_module;
-}
-
-void mmanager_enable_balance(TMotionManager *mmanager)
-{
- if(mmanager->balance!=0x00000000)
- mmanager->balance_enabled=0x01;
-}
-
-void mmanager_disable_balance(TMotionManager *mmanager)
-{
- mmanager->balance_enabled=0x00;
-}
-
-unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager)
-{
- return mmanager->balance_enabled;
-}
-
void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module)
{
if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
@@ -394,13 +354,16 @@ void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
{
mmanager->servo_values[servo_id].enabled=0x01;
/* add an operation to enable the servo */
- if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ if(mmanager->servo_configs[servo_id]!=0x00000000)
{
- mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,P_TORQUE_ENABLE,1,&data);
- dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ {
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
+ dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ }
+ else
+ dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
- else
- dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
}
@@ -412,13 +375,16 @@ void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
{
mmanager->servo_values[servo_id].enabled=0x00;
/* add an operation to enable the servo */
- if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ if(mmanager->servo_configs[servo_id]!=0x00000000)
{
- mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,P_TORQUE_ENABLE,1,&data);
- dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ {
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
+ dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ }
+ else
+ dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
- else
- dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
}
@@ -430,12 +396,6 @@ unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char s
return 0x00;
}
-void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset)
-{
- if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
- mmanager->servo_values[servo_id].offset=offset;
-}
-
void mmanager_add_module(TMotionManager *mmanager,TMotionModule *module)
{
if(module->id!=MM_NONE)
diff --git a/dynamixel_manager/src/modules/motion_pages.c b/dynamixel_manager/src/modules/motion_pages.c
index bf4f7d8a10551d3545493e0aa698a5088e136666..34bec5e5cb174d3825a84f1d76b7fcb25c88fad5 100755
--- a/dynamixel_manager/src/modules/motion_pages.c
+++ b/dynamixel_manager/src/modules/motion_pages.c
@@ -42,4 +42,4 @@ inline unsigned char pages_get_slope(TPage *page,unsigned char servo_id)
return 0x01<<(page->header.slope[servo_id]&0x0F);
}
-TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak)) ;
+TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak));
diff --git a/dynamixel_manager/test/Makefile b/dynamixel_manager/test/Makefile
index 5ebaf504ae096678463832ddd32da27cbd77bb57..76bfa2cd86f2ccf8ed4898b663537f42595ef0eb 100755
--- a/dynamixel_manager/test/Makefile
+++ b/dynamixel_manager/test/Makefile
@@ -7,10 +7,14 @@ TARGET_FILES+=$(wildcard ../src/modules/*.c)
TARGET_FILES+=$(wildcard ../src/*.c)
TARGET_FILES+=$(wildcard ../../utils/src/*.c)
TARGET_FILES+=$(wildcard ../../comm/src/*.c)
+TARGET_FILES+=$(wildcard ../../memory/src/*.c)
+TARGET_FILES+=$(wildcard ../../scheduler/src/*.c)
BUILD_PATH=build
BIN_PATH=bin
-INCLUDE_DIRS = -I../include -I../include/modules -I../../utils/include -I../../comm/include -I../../dynamixel_base/include
+INCLUDE_DIRS = -I../include -I../include/modules -I../../utils/include -I../../comm/include -I ../../memory/include -I../../dynamixel_base/include -I../../scheduler/include
+
+MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8 -DEEPROM_SIZE=64 -DRAM_SIZE=1024
CC = gcc
@@ -26,19 +30,25 @@ make_dirs:
mkdir -p $(BIN_PATH)
$(BUILD_PATH)/%.o: %.c
- $(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
$(BUILD_PATH)/%.o: ../src/%.c
- $(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
$(BUILD_PATH)/%.o: ../src/modules/%.c
- $(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
$(BUILD_PATH)/%.o: ../../utils/src/%.c
- $(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
$(BUILD_PATH)/%.o: ../../comm/src/%.c
- $(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../../memory/src/%.c
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../../scheduler/src/%.c
+ $(CC) -c -g $(INCLUDE_DIRS) $(MACROS) -o $@ $<
$(OUT_FILE): make_dirs $(TEST_OBJS)
$(CC) -g $(TEST_OBJS) -lpthread --output $@
diff --git a/dynamixel_manager/test/dyn_man_test.c b/dynamixel_manager/test/dyn_man_test.c
index ccaf642ef95ed60437a0632979739dd30f405b56..db11313e2cf84aef072d3d9b6a63202ac44fba79 100644
--- a/dynamixel_manager/test/dyn_man_test.c
+++ b/dynamixel_manager/test/dyn_man_test.c
@@ -6,10 +6,15 @@
#include "motion_manager.h"
#include "action.h"
#include "dyn_servos.h"
+#include "scheduler.h"
TDynamixelMaster dyn_master1;
TDynamixelMaster dyn_master2;
TDynManager manager;
+TMemory memory;
+TScheduler scheduler;
+
+unsigned int SystemCoreClock=168000000;
// timer functions
unsigned long long int time_get_counts(void)
@@ -92,6 +97,12 @@ void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *
}
}
+unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(length==2)
+ dyn_master_read_word(master,id,address,(unsigned short int *)data);
+}
+
unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
{
if(master==&dyn_master1)
@@ -137,11 +148,11 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns
unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
{
- if(master==&dyn_master1)
- printf("Master 1\n");
- else
- printf("Master 2\n");
- printf(" read operation to servo %d at address %d with length %d\n",id,address,length);
+// if(master==&dyn_master1)
+// printf("Master 1\n");
+// else
+// printf("Master 2\n");
+// printf(" read operation to servo %d at address %d with length %d\n",id,address,length);
return DYN_SUCCESS;
}
@@ -155,14 +166,14 @@ unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned cha
{
unsigned short int i;
- if(master==&dyn_master1)
- printf("Master 1\n");
- else
- printf("Master 2\n");
- printf(" write operation to servo %d at address %d with length %d\n",id,address,length);
- printf(" data:\n");
- for(i=0;i<length;i++)
- printf(" 0x%x\n",data[i]);
+// if(master==&dyn_master1)
+// printf("Master 1\n");
+// else
+// printf("Master 2\n");
+// printf(" write operation to servo %d at address %d with length %d\n",id,address,length);
+// printf(" data:\n");
+// for(i=0;i<length;i++)
+// printf(" 0x%x\n",data[i]);
return DYN_SUCCESS;
}
@@ -176,23 +187,23 @@ unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char
{
unsigned short int i,j;
- if(master==&dyn_master1)
- printf("Master 1\n");
- else
- printf("Master 2\n");
- printf(" sync write operation at address %d with length %d\n",address,length);
- for(j=0;j<num;j++)
- {
- printf(" servo %d data:\n",ids[j]);
- for(i=0;i<length;i++)
- printf(" 0x%x\n",data[j][i]);
- }
-/* if(master==&dyn_master1 && address==P_CW_COMPLIANCE_SLOPE && length==4)
+// if(master==&dyn_master1)
+// printf("Master 1\n");
+// else
+// printf("Master 2\n");
+// printf(" sync write operation at address %d with length %d\n",address,length);
+// for(j=0;j<num;j++)
+// {
+// printf(" servo %d data:\n",ids[j]);
+// for(i=0;i<length;i++)
+// printf(" 0x%x\n",data[j][i]);
+// }
+ if(master==&dyn_master1 && address==ax_reg[cw_comp_slope].address && length==4)
{
for(i=0;i<num;i++)
printf("%d,",data[i][2]+(data[i][3]<<8));
printf("\n");
- }*/
+ }
return DYN_SUCCESS;
}
@@ -206,13 +217,13 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
{
unsigned short int i;
- if(master==&dyn_master1)
- printf("Master 1\n");
- else
- printf("Master 2\n");
- printf(" sync read operation at address %d with length %d for servos:\n",address,length);
- for(i=0;i<num;i++)
- printf(" servo %d:\n",ids[i]);
+// if(master==&dyn_master1)
+// printf("Master 1\n");
+// else
+// printf("Master 2\n");
+// printf(" sync read operation at address %d with length %d for servos:\n",address,length);
+// for(i=0;i<num;i++)
+// printf(" servo %d:\n",ids[i]);
return DYN_SUCCESS;
}
@@ -226,17 +237,17 @@ unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char
{
unsigned short int i,j;
- if(master==&dyn_master1)
- printf("Master 1\n");
- else
- printf("Master 2\n");
- printf(" bulk write operation:\n");
- for(j=0;j<num;j++)
- {
- printf(" servo %d at address %d with length %d and data:\n",ids[j],address[j],length[j]);
- for(i=0;i<length[j];i++)
- printf(" 0x%x\n",data[j][i]);
- }
+// if(master==&dyn_master1)
+// printf("Master 1\n");
+// else
+// printf("Master 2\n");
+// printf(" bulk write operation:\n");
+// for(j=0;j<num;j++)
+// {
+// printf(" servo %d at address %d with length %d and data:\n",ids[j],address[j],length[j]);
+// for(i=0;i<length[j];i++)
+// printf(" 0x%x\n",data[j][i]);
+// }
return DYN_SUCCESS;
}
@@ -250,13 +261,13 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
{
unsigned short int i,j;
- if(master==&dyn_master1)
- printf("Master 1\n");
- else
- printf("Master 2\n");
- printf(" bulk read operation:\n");
- for(j=0;j<num;j++)
- printf(" servo %d at address %d with length %d\n",ids[j],address[j],length[j]);
+// if(master==&dyn_master1)
+// printf("Master 1\n");
+// else
+// printf("Master 2\n");
+// printf(" bulk read operation:\n");
+// for(j=0;j<num;j++)
+// printf(" servo %d at address %d with length %d\n",ids[j],address[j],length[j]);
return DYN_SUCCESS;
}
@@ -270,18 +281,14 @@ int main(void)
{
int i;
TMotionManager mmanager;
+ TActionMModule action;
TTime dyn_master1_timer;
TComm dyn_master1_comm;
TTime dyn_master2_timer;
TComm dyn_master2_comm;
- // init modules
- mmanager_init(&mmanager);
- action_init();
- mmanager_add_module(&mmanager,action_get_module());
- dyn_module_set_period(mmanager_get_dyn_module(&mmanager),1);
- dyn_module_enable(mmanager_get_dyn_module(&mmanager));
-
+ scheduler_init(&scheduler,1,100);
+
// init masters
time_init(&dyn_master1_timer,time_get_counts_per_us(),time_get_counts);
comm_init(&dyn_master1_comm,0x01,&dyn_master1_timer);
@@ -290,10 +297,17 @@ int main(void)
comm_init(&dyn_master2_comm,0x01,&dyn_master2_timer);
dyn_master_init(&dyn_master2,&dyn_master2_comm,DYN_VER2);
- dyn_manager_init(&manager);
+ dyn_manager_init(&manager,&memory,&scheduler,SCHED_CH1,0,64);
+ dyn_manager_set_period(&manager,14);
dyn_manager_add_master(&manager,&dyn_master1);
dyn_manager_add_master(&manager,&dyn_master2);
+ // init modules
+ mmanager_init(&mmanager,&memory,16,128);
+ action_init(&action,&memory,256);
+ mmanager_add_module(&mmanager,action_get_module(&action));
dyn_manager_add_module(&manager,mmanager_get_dyn_module(&mmanager));
+ dyn_module_set_period(mmanager_get_dyn_module(&mmanager),1);
+ dyn_module_enable(mmanager_get_dyn_module(&mmanager));
dyn_manager_scan(&manager);
/* enable all servos and assign them to the ACTION module */
@@ -303,12 +317,12 @@ int main(void)
mmanager_enable_servo(&mmanager,i);
}
/* load and start a page */
- if(action_load_page(9))
- action_start_page();
+ if(action_load_page(&action,7))
+ action_start_page(&action);
- for(i=0;i<10;i++)
+ for(i=0;i<100;i++)
{
- printf("***** Iteration %d *****\n",i);
+// printf("***** Iteration %d *****\n",i);
dyn_manager_loop(&manager);
}
}
diff --git a/memory/include/memory.h b/memory/include/memory.h
index 7eec4003a4352025704b164f72c58ec60afd538e..9e26965f60f8333c2002cda835427478d06717af 100644
--- a/memory/include/memory.h
+++ b/memory/include/memory.h
@@ -19,7 +19,6 @@ typedef struct
{
unsigned char num_mem_modules;
TMemModule *mem_modules[MAX_NUM_MEM_MODULES];
- unsigned char device_id;
unsigned short int (*eeprom_write_data)(unsigned short int address,unsigned short int data);
unsigned short int (*eeprom_read_data)(unsigned short int address,unsigned short int *data);
unsigned char data[RAM_SIZE];
@@ -27,7 +26,7 @@ typedef struct
unsigned char total_ram;
}TMemory;
-void mem_init(TMemory *memory,unsigned char device_id);
+void mem_init(TMemory *memory);
void mem_initialize_data(TMemory *memory);
unsigned char mem_add_module(TMemory *memory, TMemModule *module);
void mem_do_write(TMemory *memory,unsigned short int start_address,unsigned short int length,unsigned char *data);
diff --git a/memory/include/ram.h b/memory/include/ram.h
index ef2325be11b6a051e479edb100e92ec326d1f876..2b6696a7820622e7025238d5e9ec6efed3eac4b0 100644
--- a/memory/include/ram.h
+++ b/memory/include/ram.h
@@ -14,11 +14,11 @@ extern "C" {
#define RAM_BAD_ACCESS -4
unsigned char ram_init(TMemory *memory);
-inline void ram_read_byte(TMemory *memory,unsigned short int address, unsigned char *data)
+static inline void ram_read_byte(TMemory *memory,unsigned short int address, unsigned char *data)
{
(*data)=memory->data[address];
}
-inline void ram_read_word(TMemory *memory,unsigned short int address, unsigned short int *data)
+static inline void ram_read_word(TMemory *memory,unsigned short int address, unsigned short int *data)
{
(*data)=memory->data[address];
(*data)+=memory->data[address+1]*256;
@@ -29,7 +29,7 @@ unsigned char ram_clear_bit(TMemory *memory,unsigned short int address, unsigned
unsigned char ram_write_byte(TMemory *memory,unsigned short int address, unsigned char data);
unsigned char ram_write_word(TMemory *memory,unsigned short int address, unsigned short int data);
unsigned char ram_write_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data);
-inline unsigned char ram_in_range(unsigned short int reg,unsigned short int address,unsigned short int length)
+static inline unsigned char ram_in_range(unsigned short int reg,unsigned short int address,unsigned short int length)
{
if(reg>=address && reg<(address+length))
return 0x01;
diff --git a/memory/src/memory.c b/memory/src/memory.c
index af30fbf36f18d9c31e08b50fa672aa12e2b4cf57..a2c9e59c2fdc9d6df8736b5f27c3460dae4ef171 100644
--- a/memory/src/memory.c
+++ b/memory/src/memory.c
@@ -17,7 +17,7 @@ unsigned char mem_in_window(unsigned short int mem_start_address,unsigned short
}
/* public functions */
-void mem_init(TMemory *memory,unsigned char device_id)
+void mem_init(TMemory *memory)
{
unsigned char i;
@@ -26,7 +26,6 @@ void mem_init(TMemory *memory,unsigned char device_id)
memory->mem_modules[i]=0x00000000;
memory->eeprom_write_data=0x00000000;
memory->eeprom_read_data=0x00000000;
- memory->device_id=device_id;
/* initialize internal variables */
memory->total_eeprom=0;
@@ -45,7 +44,7 @@ void mem_initialize_data(TMemory *memory)
{
for(i=0;i<EEPROM_SIZE;i++)
{
- if(memory->eeprom_read_data((memory->device_id<<8)+i,&data))
+ if(memory->eeprom_read_data(i,&data))
eeprom_data[i]=0x00;
else
eeprom_data[i]=data&0x00FF;
@@ -192,7 +191,7 @@ void mem_do_write(TMemory *memory,unsigned short int start_address,unsigned shor
mem_module->write_cmd(mem_module->data,actual_address,actual_length,&data[actual_address-start_address]);
if(memory->eeprom_write_data!=0x00000000)
for(k=actual_address;k<(actual_address+actual_length);k++)
- memory->eeprom_write_data((memory->device_id<<8)+k,data[k-start_address]);
+ memory->eeprom_write_data(k,data[k-start_address]);
}
}
}
diff --git a/scheduler/include/scheduler.h b/scheduler/include/scheduler.h
index 0effa946db7e27abd75e62ddef208306c0f7b5f1..40b835104285c1c424fd69d12353fc00fb5b77f0 100644
--- a/scheduler/include/scheduler.h
+++ b/scheduler/include/scheduler.h
@@ -32,6 +32,7 @@ typedef struct{
unsigned char period_ms;
unsigned int pulse;
unsigned char enabled;
+ unsigned char one_shot;
void *data;
}TSchedulerChannel;
@@ -48,8 +49,10 @@ void scheduler_init(TScheduler *scheduler,unsigned char num_channels, unsigned s
void scheduler_interrupt(TScheduler *scheduler,sched_channel_t channel_id);
unsigned short int scheduler_get_pulse(TScheduler *scheduler, sched_channel_t channel_id);
void scheduler_set_channel(TScheduler *scheduler,sched_channel_t channel_id, void (*function)(void *data), unsigned char period_ms,void *data);
+void scheduler_set_channel_one_shot(TScheduler *scheduler,sched_channel_t channel_id, void (*function)(void *data), unsigned char delay_ms,void *data);
void scheduler_enable_channel(TScheduler *scheduler,sched_channel_t channel_id);
void scheduler_change_period(TScheduler *scheduler,sched_channel_t channel_id,unsigned char period_ms);
+void scheduler_change_delay(TScheduler *scheduler,sched_channel_t channel_id,unsigned char delay_ms);
void scheduler_disable_channel(TScheduler *scheduler,sched_channel_t channel_id);
#ifdef __cplusplus
diff --git a/scheduler/src/scheduler.c b/scheduler/src/scheduler.c
index d4f420decf26a9cba28e4f6439e294d40fbfd79c..a7501520384d9cd7af5c2fb34c72ddd4f947052c 100644
--- a/scheduler/src/scheduler.c
+++ b/scheduler/src/scheduler.c
@@ -29,6 +29,7 @@ void scheduler_init(TScheduler *scheduler,unsigned char num_channels, unsigned s
scheduler->channels[i].function=0x00000000;
scheduler->channels[i].period_ms=0;
scheduler->channels[i].enabled=0x00;
+ scheduler->channels[i].one_shot=0x00;
}
scheduler->num_channels=num_channels;
scheduler->prescaler=prescaler;
@@ -41,10 +42,19 @@ void scheduler_interrupt(TScheduler *scheduler,sched_channel_t channel_id)
{
if(channel_id<scheduler->num_channels)
{
- if(scheduler->set_pulse!=0x00000000)
- scheduler->set_pulse(scheduler_get_channel(channel_id),scheduler->channels[channel_id].pulse,0x01);
- if(scheduler->channels[channel_id].function!=0x00000000)
- scheduler->channels[channel_id].function(scheduler->channels[channel_id].data);
+ if(scheduler->channels[channel_id].one_shot==0x00)
+ {
+ if(scheduler->set_pulse!=0x00000000)
+ scheduler->set_pulse(scheduler_get_channel(channel_id),scheduler->channels[channel_id].pulse,0x01);
+ if(scheduler->channels[channel_id].function!=0x00000000)
+ scheduler->channels[channel_id].function(scheduler->channels[channel_id].data);
+ }
+ else
+ {
+ if(scheduler->channels[channel_id].function!=0x00000000)
+ scheduler->channels[channel_id].function(scheduler->channels[channel_id].data);
+ scheduler_disable_channel(scheduler,channel_id);
+ }
}
}
@@ -62,6 +72,25 @@ void scheduler_set_channel(TScheduler *scheduler,sched_channel_t channel_id, voi
scheduler->channels[channel_id].period_ms=period_ms;
scheduler->channels[channel_id].pulse=scheduler_ms_to_pulse(scheduler,period_ms);
scheduler->channels[channel_id].data=data;
+ scheduler->channels[channel_id].one_shot=0x00;
+ }
+}
+
+void scheduler_set_channel_one_shot(TScheduler *scheduler,sched_channel_t channel_id, void (*function)(void *data), unsigned char delay_ms,void *data)
+{
+ if(channel_id<scheduler->num_channels)
+ {
+ if(scheduler->channels[channel_id].enabled==0x01)
+ {
+ if(scheduler->stop!=0x00000000)
+ scheduler->stop(channel_id);
+ scheduler->channels[channel_id].enabled=0x00;
+ }
+ scheduler->channels[channel_id].function=function;
+ scheduler->channels[channel_id].period_ms=delay_ms;
+ scheduler->channels[channel_id].pulse=scheduler_ms_to_pulse(scheduler,delay_ms);
+ scheduler->channels[channel_id].data=data;
+ scheduler->channels[channel_id].one_shot=0x01;
}
}
@@ -83,7 +112,25 @@ void scheduler_enable_channel(TScheduler *scheduler,sched_channel_t channel_id)
void scheduler_change_period(TScheduler *scheduler,sched_channel_t channel_id,unsigned char period_ms)
{
if(channel_id<scheduler->num_channels)
- scheduler->channels[channel_id].period_ms=period_ms;
+ {
+ if(scheduler->channels[channel_id].one_shot==0x00)
+ {
+ scheduler->channels[channel_id].period_ms=period_ms;
+ scheduler->channels[channel_id].pulse=scheduler_ms_to_pulse(scheduler,period_ms);
+ }
+ }
+}
+
+void scheduler_change_delay(TScheduler *scheduler,sched_channel_t channel_id,unsigned char delay_ms)
+{
+ if(channel_id<scheduler->num_channels)
+ {
+ if(scheduler->channels[channel_id].one_shot==0x01)
+ {
+ scheduler->channels[channel_id].period_ms=delay_ms;
+ scheduler->channels[channel_id].pulse=scheduler_ms_to_pulse(scheduler,delay_ms);
+ }
+ }
}
void scheduler_disable_channel(TScheduler *scheduler,sched_channel_t channel_id)