diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index a2b3b0b2e804a04dee72ee7b054ee1272b8ea9a4..7f450649c17405a3ebb7b966d0b5cccc7329bcde 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -82,6 +82,8 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
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
@@ -1804,34 +1806,84 @@ void CDarwinRobot::smart_charger_disable(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-/* Set period of reading operation
- */
-void CDarwinRobot::smart_charger_set_period(unsigned short int period_ms)
+bool CDarwinRobot::is_smart_charger_detected(void)
{
- unsigned short period;
+ unsigned char status;
if(this->robot_device!=NULL)
{
- period=period_ms;
- this->robot_device->write_word_register(DARWIN_SMART_CHARGER_PERIOD_L,period);
+ 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;
-unsigned short int CDarwinRobot::smart_charger_get_period(void)
+ 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;
+ 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);
- return period;
+ 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];
@@ -1886,30 +1938,30 @@ unsigned char CDarwinRobot::smart_charger_read_status(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::smart_charger_write_limit_current(unsigned short int limit_current)
+//Set limit input current
+void CDarwinRobot::smart_charger_range_current(double min_current, double max_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");
+ this->MAX_limit_current=max_current;
+ this->MIN_limit_current=min_current;
}
-
-/* void CDarwinRobot::smart_charger_read_limit_current(double limit_current)
+void CDarwinRobot::smart_charger_set_limit_current(double limit_current)
{
- unsigned short limit_current;
+ unsigned short int limit_current_ma;
if(this->robot_device!=NULL)
{
- this->robot_device->read_word_register(BATTERY_INPUT_MAX_CURRENT_L,&limit_current);
+ //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 d7bd0e08f1f785255d5a11f158911e46e2a29de8..ebbbb1e5f26f406346e7d5002943acff4c21977c 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -66,6 +66,7 @@ class CDarwinRobot
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
@@ -199,6 +200,14 @@ class CDarwinRobot
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
*/
@@ -211,20 +220,24 @@ class CDarwinRobot
* \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);
+ void smart_charger_set_period(double period);
/**
* \brief Function to get smart charger's read operation period
*/
- unsigned short int smart_charger_get_period(void);
- /**
+ 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_write_limit_current(unsigned short int limit_current);
+ void smart_charger_set_limit_current(double limit_current);
/**
* \brief Function to get smart charger's control register
*
diff --git a/src/examples/darwin_smart_charger_test.cpp b/src/examples/darwin_smart_charger_test.cpp
index 9e575f30c321adf39106def8cf191d116be2e274..b38c28e7dceddc0541cd9749f9a7748f2c87159a 100644
--- a/src/examples/darwin_smart_charger_test.cpp
+++ b/src/examples/darwin_smart_charger_test.cpp
@@ -28,11 +28,9 @@ std::string robot_device="A603LOBS";
int main(int argc, char *argv[])
{
- unsigned int i=0;
- unsigned short int period;
- int control, status; //1=detected, 3=detected and enabled
- double data[6];
- double d;
+ //unsigned short int period;
+ double period;
+ int control, status;
TChargerData charger_data;
try{
@@ -43,61 +41,97 @@ int main(int argc, char *argv[])
darwin.gpio_clear_led(LED_4);
darwin.gpio_clear_led(LED_3);
+ std::cout << "Holaaaaa! " << std::endl;
+
//Read/write smart charger period
- period=darwin.smart_charger_get_period();
- std::cout << "smart charger period (ms): " << period << std::endl;
-
-/* darwin.smart_charger_set_period(1600);
- std::cout << "smart charger period changed to 1600ms" << std::endl;
- std::cout << "smart charger period (ms): " << darwin.smart_charger_get_period() << std::endl;
-*/
+ 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;
- /* charger_data=darwin.smart_charger_read_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;
- */
- darwin.mm_start();
+//START MOTION MANAGER
+ darwin.mm_start();
if(darwin.mm_is_running())
std::cout << "Motion manager is running" << std::endl;
- charger_data=darwin.smart_charger_read_data();
+
+//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???
+ //y totalmente cargado o descargado???
- //charger status --> batteries detected, AC detected, etc
- status=darwin.smart_charger_read_status();
- std::cout << "charger status: " << status << std::endl;
-
-
-
- while(1){
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); //
+
+
+
+ // darwin.gpio_toggle_led(LED_RX); //naranja
usleep(500000);
}
- //std::cout << "Avg time to empty: " << darwin.smart_charger_read_time_empty() << " min" << std::endl;
- //darwin.gpio_blink_led(LED_RX,1000);
}catch(CException &e){
std::cout << e.what() << std::endl;