diff --git a/comm/include/comm.h b/comm/include/comm.h
index 6509ae74248c72b8c108e7efa6b06f407de43989..6084749ba35d81860064ec27926492690011e12d 100644
--- a/comm/include/comm.h
+++ b/comm/include/comm.h
@@ -3,6 +3,10 @@
 #ifndef _COMM_H
 #define _COMM_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "buffer.h"
 #include "stm32_time.h"
 
@@ -708,7 +712,11 @@ comm_error comm_is_receive_done(TComm *dev);
  * \return the error of the last operation or COMM_BUSY if the operation
  *         is still in progress.
  */
-inline unsigned char comm_get_error(TComm *dev);
+inline unsigned char comm_get_error(TComm *dev)
+{
+  return dev->error;
+}
+
 /** 
  * \brief Function to set the communication error
  *
@@ -721,6 +729,13 @@ inline unsigned char comm_get_error(TComm *dev);
  *        behavior is unpredictable. 
  * \param error identifier of the error to be set
  */
-inline void comm_set_error(TComm *dev, unsigned char error);
+inline void comm_set_error(TComm *dev, unsigned char error)
+{
+  dev->error=error;
+}
+
+#ifdef __cplusplus
+}
+#endif
 
 #endif
diff --git a/comm/src/comm.c b/comm/src/comm.c
index b5aa27e5049634c0479ad5f031ae3d88eed59e5e..ad6033066649c35e182b78b06201f62c23af2528 100644
--- a/comm/src/comm.c
+++ b/comm/src/comm.c
@@ -134,7 +134,7 @@ comm_error comm_receive(TComm *dev,unsigned char *data,unsigned short int *lengt
   else
     comm_get_received_data(dev,data,length);
   // get any possible error
-  error=comm_get_error(dev);
+  error=(comm_error)comm_get_error(dev);
 
   return error;
 }
@@ -329,13 +329,3 @@ comm_error comm_is_receive_done(TComm *dev)
       return COMM_ERROR;
   }
 }
-
-unsigned char comm_get_error(TComm *dev)
-{
-  return dev->error;
-}
-
-void comm_set_error(TComm *dev, unsigned char error)
-{
-  dev->error=error;
-}
diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h
index ce76479b70e5bc89402ee07379eef854986570aa..802189f3d4b26142431ed10c6c535c549de8575c 100755
--- a/dynamixel_base/include/dynamixel.h
+++ b/dynamixel_base/include/dynamixel.h
@@ -19,17 +19,26 @@ void dyn_copy_packet(unsigned char *source, unsigned char *destination);
  * \brief
  *
  */
