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

REmoved the template from the class. Only the constructor and the new close()... 

REmoved the template from the class. Only the constructor and the new close() function are templates.
Moved all non-template functions to the cpp file.
parent 0bf4f243
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 1
− 1
View file @ e38a0bf2
# driver source files
SET(sources ir_feet_exceptions.cpp)
SET(sources ir_feet.cpp ir_feet_exceptions.cpp)
# application header files
SET(headers ir_feet.h ir_feet_exceptions.h)
# locate the necessary dependencies
......
src/examples/ir_feet_test.cpp
+ 2
− 1
View file @ e38a0bf2
......@@ -11,7 +11,7 @@ int main(int argc, char *argv[])
unsigned int i=0;
try{
CIRFeet<CDynamixelServerFTDI> foot_sensor(foot_sensor_name,dyn_serial,115200,0x01);
CIRFeet foot_sensor=CIRFeet<CDynamixelServerFTDI>(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;
......@@ -24,6 +24,7 @@ int main(int argc, char *argv[])
std::cout << "Front right sensor: " << foot_sensor.get_sensor_voltage(FRONT_RIGHT_CH) << " threshold set to: " << foot_sensor.get_sensor_threshold(FRONT_RIGHT_CH) << std::endl;
usleep(100000);
}
foot_sensor.close<CDynamixelServerFTDI>();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
......
src/ir_feet.cpp 0 → 100644
+ 241
− 0
View file @ e38a0bf2
#include "ir_feet.h"
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()
{
}
src/ir_feet.h
+ 9
− 252
View file @ e38a0bf2
......@@ -15,17 +15,15 @@ typedef enum {DOWN_LEFT_MIDDLE_CH=0,DOWN_LEFT_REAR_CH,DOWN_ANALOG_CH,DOWN_LEFT_F
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;
T *dyn_server;
CDynamixel *dyn_device;
protected:
void check_model(void);
public:
CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudrate,unsigned char dev_id);
template<class T> 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);
double get_sensor_threshold(adc_dma_ch_t channel_id);
......@@ -33,18 +31,19 @@ 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);
template<class T> void close(void);
~CIRFeet();
};
template<class T>
CIRFeet<T>::CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudrate,unsigned char dev_id)
CIRFeet::CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudrate,unsigned char dev_id)
{
this->dyn_server=T::instance();
T *dyn_server=T::instance();
this->dyn_device=NULL;
try{
this->dyn_server->config_bus(bus_id,baudrate);
dyn_server->config_bus(bus_id,baudrate);
sleep(1);
this->dyn_device=this->dyn_server->get_device(dev_id,dyn_version1);
this->dyn_device=dyn_server->get_device(dev_id,dyn_version1);
this->ir_feet_id=ir_feet_id;
}catch(CException &e){
/* handle exceptions */
......@@ -56,255 +55,13 @@ CIRFeet<T>::CIRFeet(const std::string& ir_feet_id,std::string& bus_id,int baudra
}
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)
void CIRFeet::close(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;
T *dyn_server=T::instance();
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());
dyn_server->free_device(this->dyn_device->get_id());
delete this->dyn_device;
this->dyn_device=NULL;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment