From 0f4a04e5cea46cd93e312b454b559e01d7a2e656 Mon Sep 17 00:00:00 2001 From: Irene Garcia Camacho <igarcia@iri.upc.edu> Date: Mon, 3 Apr 2017 12:42:51 +0200 Subject: [PATCH] Added smart charger's module to the darwin_robot_driver Functions to access the smart charger module has been added to the driver of the Darwin robot. Allows to enable or disable module, set and get period, get smart charger's data (average time to empty and to full and battery status), read smart charger's control register (smart charger detected and/or enabled), write limit current. --- src/CMakeLists.txt | 5 +- src/darwin_robot.cpp | 134 +++++++++++++++++++++++++++++++++++++++++++ src/darwin_robot.h | 56 ++++++++++++++++++ 3 files changed, 193 insertions(+), 2 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 52c0d68..d2466f0 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 578a6cb..a2b3b0b 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 597128f..d7bd0e0 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(); }; -- GitLab