-inline unsigned char dyn_get_id(unsigned char *packet);
+inline unsigned char dyn_get_id(unsigned char *packet)
+{
+  return packet[DYN_ID_OFF];
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_get_length(unsigned char *packet);
+inline unsigned char dyn_get_length(unsigned char *packet)
+{
+  return packet[DYN_LENGTH_OFF];
+}
 /**
  * \brief
  *
  */
-inline TDynInstruction dyn_get_instruction(unsigned char *packet);
+inline TDynInstruction dyn_get_instruction(unsigned char *packet)
+{
+  return (TDynInstruction)packet[DYN_INST_OFF];
+}
 /**
  * \brief
  *
@@ -68,12 +77,18 @@ 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);
+inline unsigned char dyn_get_read_length(unsigned char *packet)
+{
+  return packet[DYN_DATA_OFF+1];
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_get_read_address(unsigned char *packet);
+inline unsigned char dyn_get_read_address(unsigned char *packet)
+{
+  return packet[DYN_DATA_OFF];
+}
 /* write instruction */
 /**
  * \brief
@@ -84,17 +99,23 @@ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char
  * \brief
  *
  */
-inline unsigned char dyn_get_write_address(unsigned char *packet);
+inline unsigned char dyn_get_write_address(unsigned char *packet)
+{
+  return packet[DYN_DATA_OFF];
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_get_write_length(unsigned char *packet);
+inline unsigned char dyn_get_write_length(unsigned char *packet)
+{
+  return packet[DYN_LENGTH_OFF]-3;
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_get_write_data(unsigned char *packet,unsigned char *data);
+unsigned char dyn_get_write_data(unsigned char *packet,unsigned char *data);
 /* registered write instruction */
 /**
  * \brief
@@ -105,12 +126,18 @@ 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);
+inline unsigned char dyn_get_reg_write_address(unsigned char *packet)
+{
+  return packet[DYN_DATA_OFF];
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_get_reg_write_length(unsigned char *packet);
+inline unsigned char dyn_get_reg_write_length(unsigned char *packet)
+{
+  return packet[DYN_LENGTH_OFF]-3;
+}
 /**
  * \brief
  *
@@ -177,7 +204,10 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err
  * \brief
  *
  */
-inline TDynError dyn_get_status_error(unsigned char *packet);
+inline TDynError dyn_get_status_error(unsigned char *packet)
+{
+  return (TDynError)packet[DYN_ERROR_OFF];
+}
 /* read instruction status packet */
 /**
  * \brief
@@ -188,6 +218,9 @@ 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);
+inline unsigned char dyn_get_read_status_id(unsigned char *packet)
+{
+  return packet[DYN_ID_OFF];
+}
 
 #endif
diff --git a/dynamixel_base/include/dynamixel2.h b/dynamixel_base/include/dynamixel2.h
index a84821557ccf3a079fed9793e9b3c7ca3ab96133..81046051606138c7589d56fff22308b4e9289830 100644
--- a/dynamixel_base/include/dynamixel2.h
+++ b/dynamixel_base/include/dynamixel2.h
@@ -20,17 +20,26 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination);
  * \brief
  * 
  */
-inline unsigned char dyn2_get_id(unsigned char *packet);
+inline unsigned char dyn2_get_id(unsigned char *packet)
+{
+  return packet[DYN2_ID_OFF];
+}
 /**
  * \brief
  * 
  */
-inline unsigned short int dyn2_get_length(unsigned char *packet);
+inline unsigned short int dyn2_get_length(unsigned char *packet)
+{
+  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256);
+}
 /**
  * \brief
  * 
  */
-inline TDynInstruction dyn2_get_instruction(unsigned char *packet);
+inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
+{
+  return (TDynInstruction)packet[DYN2_INST_OFF];
+}
 /**
  * \brief
  * 
@@ -69,12 +78,18 @@ 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);
+inline unsigned short int dyn2_get_read_length(unsigned char *packet)
+{
+  return (packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256);
+}
 /**
  * \brief
  * 
  */
-inline unsigned short int dyn2_get_read_address(unsigned char *packet);
+inline unsigned short int dyn2_get_read_address(unsigned char *packet)
+{
+  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
+}
 /* write instruction */
 /**
  * \brief
@@ -85,17 +100,23 @@ 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);
+inline unsigned short int dyn2_get_write_address(unsigned char *packet)
+{
+  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
+}
 /**
  * \brief
  * 
  */
-inline unsigned short int dyn2_get_write_length(unsigned char *packet);
+inline unsigned short int dyn2_get_write_length(unsigned char *packet)
+{
+  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
+}
 /**
  * \brief
  * 
  */
-inline unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data);
+unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data);
 /* registered write instruction */
 /**
  * \brief
@@ -106,12 +127,18 @@ 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);
+inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
+{
+  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
+}
 /**
  * \brief
  * 
  */
-inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet);
+inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
+{
+  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
+}
 /**
  * \brief
  * 
@@ -200,7 +227,10 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er
  * \brief
  * 
  */
-inline TDynError dyn2_get_status_error(unsigned char *packet);
+inline TDynError dyn2_get_status_error(unsigned char *packet)
+{
+  return (TDynError)packet[DYN2_DATA_OFF];
+}
 /* read instruction status packet */
 /**
  * \brief
@@ -211,6 +241,9 @@ 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);
+inline unsigned char dyn2_get_read_status_id(unsigned char *packet)
+{
+  return packet[DYN2_ID_OFF];
+}
 
 #endif
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 8ee13ea4ea16273afe5b31e7ec1b20ef404d227b..d88a46f0f077b9095a75592f41abd7554f14b87b 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -96,17 +96,31 @@ 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);
+inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
+{
+  if(master!=0x00000000)
+    master->rx_timeout_ms=timeout_ms;
+}
 /**
  * \brief
  *
  */
-inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level);
+inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
+{
+  if(master!=0x00000000)
+    master->return_level=level;
+}
 /**
  * \brief
  *
  */
-inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master);
+inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
+{
+  if(master!=0x00000000)
+    return master->return_level;
+  else 
+    return return_all; 
+}
 /**
  * \brief
  *
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 64f8d751f22e4f766651c3b764d9c2e19a78a6d8..6d92d9fa513ba7c6fd6a64a628b4775a485b68ab 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -122,26 +122,46 @@ unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice
  * \brief
  *
  */
-void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate);
+inline void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
+{
+  if(slave!=0x00000000)
+    slave->set_baudrate(slave->comm_dev,baudrate);
+}
 /**
  * \brief
  *
  */
-inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms);
+inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
+{
+  if(slave!=0x00000000)
+    slave->rx_timeout_ms=timeout_ms;
+}
 /**
  * \brief
  *
  */
-inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave);
+inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
+{
+  if(slave!=0x00000000)
+    return slave->version;
+  else
+    return DYN_VER1;
+}
 /**
  * \brief
  *
  */
-void dyn_slave_start(TDynamixelSlave *slave);
+inline void dyn_slave_start(TDynamixelSlave *slave)
+{
+  scheduler_enable_channel(slave->scheduler,slave->sch_channel);
+}
 /**
  * \brief
  *
  */
-void dyn_slave_stop(TDynamixelSlave *slave);
+inline void dyn_slave_stop(TDynamixelSlave *slave)
+{
+  scheduler_disable_channel(slave->scheduler,slave->sch_channel);
+}
 
 #endif
diff --git a/dynamixel_base/include/dynamixel_slave_device.h b/dynamixel_base/include/dynamixel_slave_device.h
index c43300b02604378f4a2699ffc3c2d29a0989346c..76e3540d87e58c2660dcf68fe50252f0c73e7dd8 100644
--- a/dynamixel_base/include/dynamixel_slave_device.h
+++ b/dynamixel_base/include/dynamixel_slave_device.h
@@ -128,32 +128,62 @@ unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char
  * \brief
  *
  */
-inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address);
+inline void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address)
+{
+  if(device!=0x00000000)
+    device->address=address;
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device);
+inline unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
+{
+  if(device!=0x00000000)
+    return device->address;
+  else
+    return 0xFF;
+}
 /**
  * \brief
  *
  */
-inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay);
+inline void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay)
+{
+  if(device!=0x00000000)
+    device->return_delay=delay;
+}
 /**
  * \brief
  *
  */
-inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device);
+inline unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device)
+{
+  if(device!=0x00000000)
+    return device->return_delay;
+  else
+    return 0xFF;
+}
 /**
  * \brief
  *
  */
-inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level);
+inline void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level)
+{
+  if(device!=0x00000000)
+    device->return_level=level;
+}
 /**
  * \brief
  *
  */
-inline return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device);
+inline return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device)
+{
+  if(device!=0x00000000)
+    return device->return_level;
+  else
+    return return_all;
+}
 /**
  * \brief
  *
@@ -168,6 +198,9 @@ unsigned char dyn_v2_slave_loop(TDynamixelSlaveDevice *device,unsigned char *rx_
  * \brief
  *
  */
-inline TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device);
+inline TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device)
+{
+  return &device->memory;
+}
 
 #endif
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index eb18001bc2560df2627a00445cfe060daf2cc1c3..e25fdb7d54f046e2f9c10587e61bebd062860c46 100755
--- a/dynamixel_base/src/dynamixel.c
+++ b/dynamixel_base/src/dynamixel.c
@@ -40,21 +40,6 @@ void dyn_copy_packet(unsigned char *source, unsigned char *destination)
     destination[i]=source[i];
 }
 
