diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1cbfb4691a3a6236d84fe96d74da3676d0a7bbef..80314ba847289e5078c2cb69ffa0442d542d3131 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,5 +1,5 @@
# 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)
# locate the necessary dependencies
diff --git a/src/examples/ir_feet_test.cpp b/src/examples/ir_feet_test.cpp
index 7b5346738070c1a2328a374e875b06574accc465..c02d9d375b29ba5e692f651fded482b6f2c74f72 100644
--- a/src/examples/ir_feet_test.cpp
+++ b/src/examples/ir_feet_test.cpp
@@ -1,3 +1,4 @@
+#include "dynamixelserver_ftdi.h"
#include "ir_feet.h"
#include "exceptions.h"
#include <iostream>
@@ -10,7 +11,7 @@ int main(int argc, char *argv[])
unsigned int i=0;
try{
- CIRFeet foot_sensor(foot_sensor_name,dyn_serial,115200,0x01);
+ CIRFeet<CDynamixelServerFTDI> foot_sensor(foot_sensor_name,dyn_serial,115200,0x01);
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;
diff --git a/src/ir_feet.cpp b/src/ir_feet.cpp
deleted file mode 100644
index cb1f1a0abcf983a3bff194a96d50ba274dc59351..0000000000000000000000000000000000000000
--- a/src/ir_feet.cpp
+++ /dev/null
@@ -1,290 +0,0 @@
-#include "ir_feet.h"
-#include "exceptions.h"
-#include "ir_feet_exceptions.h"
-#include "adc_dma_registers.h"
-#include "ir_feet_dyn_slave_registers.h"
-#include "thresholds_registers.h"
-#include <iostream>
-
-CIRFeet::CIRFeet(const std::string& ir_feet_id,unsigned char bus_id,int baudrate,unsigned char dev_id)
-{
- this->dyn_server=CDynamixelServerFTDI::instance();
- this->dyn_device=NULL;
- try{
- this->dyn_server->config_bus(bus_id,baudrate);
- sleep(1);
- this->dyn_device=this->dyn_server->get_device(dev_id,dyn_version2);
- this->ir_feet_id=ir_feet_id;
- }catch(CException &e){
- /* handle exceptions */
- if(this->dyn_device!=NULL)
- delete this->dyn_device;
- this->dyn_device=NULL;
- throw;
- }
-}
-
-CIRFeet::CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudrate,unsigned char dev_id)
-{
- this->dyn_server=CDynamixelServerFTDI::instance();
- this->dyn_device=NULL;
- try{
- this->dyn_server->config_bus(bus_id,baudrate);
- sleep(1);
- this->dyn_device=this->dyn_server->get_device(dev_id,dyn_version1);
- this->ir_feet_id=ir_feet_id;
- }catch(CException &e){
- /* handle exceptions */
- if(this->dyn_device!=NULL)
- delete this->dyn_device;
- this->dyn_device=NULL;
- throw;
- }
-}
-
-void CIRFeet::check_model(void)
-{
- unsigned short int model;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- this->dyn_device->read_word_register(DEVICE_MODEL,&model);
- if(model!=DEFAULT_DEVICE_MODEL)
- throw CIRFeetException(_HERE_,"The connected device is not an IR feet sensor module.");
- }
-}
-
-double CIRFeet::get_sensor_voltage(adc_dma_ch_t channel_id)
-{
- unsigned short voltage,address;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- switch(channel_id)
- {
- case DOWN_LEFT_MIDDLE_CH: address=DOWN_LEFT_MIDDLE_VOLTAGE;
- break;
- case DOWN_LEFT_REAR_CH: address=DOWN_LEFT_REAR_VOLTAGE;
- break;
- case DOWN_ANALOG_CH: address=DOWN_ANALOG_VOLTAGE;
- break;
- case DOWN_LEFT_FRONT_CH: address=DOWN_LEFT_FRONT_VOLTAGE;
- break;
- case DOWN_RIGHT_REAR_CH: address=DOWN_RIGHT_REAR_VOLTAGE;
- break;
- case DOWN_RIGHT_MIDDLE_CH: address=DOWN_RIGHT_MIDDLE_VOLTAGE;
- break;
- case DOWN_RIGHT_FRONT_CH: address=DOWN_RIGHT_FRONT_VOLTAGE;
- break;
- case FRONT_LEFT_CH: address=FRONT_LEFT_VOLTAGE;
- break;
- case FRONT_RIGHT_CH: address=FRONT_RIGHT_VOLTAGE;
- break;
- case FRONT_ANALOG_CH: address=FRONT_ANALOG_VOLTAGE;
- break;
- }
- this->dyn_device->read_word_register(address,&voltage);
- return ((double)voltage)/1000.0;
- }
-}
-
-std::vector<double> CIRFeet::get_all_sensor_voltages(void)
-{
- unsigned short int values[10];
- std::vector<double> voltages;
- unsigned int i=0;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- this->dyn_device->read_registers(DOWN_LEFT_MIDDLE_VOLTAGE,(unsigned char *)values,20);
- voltages.resize(10);
- for(i=0;i<10;i++)
- voltages[i]=((double)values[i])/1000.0;
-
- return voltages;
- }
-}
-
-double CIRFeet::get_sensor_threshold(adc_dma_ch_t channel_id)
-{
- unsigned short int threshold,address;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- switch(channel_id)
- {
- case DOWN_LEFT_MIDDLE_CH: address=DOWN_LEFT_MIDDLE_THRES;
- break;
- case DOWN_LEFT_REAR_CH: address=DOWN_LEFT_REAR_THRES;
- break;
- case DOWN_ANALOG_CH: address=DOWN_ANALOG_THRES;
- break;
- case DOWN_LEFT_FRONT_CH: address=DOWN_LEFT_FRONT_THRES;
- break;
- case DOWN_RIGHT_REAR_CH: address=DOWN_RIGHT_REAR_THRES;
- break;
- case DOWN_RIGHT_MIDDLE_CH: address=DOWN_RIGHT_MIDDLE_THRES;
- break;
- case DOWN_RIGHT_FRONT_CH: address=DOWN_RIGHT_FRONT_THRES;
- break;
- case FRONT_LEFT_CH: address=FRONT_LEFT_THRES;
- break;
- case FRONT_RIGHT_CH: address=FRONT_RIGHT_THRES;
- break;
- case FRONT_ANALOG_CH: address=FRONT_ANALOG_THRES;
- break;
- }
- this->dyn_device->read_word_register(address,&threshold);
- return ((double)threshold)/1000.0;
- }
-}
-
-std::vector<double> CIRFeet::get_all_sensor_thresholds(void)
-{
- unsigned short int values[10];
- std::vector<double> thresholds;
- unsigned int i=0;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- this->dyn_device->read_registers(DOWN_LEFT_MIDDLE_THRES,(unsigned char *)values,10);
- this->dyn_device->read_registers(DOWN_RIGHT_MIDDLE_THRES,(unsigned char *)&values[5],10);
- thresholds.resize(10);
- for(i=0;i<10;i++)
- thresholds[i]=((double)values[i])/1000.0;
-
- return thresholds;
- }
-}
-
-void CIRFeet::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
-{
- unsigned short int value,address;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- switch(channel_id)
- {
- case DOWN_LEFT_MIDDLE_CH: address=DOWN_LEFT_MIDDLE_THRES;
- break;
- case DOWN_LEFT_REAR_CH: address=DOWN_LEFT_REAR_THRES;
- break;
- case DOWN_ANALOG_CH: address=DOWN_ANALOG_THRES;
- break;
- case DOWN_LEFT_FRONT_CH: address=DOWN_LEFT_FRONT_THRES;
- break;
- case DOWN_RIGHT_REAR_CH: address=DOWN_RIGHT_REAR_THRES;
- break;
- case DOWN_RIGHT_MIDDLE_CH: address=DOWN_RIGHT_MIDDLE_THRES;
- break;
- case DOWN_RIGHT_FRONT_CH: address=DOWN_RIGHT_FRONT_THRES;
- break;
- case FRONT_LEFT_CH: address=FRONT_LEFT_THRES;
- break;
- case FRONT_RIGHT_CH: address=FRONT_RIGHT_THRES;
- break;
- case FRONT_ANALOG_CH: address=FRONT_ANALOG_THRES;
- break;
- }
- value=(unsigned short int)(threshold*1000.0);
- this->dyn_device->write_word_register(address,value);
- usleep(1000);
- }
-}
-
-bool CIRFeet::is_sensor_active(adc_dma_ch_t channel_id)
-{
- unsigned short int thresholds;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- this->dyn_device->read_word_register(THRESHOLDS,&thresholds);
- switch(channel_id)
- {
- case DOWN_LEFT_MIDDLE_CH: return ~((thresholds&DOWN_LEFT_MIDDLE_BIT)==0x00);
- case DOWN_LEFT_REAR_CH: return ~((thresholds&DOWN_LEFT_REAR_BIT)==0x00);
- case DOWN_ANALOG_CH: return ~((thresholds&DOWN_ANALOG_BIT)==0x00);
- case DOWN_LEFT_FRONT_CH: return ~((thresholds&DOWN_LEFT_FRONT_BIT)==0x00);
- case DOWN_RIGHT_REAR_CH: return ~((thresholds&DOWN_RIGHT_REAR_BIT)==0x00);
- case DOWN_RIGHT_MIDDLE_CH: return ~((thresholds&DOWN_RIGHT_MIDDLE_BIT)==0x00);
- case DOWN_RIGHT_FRONT_CH: return ~((thresholds&DOWN_RIGHT_FRONT_BIT)==0x00);
- case FRONT_LEFT_CH: return ~((thresholds&FRONT_LEFT_BIT)==0x00);
- case FRONT_RIGHT_CH: return ~((thresholds&FRONT_RIGHT_BIT)==0x00);
- case FRONT_ANALOG_CH: return ~((thresholds&FRONT_ANALOG_BIT)==0x00);
- default: return false;
- }
- }
-}
-
-std::vector<bool> CIRFeet::get_all_sensor_status(void)
-{
- unsigned short int thresholds;
- std::vector<bool> activated;
- unsigned int i=0;
-
- if(this->dyn_device==NULL)
- {
- /* handle exceptions */
- throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
- }
- else
- {
- this->dyn_device->read_word_register(THRESHOLDS,&thresholds);
- activated.resize(10);
- for(i=0;i<10;i++)
- {
- if(thresholds&(0x01<<i))
- activated[i]=true;
- else
- activated[i]=false;
- }
-
- return activated;
- }
-}
-
-CIRFeet::~CIRFeet()
-{
- if(this->dyn_device!=NULL)
- {
- this->dyn_server->free_device(this->dyn_device->get_id());
- delete this->dyn_device;
- this->dyn_device=NULL;
- }
-}
-
diff --git a/src/ir_feet.h b/src/ir_feet.h
index 57336937cd9eb0841d14be8c48ed9526addabbf8..f7c434eecbf01273fc89edebc32b0a4c15ae3ad4 100644
--- a/src/ir_feet.h
+++ b/src/ir_feet.h
@@ -1,25 +1,30 @@
#ifndef _IR_FEET_H
#define _IR_FEET_H
-#include "dynamixelserver_ftdi.h"
#include "dynamixel.h"
#include "threadserver.h"
#include "eventserver.h"
+#include "exceptions.h"
+#include "ir_feet_exceptions.h"
+#include "adc_dma_registers.h"
+#include "ir_feet_dyn_slave_registers.h"
+#include "thresholds_registers.h"
+#include <iostream>
typedef enum {DOWN_LEFT_MIDDLE_CH=0,DOWN_LEFT_REAR_CH,DOWN_ANALOG_CH,DOWN_LEFT_FRONT_CH,
DOWN_RIGHT_REAR_CH,DOWN_RIGHT_MIDDLE_CH,DOWN_RIGHT_FRONT_CH,
FRONT_LEFT_CH,FRONT_RIGHT_CH,FRONT_ANALOG_CH} adc_dma_ch_t;
+template<class T>
class CIRFeet
{
private:
std::string ir_feet_id;
- CDynamixelServerFTDI *dyn_server;
+ T *dyn_server;
CDynamixel *dyn_device;
protected:
void check_model(void);
public:
- CIRFeet(const std::string& ir_feet_id,unsigned char bus_id,int baudrate,unsigned char dev_id);
CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudrate,unsigned char dev_id);
double get_sensor_voltage(adc_dma_ch_t channel_id);
std::vector<double> get_all_sensor_voltages(void);
@@ -31,4 +36,279 @@ class CIRFeet
~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);
+ sleep(1);
+ this->dyn_device=this->dyn_server->get_device(dev_id,dyn_version1);
+ this->ir_feet_id=ir_feet_id;
+ }catch(CException &e){
+ /* handle exceptions */
+ if(this->dyn_device!=NULL)
+ delete this->dyn_device;
+ this->dyn_device=NULL;
+ throw;
+ }
+}
+
+template<class T>
+void CIRFeet<T>::check_model(void)
+{
+ unsigned short int model;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dyn_device->read_word_register(DEVICE_MODEL,&model);
+ if(model!=DEFAULT_DEVICE_MODEL)
+ throw CIRFeetException(_HERE_,"The connected device is not an IR feet sensor module.");
+ }
+}
+
+template<class T>
+double CIRFeet<T>::get_sensor_voltage(adc_dma_ch_t channel_id)
+{
+ unsigned short voltage,address;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ switch(channel_id)
+ {
+ case DOWN_LEFT_MIDDLE_CH: address=DOWN_LEFT_MIDDLE_VOLTAGE;
+ break;
+ case DOWN_LEFT_REAR_CH: address=DOWN_LEFT_REAR_VOLTAGE;
+ break;
+ case DOWN_ANALOG_CH: address=DOWN_ANALOG_VOLTAGE;
+ break;
+ case DOWN_LEFT_FRONT_CH: address=DOWN_LEFT_FRONT_VOLTAGE;
+ break;
+ case DOWN_RIGHT_REAR_CH: address=DOWN_RIGHT_REAR_VOLTAGE;
+ break;
+ case DOWN_RIGHT_MIDDLE_CH: address=DOWN_RIGHT_MIDDLE_VOLTAGE;
+ break;
+ case DOWN_RIGHT_FRONT_CH: address=DOWN_RIGHT_FRONT_VOLTAGE;
+ break;
+ case FRONT_LEFT_CH: address=FRONT_LEFT_VOLTAGE;
+ break;
+ case FRONT_RIGHT_CH: address=FRONT_RIGHT_VOLTAGE;
+ break;
+ case FRONT_ANALOG_CH: address=FRONT_ANALOG_VOLTAGE;
+ break;
+ }
+ this->dyn_device->read_word_register(address,&voltage);
+ return ((double)voltage)/1000.0;
+ }
+}
+
+template<class T>
+std::vector<double> CIRFeet<T>::get_all_sensor_voltages(void)
+{
+ unsigned short int values[10];
+ std::vector<double> voltages;
+ unsigned int i=0;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dyn_device->read_registers(DOWN_LEFT_MIDDLE_VOLTAGE,(unsigned char *)values,20);
+ voltages.resize(10);
+ for(i=0;i<10;i++)
+ voltages[i]=((double)values[i])/1000.0;
+
+ return voltages;
+ }
+}
+
+template<class T>
+double CIRFeet<T>::get_sensor_threshold(adc_dma_ch_t channel_id)
+{
+ unsigned short int threshold,address;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ switch(channel_id)
+ {
+ case DOWN_LEFT_MIDDLE_CH: address=DOWN_LEFT_MIDDLE_THRES;
+ break;
+ case DOWN_LEFT_REAR_CH: address=DOWN_LEFT_REAR_THRES;
+ break;
+ case DOWN_ANALOG_CH: address=DOWN_ANALOG_THRES;
+ break;
+ case DOWN_LEFT_FRONT_CH: address=DOWN_LEFT_FRONT_THRES;
+ break;
+ case DOWN_RIGHT_REAR_CH: address=DOWN_RIGHT_REAR_THRES;
+ break;
+ case DOWN_RIGHT_MIDDLE_CH: address=DOWN_RIGHT_MIDDLE_THRES;
+ break;
+ case DOWN_RIGHT_FRONT_CH: address=DOWN_RIGHT_FRONT_THRES;
+ break;
+ case FRONT_LEFT_CH: address=FRONT_LEFT_THRES;
+ break;
+ case FRONT_RIGHT_CH: address=FRONT_RIGHT_THRES;
+ break;
+ case FRONT_ANALOG_CH: address=FRONT_ANALOG_THRES;
+ break;
+ }
+ this->dyn_device->read_word_register(address,&threshold);
+ return ((double)threshold)/1000.0;
+ }
+}
+
+template<class T>
+std::vector<double> CIRFeet<T>::get_all_sensor_thresholds(void)
+{
+ unsigned short int values[10];
+ std::vector<double> thresholds;
+ unsigned int i=0;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dyn_device->read_registers(DOWN_LEFT_MIDDLE_THRES,(unsigned char *)values,10);
+ this->dyn_device->read_registers(DOWN_RIGHT_MIDDLE_THRES,(unsigned char *)&values[5],10);
+ thresholds.resize(10);
+ for(i=0;i<10;i++)
+ thresholds[i]=((double)values[i])/1000.0;
+
+ return thresholds;
+ }
+}
+
+template<class T>
+void CIRFeet<T>::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
+{
+ unsigned short int value,address;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ switch(channel_id)
+ {
+ case DOWN_LEFT_MIDDLE_CH: address=DOWN_LEFT_MIDDLE_THRES;
+ break;
+ case DOWN_LEFT_REAR_CH: address=DOWN_LEFT_REAR_THRES;
+ break;
+ case DOWN_ANALOG_CH: address=DOWN_ANALOG_THRES;
+ break;
+ case DOWN_LEFT_FRONT_CH: address=DOWN_LEFT_FRONT_THRES;
+ break;
+ case DOWN_RIGHT_REAR_CH: address=DOWN_RIGHT_REAR_THRES;
+ break;
+ case DOWN_RIGHT_MIDDLE_CH: address=DOWN_RIGHT_MIDDLE_THRES;
+ break;
+ case DOWN_RIGHT_FRONT_CH: address=DOWN_RIGHT_FRONT_THRES;
+ break;
+ case FRONT_LEFT_CH: address=FRONT_LEFT_THRES;
+ break;
+ case FRONT_RIGHT_CH: address=FRONT_RIGHT_THRES;
+ break;
+ case FRONT_ANALOG_CH: address=FRONT_ANALOG_THRES;
+ break;
+ }
+ value=(unsigned short int)(threshold*1000.0);
+ this->dyn_device->write_word_register(address,value);
+ usleep(1000);
+ }
+}
+
+template<class T>
+bool CIRFeet<T>::is_sensor_active(adc_dma_ch_t channel_id)
+{
+ unsigned short int thresholds;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dyn_device->read_word_register(THRESHOLDS,&thresholds);
+ switch(channel_id)
+ {
+ case DOWN_LEFT_MIDDLE_CH: return ~((thresholds&DOWN_LEFT_MIDDLE_BIT)==0x00);
+ case DOWN_LEFT_REAR_CH: return ~((thresholds&DOWN_LEFT_REAR_BIT)==0x00);
+ case DOWN_ANALOG_CH: return ~((thresholds&DOWN_ANALOG_BIT)==0x00);
+ case DOWN_LEFT_FRONT_CH: return ~((thresholds&DOWN_LEFT_FRONT_BIT)==0x00);
+ case DOWN_RIGHT_REAR_CH: return ~((thresholds&DOWN_RIGHT_REAR_BIT)==0x00);
+ case DOWN_RIGHT_MIDDLE_CH: return ~((thresholds&DOWN_RIGHT_MIDDLE_BIT)==0x00);
+ case DOWN_RIGHT_FRONT_CH: return ~((thresholds&DOWN_RIGHT_FRONT_BIT)==0x00);
+ case FRONT_LEFT_CH: return ~((thresholds&FRONT_LEFT_BIT)==0x00);
+ case FRONT_RIGHT_CH: return ~((thresholds&FRONT_RIGHT_BIT)==0x00);
+ case FRONT_ANALOG_CH: return ~((thresholds&FRONT_ANALOG_BIT)==0x00);
+ default: return false;
+ }
+ }
+}
+
+template<class T>
+std::vector<bool> CIRFeet<T>::get_all_sensor_status(void)
+{
+ unsigned short int thresholds;
+ std::vector<bool> activated;
+ unsigned int i=0;
+
+ if(this->dyn_device==NULL)
+ {
+ /* handle exceptions */
+ throw CIRFeetException(_HERE_,"The Dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dyn_device->read_word_register(THRESHOLDS,&thresholds);
+ activated.resize(10);
+ for(i=0;i<10;i++)
+ {
+ if(thresholds&(0x01<<i))
+ activated[i]=true;
+ else
+ activated[i]=false;
+ }
+
+ return activated;
+ }
+}
+
+template<class T>
+CIRFeet<T>::~CIRFeet()
+{
+ if(this->dyn_device!=NULL)
+ {
+ this->dyn_server->free_device(this->dyn_device->get_id());
+ delete this->dyn_device;
+ this->dyn_device=NULL;
+ }
+}
+
+
#endif
diff --git a/src/tools/CMakeLists.txt b/src/tools/CMakeLists.txt
index 169bf9d9e9fdc809e919ab2d150efef9ed46f5be..7e5d8c9d4cb7cf41e2af4a7f2565ab7ca015c459 100644
--- a/src/tools/CMakeLists.txt
+++ b/src/tools/CMakeLists.txt
@@ -8,5 +8,5 @@ INCLUDE_DIRECTORIES(${CURSES_INCLUDE_DIR})
ADD_EXECUTABLE(ir_feet_tuner ir_feet_tuner.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(ir_feet_tuner ir_feet)
-TARGET_LINK_LIBRARIES(ir_feet_tuner ${CURSES_LIBRARIES})
+TARGET_LINK_LIBRARIES(ir_feet_tuner ${CURSES_LIBRARIES} ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY})
diff --git a/src/tools/ir_feet_tuner.cpp b/src/tools/ir_feet_tuner.cpp
index 7f4161b0e70b29afa7c9db71cc4a87436aa315dd..4d3d4bb7390c1b818e6e4f0f0d6c775932cd13aa 100644
--- a/src/tools/ir_feet_tuner.cpp
+++ b/src/tools/ir_feet_tuner.cpp
@@ -1,3 +1,4 @@
+#include "dynamixelserver_ftdi.h"
#include "ir_feet.h"
#include "mutex.h"
#include "threadserver.h"
@@ -39,7 +40,7 @@ const char *sensor_names[10]={"DOWN_LEFT_MIDDLE",
// bioloid global attributes
int current_sensor_sel=1;
double increment=0.01;
-CIRFeet *ir_feet=NULL;
+CIRFeet<CDynamixelServerFTDI> *ir_feet=NULL;
// window global attributes
CMutex window_access;
std::string resize_event_id;
@@ -207,7 +208,6 @@ int main(int argc, char *argv[])
CThreadServer *thread_server=CThreadServer::instance();
std::string resize_thread_id;
std::vector<double> thresholds;
- unsigned int row,col;
unsigned char key;
try{
@@ -222,7 +222,7 @@ int main(int argc, char *argv[])
// assign a function to handle window resize events
signal(SIGWINCH, do_resize);
// initialize the robot object
- ir_feet=new CIRFeet("foot",robot_device,115200,0x01);
+ ir_feet=new CIRFeet<CDynamixelServerFTDI>("foot",robot_device,115200,0x01);
// get current thresholds
thresholds=ir_feet->get_all_sensor_thresholds();
// initialize the screen