Get the instance to the dynamixel server in the constructor.

src/CMakeLists.txt

 # driver source files
-SET(sources ir_feet.cpp ir_feet_exceptions.cpp)
+SET(sources ir_feet_exceptions.cpp)
 # application header files
 SET(headers ir_feet.h ir_feet_exceptions.h)
 SET(fw_headers ../../ir_feet_fw/include/adc_dma_registers.h ../../ir_feet_fw/include/ir_feet_dyn_slave_registers.h ../../ir_feet_fw/include/thresholds_registers.h ../../ir_feet_fw/include/ir_feet_conf.h)

src/examples/ir_feet_test.cpp

@@ -12,6 +12,7 @@ int main(int argc, char *argv[])
 
   try{
     CIRFeet<CDynamixelServerFTDI> foot_sensor(foot_sensor_name,dyn_serial,115200,0x01);
+    CIRFeet<CDynamixelServerFTDI> foot_sensor2(foot_sensor_name,dyn_serial,115200,0x02);
     for(i=0;i<1000;i++)
     {
       std::cout << "Down left middle sensor: " << foot_sensor.get_sensor_voltage(DOWN_LEFT_MIDDLE_CH) << " threshold set to: " << foot_sensor.get_sensor_threshold(DOWN_LEFT_MIDDLE_CH) << std::endl;

src/ir_feet.h

@@ -32,12 +32,13 @@ class CIRFeet
     void set_sensor_threshold(adc_dma_ch_t channel_id, double threshold);
     bool is_sensor_active(adc_dma_ch_t channel_id);
     std::vector<bool> get_all_sensor_status(void);
-    ~CIRFeet();
+    virtual ~CIRFeet();
 };
 
 template <class T>
 CIRFeet<T>::CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudrate,unsigned char dev_id)
 {
+  this->dyn_server=T::instance();
   this->dyn_device=NULL;
   try{
     this->dyn_server->config_bus(bus_id,baudrate);
@@ -49,6 +50,7 @@ CIRFeet<T>::CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudra
     if(this->dyn_device!=NULL)
       delete this->dyn_device;
     this->dyn_device=NULL;
+    this->dyn_server->free_device(dev_id);
     throw;
   }
 }