-unsigned char dyn_get_id(unsigned char *packet)
-{
-  return packet[DYN_ID_OFF];
-}
-
-unsigned char dyn_get_length(unsigned char *packet)
-{
-  return packet[DYN_LENGTH_OFF];
-}
-
-TDynInstruction dyn_get_instruction(unsigned char *packet)
-{
-  return packet[DYN_INST_OFF];
-}
-
 unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *destination)
 {
   unsigned char i;
@@ -99,16 +84,6 @@ void dyn_init_read_packet(unsigned char *packet,unsigned char id,unsigned char a
   dyn_set_checksum(packet);
 }
 
-unsigned char dyn_get_read_length(unsigned char *packet)
-{
-  return packet[DYN_DATA_OFF+1];
-}
-
-unsigned char dyn_get_read_address(unsigned char *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-
 /* write instruction */
 void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char address,unsigned char length,unsigned char *data)
 {
@@ -125,16 +100,6 @@ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char
   dyn_set_checksum(packet);
 }
 
-unsigned char dyn_get_write_address(unsigned char *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-
-unsigned char dyn_get_write_length(unsigned char *packet)
-{
-  return packet[DYN_LENGTH_OFF]-3;
-}
-
 unsigned char dyn_get_write_data(unsigned char *packet,unsigned char *data)
 {
   unsigned char i;
@@ -161,16 +126,6 @@ void dyn_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned c
   dyn_set_checksum(packet);
 }
 
-unsigned char dyn_get_reg_write_address(unsigned char *packet)
-{
-  return packet[DYN_DATA_OFF];
-}
-
-unsigned char dyn_get_reg_write_length(unsigned char *packet)
-{
-  return packet[DYN_LENGTH_OFF]-3;
-}
-
 unsigned char dyn_get_reg_write_data(unsigned char *packet,unsigned char *data)
 {
   unsigned char i;
@@ -315,11 +270,6 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err
   dyn_set_checksum(packet); 
 }
 
-TDynError dyn_get_status_error(unsigned char *packet)
-{
-  return packet[DYN_ERROR_OFF];
-}
-
 /* read instruction status packet */
 unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data)
 {
@@ -330,9 +280,3 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data
 
   return packet[DYN_LENGTH_OFF]-0x02;
 }
-
-unsigned char dyn_get_read_status_id(unsigned char *packet)
-{
-  return packet[DYN_ID_OFF];
-}
-
diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c
index e645c1de97f8b22b5f2616528571bbc0c343a6bb..dc26232184df175583bddc9fa84c80a00e06d7a5 100644
--- a/dynamixel_base/src/dynamixel2.c
+++ b/dynamixel_base/src/dynamixel2.c
@@ -79,21 +79,6 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination)
     destination[i]=source[i];
 }
 
-unsigned char dyn2_get_id(unsigned char *packet)
-{
-  return packet[DYN2_ID_OFF];
-}
-
-unsigned short int dyn2_get_length(unsigned char *packet)
-{
-  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256);
-}
-
-TDynInstruction dyn2_get_instruction(unsigned char *packet)
-{
-  return packet[DYN2_INST_OFF];
-}
-
 unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *destination)
 {
   unsigned short int i;
@@ -145,16 +130,6 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short
   dyn2_set_checksum(packet);
 }
 
-unsigned short int dyn2_get_read_length(unsigned char *packet)
-{
-  return (packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256);
-}
-
-unsigned short int dyn2_get_read_address(unsigned char *packet)
-{
-  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
-}
-
 /* write instruction */
 void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
 {
@@ -175,16 +150,6 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor
   dyn2_set_checksum(packet);
 }
 
-unsigned short int dyn2_get_write_address(unsigned char *packet)
-{
-  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
-}
-
-unsigned short int dyn2_get_write_length(unsigned char *packet)
-{
-  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
-}
-
 unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data)
 {
   unsigned short int i,length;
@@ -216,16 +181,6 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned
   dyn2_set_checksum(packet);
 }
 
-unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
-{
-  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
-}
-
-unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
-{
-  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
-}
-
 unsigned short int dyn2_get_reg_write_data(unsigned char *packet,unsigned char *data)
 {
   unsigned short int i,length;
@@ -491,11 +446,6 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er
   dyn2_set_checksum(packet); 
 }
 
-TDynError dyn2_get_status_error(unsigned char *packet)
-{
-  return packet[DYN2_DATA_OFF];
-}
-
 /* read instruction status packet */
 unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char *data)
 {
@@ -507,8 +457,3 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char
 
   return length;
 }
-
-unsigned char dyn2_get_read_status_id(unsigned char *packet)
-{
-  return packet[DYN2_ID_OFF];
-}
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index cb3757420c2021a612d0f3acb13cf9b7b2719a63..ba4ab98207b6b880e91f02862afc9a9009ea59a6 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -743,26 +743,6 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
   master->set_rx_mode();
 }
 
-void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
-{
-  if(master!=0x00000000)
-    master->rx_timeout_ms=timeout_ms;
-}
-
-void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
-{
-  if(master!=0x00000000)
-    master->return_level=level;
-}
-
-return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
-{
-  if(master!=0x00000000)
-    return master->return_level;
-  else 
-    return return_all; 
-}
-
 void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
 {
   unsigned char i;
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index 6afdbe8076451d1cd205c32119123762182443af..f2b8c70999f8a7fe52489e485943ba00fbc64f3a 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -131,7 +131,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
     if(slave->version==DYN_VER1)
     {
       // create the status packet
-      dyn_init_status_packet(slave->tx_buffer,address,error,length,data);
+      dyn_init_status_packet(slave->tx_buffer,address,(TDynError)error,length,data);
       // set the tx mode, if necessary
       slave->set_tx_mode();
       // start transmission by DMA
@@ -140,7 +140,7 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char address,u
     else
     {
       // create the status packet
-      dyn2_init_status_packet(slave->tx_buffer,address,error,length,data);
+      dyn2_init_status_packet(slave->tx_buffer,address,(TDynError)error,length,data);
       // set the tx mode, if necessary
       slave->set_tx_mode();
       // start transmission by DMA
@@ -250,33 +250,3 @@ unsigned char dyn_slave_add_device(TDynamixelSlave *slave, TDynamixelSlaveDevice
   else
     return 0x00;
 }
-
-void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate)
-{
-  slave->set_baudrate(slave->comm_dev,baudrate);
-}
-
-void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
-{
-  if(slave!=0x00000000)
-    slave->rx_timeout_ms=timeout_ms;
-}
-
-TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
-{
-  if(slave!=0x00000000)
-    return slave->version;
-  else
-    return DYN_VER1;
-}
-
-void dyn_slave_start(TDynamixelSlave *slave)
-{
-  scheduler_enable_channel(slave->scheduler,slave->sch_channel);
-}
-
-void dyn_slave_stop(TDynamixelSlave *slave)
-{
-  scheduler_disable_channel(slave->scheduler,slave->sch_channel);
-}
-
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
index c11eecc6446d528b375a4c9dfb659730938d5642..25ae4dfa993643d1b031add57f913495caace089 100644
--- a/dynamixel_base/src/dynamixel_slave_device.c
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -32,7 +32,7 @@ void dyn_slave_device_write_cmd(TDynamixelSlaveDevice *device,unsigned short int
   offset=address-device->eeprom_base_address2;
   if(ram_in_range(device->eeprom_base_address2+RETURN_LEVEL,address,length))
   {
-    dyn_slave_device_set_return_level(device,data[RETURN_LEVEL-offset]);
+    dyn_slave_device_set_return_level(device,(return_level_t)data[RETURN_LEVEL-offset]);
     device->memory.data[device->eeprom_base_address2+RETURN_LEVEL]=data[RETURN_LEVEL-offset];
   }
 }
@@ -419,50 +419,3 @@ unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char
 
   return 0x01;
 }
-
-void dyn_slave_device_set_address(TDynamixelSlaveDevice *device,unsigned char address)
-{
-  if(device!=0x00000000)
-    device->address=address;
-}
-
-unsigned char dyn_slave_device_get_address(TDynamixelSlaveDevice *device)
-{
-  if(device!=0x00000000)
-    return device->address;
-  else
-    return 0xFF;
-}
-
-void dyn_slave_device_set_return_delay(TDynamixelSlaveDevice *device,unsigned char delay)
-{
-  if(device!=0x00000000)
-    device->return_delay=delay;
-}
-
-unsigned char dyn_slave_device_get_return_delay(TDynamixelSlaveDevice *device)
-{
-  if(device!=0x00000000)
-    return device->return_delay;
-  else
-    return 0xFF;
-}
-
-void dyn_slave_device_set_return_level(TDynamixelSlaveDevice *device,return_level_t level)
-{
-  if(device!=0x00000000)
-    device->return_level=level;
-}
-
-return_level_t dyn_slave_device_get_return_level(TDynamixelSlaveDevice *device)
-{
-  if(device!=0x00000000)
-    return device->return_level;
-  else
-    return return_all;
-}
-
-TMemory *dyn_slave_device_get_memory(TDynamixelSlaveDevice *device)
-{
-  return &device->memory;
-}
diff --git a/memory/include/mem_module.h b/memory/include/mem_module.h
index ae07cc71aaddda066c603f50d964257633ce7a82..9dd691e64ea82f77f2424f7d4d0471cfd63f5c5f 100644
--- a/memory/include/mem_module.h
+++ b/memory/include/mem_module.h
@@ -1,6 +1,10 @@
 #ifndef _MEM_MODULE_H
 #define _MEM_MODULE_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #define MAX_NUM_SEGMENTS         4
 
 typedef struct
@@ -24,4 +28,8 @@ void mem_module_init(TMemModule *module);
 unsigned char mem_module_add_ram_segment(TMemModule *module,unsigned short int start_address, unsigned short int length);
 unsigned char mem_module_add_eeprom_segment(TMemModule *module,unsigned short int start_address, unsigned short int length);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/memory/include/memory.h b/memory/include/memory.h
index e01445f9abe68e5c69b74d720e65f5daf1ab78bf..7eec4003a4352025704b164f72c58ec60afd538e 100644
--- a/memory/include/memory.h
+++ b/memory/include/memory.h
@@ -1,6 +1,10 @@
 #ifndef _MEMORY_H
 #define _MEMORY_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "mem_module.h"
 
 #define MAX_NUM_MEM_MODULES       16
@@ -29,4 +33,8 @@ 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);
 void mem_do_read(TMemory *memory,unsigned short int start_address,unsigned short int length,unsigned char *data);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/memory/include/ram.h b/memory/include/ram.h
