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..7f450649c17405a3ebb7b966d0b5cccc7329bcde 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -80,6 +80,11 @@ 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);
+ this->MIN_limit_current=0.255;
+ this->MAX_limit_current=1.0;
+
}
else
{
@@ -1776,6 +1781,187 @@ 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");
+}
+
+bool CDarwinRobot::is_smart_charger_detected(void)
+{
+ unsigned char status;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&status);
+ if(status&SMART_CHARGER_DET)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+bool CDarwinRobot::is_smart_charger_det_and_en(void)
+{
+ unsigned char status;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&status);
+ if(status&SMART_CHARGER_EN && status&SMART_CHARGER_DET)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+
+void CDarwinRobot::smart_charger_set_period(double period)
+{
+ unsigned short int period_ms;
+
+ if(this->robot_device!=NULL)
+ {
+ period_ms=period*1000;
+ if(period_ms>=1400 && period_ms<=1600)
+ this->robot_device->write_word_register(DARWIN_SMART_CHARGER_PERIOD_L,period_ms);
+ else
+ std::cout <<" Invalid period value" << std::endl;
+ //throw CDarwinRobotException(_HERE_,"Invalid period value");
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+
+double CDarwinRobot::smart_charger_get_period(void)
+{
+ unsigned short int period_ms;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_SMART_CHARGER_PERIOD_L,&period_ms);
+
+ return ((double)period_ms)/1000;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+/*
+void CDarwinRobot::smart_charger_get_period(double *period)
+{
+ unsigned short int period_ms;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_SMART_CHARGER_PERIOD_L,&period_ms);
+ *period= ((double)period_ms)/1000;
+ }
+ 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");
+}
+
+//Set limit input current
+void CDarwinRobot::smart_charger_range_current(double min_current, double max_current)
+{
+ this->MAX_limit_current=max_current;
+ this->MIN_limit_current=min_current;
+}
+void CDarwinRobot::smart_charger_set_limit_current(double limit_current)
+{
+ unsigned short int limit_current_ma;
+
+ if(this->robot_device!=NULL)
+ {
+ //if(limit_current > MIN_limit_current && limit_current < MAX_limit_current){
+ if(limit_current > 0.255 && limit_current < 1.0){
+ limit_current_ma=limit_current*1000;
+ this->robot_device->write_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,limit_current_ma);
+ }else{
+ std::cout <<" Invalid current value" << std::endl;
+ }
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
CDarwinRobot::~CDarwinRobot()
{
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 597128f3f46817be92cb85f108a7432821483404..ebbbb1e5f26f406346e7d5002943acff4c21977c 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,9 @@ class CDarwinRobot
unsigned char manager_status;
/* action status */
unsigned char action_status;
+ //smart charger status
+ unsigned char smart_charger_status;
+ double MIN_limit_current, MAX_limit_current;
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
@@ -183,6 +198,60 @@ 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 check if smart charger module is detected
+ */
+ bool is_smart_charger_detected(void);
+ /**
+ * \brief Function to check if smart charger module is detected and enabled
+ */
+ bool is_smart_charger_det_and_en(void);
+/**
+ * \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(double period);
+ /**
+ * \brief Function to get smart charger's read operation period
+ */
+ double 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);
+ /**
+ *
+ */
+ void smart_charger_range_current(double min_current, double max_current);
+ /**
+ * \brief Function to set smart charger's period
+ * \param limit_current Value of limit current
+ */
+ void smart_charger_set_limit_current(double 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();
};
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a21e19c4def6a741116edbd724ad998e58260818..f505e95a9afbfb6bd26af054730e76f18b8bf493 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -38,13 +38,19 @@ ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
-# create an example application
-ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
-# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
+IF(KDL_FOUND)
+ # create an example application
+ ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
+ # link necessary libraries
+ TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
+
+ # create an example application
+ ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
+ # link necessary libraries
+ TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
+ENDIF(KDL_FOUND)
# create an example application
-ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
+ADD_EXECUTABLE(darwin_smart_charger_test darwin_smart_charger_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
-
+TARGET_LINK_LIBRARIES(darwin_smart_charger_test darwin_robot)
\ No newline at end of file
diff --git a/src/examples/darwin_smart_charger_test.cpp b/src/examples/darwin_smart_charger_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b38c28e7dceddc0541cd9749f9a7748f2c87159a
--- /dev/null
+++ b/src/examples/darwin_smart_charger_test.cpp
@@ -0,0 +1,139 @@
+
+/* DRIVER of the DARWIN ROBOT firmware
+ *
+ * Test with the robot darwin with smart charger connected to dynamixel v2, baudrate 1M
+ *
+ * In terminal:
+ * Default period of the smart charger (read smart charger data period)
+ * Smart charger detected, not detected or detected and enabled
+ * Start motion manager (if smart charger detected and enabled, it will execute the read operations)
+ * Battery status: connected, disconnected or connected and charging
+ * Shows average time to full and to empty in min (65535 it means -1)
+ *
+ NOTES: First download darwin_stm32_fw with cm730_fw main code (MakeFile)
+ *
+ *
+ */
+
+
+
+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+
+#include <iostream>
+
+//thread
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
+
+int main(int argc, char *argv[])
+{
+ //unsigned short int period;
+ double period;
+ int control, status;
+ TChargerData charger_data;
+
+ try{
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
+ std::cout << "found darwin controller" << std::endl;
+
+ darwin.gpio_clear_led(LED_TX);
+ darwin.gpio_clear_led(LED_4);
+ darwin.gpio_clear_led(LED_3);
+
+ std::cout << "Holaaaaa! " << std::endl;
+
+//Read/write smart charger period
+ std::cout << "Default smart charger period: " << darwin.smart_charger_get_period() << " seg" << std::endl;
+ std::cout << "Changing smart charger period to 1500ms..." << std::endl;
+ darwin.smart_charger_set_period(1.5);
+ std::cout << "New smart charger period: " << darwin.smart_charger_get_period() << " seg" << std::endl;
+//Write invalid period value
+/* std::cout << "Changing smart charger period to 1000ms" << std::endl;
+ darwin.smart_charger_set_period(1.00);
+ std::cout << "smart charger period (s): " << darwin.smart_charger_get_period() << std::endl;
+*/
+
+//DETECTING AND ENABLING SMART CHARGER
+/* if(darwin.smart_charger_is_detected())
+ std::cout << "smart charger detected" << std::endl;
+*/
+ control=darwin.smart_charger_read_control();
+ if(control==1)
+ std::cout << "smart charger detected" << std::endl;
+ else if(control==0)
+ std::cout << "smart charger not detected" << std::endl;
+
+ std::cout << "Enabling smart charger module..." << std::endl;
+ darwin.smart_charger_enable();
+ control=darwin.smart_charger_read_control();
+ if(control==3)
+ std::cout << "smart charger detected and enabled" << std::endl;
+
+//START MOTION MANAGER
+ darwin.mm_start();
+ if(darwin.mm_is_running())
+ std::cout << "Motion manager is running" << std::endl;
+
+//WRITE EEPROM
+ /* unsigned short int length = 6; //8 bytes - RAM 0x20 to 0x27
+ unsigned short int data_eeprom[3]; //BATTERY_INPUT_CURRENT / BATTERY_CHARGE_CURRENT / BATTERY_CHARGE_VOLTAGE / BATTERY_LIMIT_CURRENT
+ unsigned char *p_eeprom;
+ p_eeprom=(uint8_t *)&data_eeprom;
+ error=dyn_master_read_table(&darwin_dyn_master,id,BATTERY_INPUT_MAX_CURRENT_L,length,p_eeprom);
+ std::cout << "Limit current eeprom: " << data_eeprom[0] << std::endl;
+ std::cout << "Output current eeprom: " << data_eeprom[1] << std::endl;
+ std::cout << "Output voltage eeprom: " << data_eeprom[2] << std::endl;
+ */
+ double max_current=1.0;
+ double min_current=0.255;
+ double limit_current = 0.512; //(A)
+ std::cout << "Setting limit current to 0.512 A..." << std::endl;
+ // darwin.smart_charger_range_current(min_current, max_current);
+ darwin.smart_charger_set_limit_current(limit_current);
+
+
+//READ SMART CHARGER DATA
+// charger_data=darwin.smart_charger_get_data();
+// std::cout << "battery status: " << charger_data.bat_status << std::endl;
+// std::cout << "Avg time to empty: " << charger_data.avg_time_empty << std::endl;
+// std::cout << "Avg time to full: " << charger_data.avg_time_full << std::endl;
+
+
+ while(1){
+ charger_data=darwin.smart_charger_get_data();
+ if(charger_data.bat_status==0)
+ std::cout << "battery status: disconnected " << std::endl;
+ else if(charger_data.bat_status==192)
+ std::cout << "battery status: connected" << std::endl;
+ else if(charger_data.bat_status==128)
+ std::cout << "battery status: connected and charging " << std::endl;
+ //y totalmente cargado o descargado???
+
+ std::cout << "Avg time to empty: " << charger_data.avg_time_empty << " min" << std::endl;
+ std::cout << "Avg time to full: " << charger_data.avg_time_full << " min" << std::endl;
+ std::cout << "---------------------- " << std::endl;
+ //std::cout << "Avg time to empty: " << darwin.smart_charger_read_time_empty() << " min" << std::endl;
+
+
+ //charger status --> batteries detected, AC detected, etc
+ //status=darwin.smart_charger_read_status();
+ //std::cout << "charger status: " << status << std::endl;
+ //3=solo conector
+ //5=solo bateria
+ //7=conector + bateria
+
+
+
+
+ // darwin.gpio_toggle_led(LED_RX); //naranja
+
+ usleep(500000);
+ }
+
+
+
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}