diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 52c0d68dde1cb584907981a20bb9e95182c56b6e..d2466f05dc71ec5780be6225d4023709c78f2ec6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,4 +1,5 @@
-SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
+#SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
+SET(DARWIN_FW_PATH ~/Desktop/IRI/humanoids/darwin_stm32_fw)
ADD_SUBDIRECTORY(xml)
IF(HAVE_XSD)
@@ -38,10 +39,10 @@ ENDIF(EIGEN3_FOUND)
# add the necessary include directories
INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(${DARWIN_FW_PATH}/include)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
-INCLUDE_DIRECTORIES(${DARWIN_FW_PATH}/include)
# create the shared library
ADD_LIBRARY(darwin_robot SHARED ${robot_sources} ${XSD_SOURCES})
ADD_DEPENDENCIES(darwin_robot xsd_files_gen)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 578a6cbe9149eb43688c75e4834e8708f43fc838..a2b3b0b2e804a04dee72ee7b054ee1272b8ea9a4 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -80,6 +80,9 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
/* get the current action status */
this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
+ /* get the current smart charger status (detected or not)*/
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status);
+
}
else
{
@@ -1776,6 +1779,137 @@ void CDarwinRobot::head_get_current_target(double *pan,double *tilt)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
+//Smart charger interface
+void CDarwinRobot::smart_charger_enable(void)
+{
+ if(this->robot_device!=NULL)
+ {
+ this->smart_charger_status|=SMART_CHARGER_EN;
+ this->robot_device->write_byte_register(DARWIN_SMART_CHARGER_CNTRL,this->smart_charger_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+/* Disables the smart charger module
+ */
+void CDarwinRobot::smart_charger_disable(void)
+{
+ if(this->robot_device!=NULL)
+ {
+ this->smart_charger_status&=(~SMART_CHARGER_EN);
+ this->robot_device->write_byte_register(DARWIN_SMART_CHARGER_CNTRL,this->smart_charger_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+/* Set period of reading operation
+ */
+void CDarwinRobot::smart_charger_set_period(unsigned short int period_ms)
+{
+ unsigned short period;
+
+ if(this->robot_device!=NULL)
+ {
+ period=period_ms;
+ this->robot_device->write_word_register(DARWIN_SMART_CHARGER_PERIOD_L,period);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+unsigned short int CDarwinRobot::smart_charger_get_period(void)
+{
+ unsigned short int period;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_SMART_CHARGER_PERIOD_L,&period);
+ return period;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+TChargerData CDarwinRobot::smart_charger_get_data(void)
+{
+ // unsigned short charger_data[3];
+ TChargerData smart_charger_data;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_registers(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L,(uint8_t *)&smart_charger_data,6);
+ return smart_charger_data;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+
+unsigned int CDarwinRobot::smart_charger_read_time_empty(void)
+{
+ unsigned char data[2];
+ unsigned int avg_time_empty;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L,data);
+ avg_time_empty=(data[0]+(data[1]<<8));
+ return avg_time_empty;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+unsigned char CDarwinRobot::smart_charger_read_control(void)
+{
+ unsigned char smart_charger_control;
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&smart_charger_control);
+ return smart_charger_control;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+unsigned char CDarwinRobot::smart_charger_read_status(void)
+{
+ unsigned char smart_charger_stat;
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_STATUS,&smart_charger_stat);
+ return smart_charger_stat;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::smart_charger_write_limit_current(unsigned short int limit_current)
+{
+ //unsigned short limit_current;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->write_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,limit_current);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+/* void CDarwinRobot::smart_charger_read_limit_current(double limit_current)
+{
+ unsigned short limit_current;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(BATTERY_INPUT_MAX_CURRENT_L,&limit_current);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+*/
CDarwinRobot::~CDarwinRobot()
{
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 597128f3f46817be92cb85f108a7432821483404..d7bd0e08f1f785255d5a11f158911e46e2a29de8 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -40,6 +40,18 @@ typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH
typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
+//smart charger read data
+#pragma pack (push, 1)
+typedef struct{
+// uint16_t limit_current;
+// uint8_t rel_soc;
+// uint8_t abs_soc;
+unsigned short int avg_time_empty;
+unsigned short int avg_time_full;
+unsigned short int bat_status;
+}TChargerData;
+#pragma pack (pop)
+
class CDarwinRobot
{
private:
@@ -52,6 +64,8 @@ class CDarwinRobot
unsigned char manager_status;
/* action status */
unsigned char action_status;
+ //smart charger status
+ unsigned char smart_charger_status;
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
@@ -183,6 +197,48 @@ class CDarwinRobot
bool head_is_tracking(void);
void head_set_new_target(double pan,double tilt);
void head_get_current_target(double *pan,double *tilt);
+
+ // smart charger interface
+/**
+ * \brief Function to enable smart charger module
+ */
+ void smart_charger_enable(void);
+ /**
+ * \brief Function to disable smart charger module
+ */
+ void smart_charger_disable(void);
+ /**
+ * \brief Function to set smart charger's read operation period
+ * \param period_ms Period in ms of smart charger module
+ */
+ void smart_charger_set_period(unsigned short int period_ms);
+ /**
+ * \brief Function to get smart charger's read operation period
+ */
+ unsigned short int smart_charger_get_period(void);
+ /**
+ * \brief Function to get smart charger's data: Battery average time to empty and to full and battery status
+ */
+ TChargerData smart_charger_get_data(void);
+ /**
+ * \brief Function to set smart charger's period
+ * \param limit_current Value of limit current
+ */
+ void smart_charger_write_limit_current(unsigned short int limit_current);
+ /**
+ * \brief Function to get smart charger's control register
+ *
+ * Control register shows if the smart charger is detected or not and enabled or not
+ */
+ unsigned char smart_charger_read_control(void);
+ /**
+ * \brief Function to get smart charger status
+ *
+ * Smart charger status register indicates whether AC and/or battery are present or not
+ */
+ unsigned char smart_charger_read_status(void);
+ unsigned int smart_charger_read_time_empty(void);
+
~CDarwinRobot();
};