index 1aa611f1bf238e8b1779bccfdccfcd54236d5579..ef2325be11b6a051e479edb100e92ec326d1f876 100644
--- a/memory/include/ram.h
+++ b/memory/include/ram.h
@@ -14,15 +14,28 @@ 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);
-inline void ram_read_word(TMemory *memory,unsigned short int address, unsigned short int *data);
+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)
+{
+  (*data)=memory->data[address];
+  (*data)+=memory->data[address+1]*256;
+}
 unsigned char ram_read_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data);
 unsigned char ram_set_bit(TMemory *memory,unsigned short int address, unsigned char bit);
 unsigned char ram_clear_bit(TMemory *memory,unsigned short int address, unsigned char bit);
 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);
+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;
+  else
+    return 0x00;
+}
 unsigned char ram_in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length);
 
 #ifdef __cplusplus
diff --git a/memory/src/ram.c b/memory/src/ram.c
index 326ab7fd02830c2506f144983ebadf33558fd864..d135b66c4ca0ad2003fca0587eef9d15f68a083c 100644
--- a/memory/src/ram.c
+++ b/memory/src/ram.c
@@ -7,17 +7,6 @@ unsigned char ram_init(TMemory *memory)
   return 0x01;
 }
 
-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)
-{
-  (*data)=memory->data[address];
-  (*data)+=memory->data[address+1]*256;
-}
-
 unsigned char ram_read_table(TMemory *memory,unsigned short int address, unsigned short int length,unsigned char *data)
 {
   unsigned short int i;
@@ -88,14 +77,6 @@ unsigned char ram_write_table(TMemory *memory,unsigned short int address, unsign
     return RAM_BAD_ADDRESS;
 }
 
-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;
-  else
-    return 0x00;
-}
-
 unsigned char ram_in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length)
 {
   unsigned short int end_reg=start_reg+reg_length-1;
diff --git a/scheduler/include/scheduler.h b/scheduler/include/scheduler.h
index 539b9c502894b20471fa03ca6ba503c445db991d..0effa946db7e27abd75e62ddef208306c0f7b5f1 100644
--- a/scheduler/include/scheduler.h
+++ b/scheduler/include/scheduler.h
@@ -1,6 +1,10 @@
 #ifndef _SCHEDULER_H
 #define _SCHEDULER_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #ifndef SCHEDULER_MAX_CHANNELS
   #define SCHEDULER_MAX_CHANNELS      4
 #endif
@@ -48,5 +52,9 @@ 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_disable_channel(TScheduler *scheduler,sched_channel_t channel_id);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
 
diff --git a/utils/include/buffer.h b/utils/include/buffer.h
index 69a3274385fbd520334684dbfea4e0fe9df39d91..f5ab293d36c92023f8feaee8a5c7e36de6f67550 100644
--- a/utils/include/buffer.h
+++ b/utils/include/buffer.h
@@ -3,6 +3,10 @@
 #ifndef _BUFFER_H
 #define _BUFFER_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #ifndef MAX_BUFFER_LENGTH
   /**
    * \brief Size of the buffer in bytes
@@ -94,7 +98,13 @@ typedef struct
  *        memory is not pre-allocated before calling this function, its 
  *        behavior is unpredictable.
  */
-void buffer_init(TBuffer *buffer);
+inline void buffer_init(TBuffer *buffer)
+{
+  buffer->wr_ptr=0x00000000;
+  buffer->rd_ptr=0x00000000;
+  buffer->num_data=0;
+}
+
 /**
  * \brief Function to empty the buffer structure
  * 
@@ -106,7 +116,12 @@ void buffer_init(TBuffer *buffer);
  *        memory is not pre-allocated before calling this function, its 
  *        behavior is unpredictable.
  */
-void buffer_flush(TBuffer *buffer);
+inline void buffer_flush(TBuffer *buffer)
+{
+  buffer->wr_ptr=buffer->rd_ptr;
+  buffer->num_data=0;
+}
+
 /**
  * \brief Function to get the number of bytes in the buffer
  * 
@@ -121,7 +136,11 @@ void buffer_flush(TBuffer *buffer);
  * \return the number of bytes in the buffer. This is a number between 0 and
  *         MAX_BUFFER_LENGTH.
  */
-inline unsigned short int buffer_get_num_data(TBuffer *buffer);
+inline unsigned short int buffer_get_num_data(TBuffer *buffer)
+{
+  return buffer->num_data;
+}
+
 /**
  * \brief Function to write a single byte to the buffer
  * 
@@ -202,4 +221,8 @@ unsigned short int buffer_write(TBuffer *buffer,unsigned char *data,unsigned sho
  */
 unsigned short int buffer_read(TBuffer *buffer,unsigned char *data,unsigned short int length);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/utils/include/stm32_time.h b/utils/include/stm32_time.h
index 22d7f45268c7274bda8d58ab8f1d41dfcbe700f5..f45ac7bdfd8603e534e8febbfd34068b1add4df1 100644
--- a/utils/include/stm32_time.h
+++ b/utils/include/stm32_time.h
@@ -3,6 +3,10 @@
 #ifndef _STM32_TIME_H
 #define _STM32_TIME_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /**
  * \brief Structure to handle blocking time delays and timeouts using a timer
  *
@@ -125,7 +129,11 @@ unsigned char time_is_timeout(TTime *time);
  *        memory is not pre-allocated before calling this function, its 
  *        behavior is unpredictable.
  */
-void time_cancel_timeout(TTime *time);
+inline void time_cancel_timeout(TTime *time)
+{
+  time->target_time=(unsigned long long int)-1;
+}
+
 /**
  * \brief Function to wait a given time in micro-seconds
  *
@@ -166,4 +174,8 @@ void time_delay_ms(TTime *time,int delay_ms);
  */
 void time_delay_s(TTime *time,int delay_s);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/utils/src/buffer.c b/utils/src/buffer.c
index 8d0045b86c72a21e3e232ca0845338553ded77d1..0c512f087544d9608177da37a62f27ebcdf2e0e8 100644
--- a/utils/src/buffer.c
+++ b/utils/src/buffer.c
@@ -1,24 +1,6 @@
 #include "buffer.h"
 
 /* public functions */
-void buffer_init(TBuffer *buffer)
-{
-  buffer->wr_ptr=0x00000000;
-  buffer->rd_ptr=0x00000000;
-  buffer->num_data=0;
-}
-
-void buffer_flush(TBuffer *buffer)
-{
-  buffer->wr_ptr=buffer->rd_ptr;
-  buffer->num_data=0;
-}
-
-unsigned short int buffer_get_num_data(TBuffer *buffer)
-{
-  return buffer->num_data;
-}
-
 unsigned char buffer_write_byte(TBuffer *buffer,unsigned char data)
 {
   if(buffer->num_data<MAX_BUFFER_LENGTH)
diff --git a/utils/src/stm32_time.c b/utils/src/stm32_time.c
index b4dd4f3e0193a0ef7c079c0ee9dce574b4d27356..865a7660cc20c08dc727e6693a740726d235cb70 100644
--- a/utils/src/stm32_time.c
+++ b/utils/src/stm32_time.c
@@ -32,11 +32,6 @@ unsigned char time_is_timeout(TTime *time)
     return 0x00;
 }
 
-void time_cancel_timeout(TTime *time)
-{
-  time->target_time=(unsigned long long int)-1;
-}
-
 void time_delay_us(TTime *time,int delay_us)
 {
   time_set_timeout(time,delay_us);