diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 9438169a651d1a5c8748d06bac30d62e70bd9e15..27c20ef0bcc83a6bb0ed2dbc0e19dbf1948438b7 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -8,7 +8,10 @@
#include "xml/darwin_config.hxx"
#endif
-#define PI 3.14159
+#define PI 3.14159
+
+#define MAX_LIMIT_CURRENT 1.025
+#define MIN_LIMIT_CURRENT 0.255
const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
std::string("j_shoulder_pitch_r"),
@@ -82,8 +85,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;
+ this->min_limit_current=MIN_LIMIT_CURRENT;
+ this->max_limit_current=MAX_LIMIT_CURRENT;
}
else
@@ -1781,7 +1784,8 @@ void CDarwinRobot::head_get_current_target(double *pan,double *tilt)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-//Smart charger interface
+/* Smart charger interface */
+// Enable smart charger module
void CDarwinRobot::smart_charger_enable(void)
{
if(this->robot_device!=NULL)
@@ -1793,8 +1797,7 @@ void CDarwinRobot::smart_charger_enable(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-/* Disables the smart charger module
- */
+// Disable smart charger module
void CDarwinRobot::smart_charger_disable(void)
{
if(this->robot_device!=NULL)
@@ -1806,6 +1809,7 @@ void CDarwinRobot::smart_charger_disable(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
+// Returns true if smart charger is detected
bool CDarwinRobot::is_smart_charger_detected(void)
{
unsigned char status;
@@ -1821,14 +1825,16 @@ bool CDarwinRobot::is_smart_charger_detected(void)
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-bool CDarwinRobot::is_smart_charger_det_and_en(void)
+
+// returns true of smart charger module is enabled
+bool CDarwinRobot::is_smart_charger_enabled(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)
+ if(status&SMART_CHARGER_EN)
return true;
else
return false;
@@ -1845,11 +1851,10 @@ void CDarwinRobot::smart_charger_set_period(double period)
if(this->robot_device!=NULL)
{
period_ms=period*1000;
- if(period_ms>=1400 && period_ms<=1600)
+ if(period_ms>=1000 && period_ms<=5000)
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");
+ throw CDarwinRobotException(_HERE_,"Invalid period value");
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1863,37 +1868,20 @@ double CDarwinRobot::smart_charger_get_period(void)
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");
-}
-*/
-
-/* Obtain time to full, to empty and battery status
- */
+// Obtain time to full, to empty and battery status
TChargerData CDarwinRobot::smart_charger_get_data(void)
{
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);
+ this->robot_device->read_registers(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L,(uint8_t *)&smart_charger_data,sizeof(TChargerData));
return smart_charger_data;
}
else
@@ -1901,10 +1889,10 @@ TChargerData CDarwinRobot::smart_charger_get_data(void)
}
//Set limit input current
-void CDarwinRobot::smart_charger_range_current(double min_current, double max_current)
+void CDarwinRobot::smart_charger_set_range_current(double min_current, double max_current)
{
- this->MAX_limit_current=max_current;
- this->MIN_limit_current=min_current;
+ this->max_limit_current=max_current;
+ this->min_limit_current=min_current;
}
//Get limit current in A
@@ -1915,8 +1903,7 @@ double CDarwinRobot::smart_charger_get_limit_current(void)
if(this->robot_device!=NULL)
{
this->robot_device->read_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,&limit_current_ma);
-
- return ((double)limit_current_ma)/1000;
+ return ((double)(limit_current_ma*2))/1000;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1929,52 +1916,13 @@ void CDarwinRobot::smart_charger_set_limit_current(double limit_current)
if(this->robot_device!=NULL)
{
- //if(limit_current > MIN_limit_current && limit_current < MAX_limit_current){
- if(limit_current > 0.255 && limit_current < 1.025){
+ if(limit_current > min_limit_current && limit_current < max_limit_current)
+ {
limit_current_ma=(limit_current/2)*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");
-}
-
-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 current value");
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 80881293b9bc504b3f1655e44088bc7c1b12cd10..e98f70979a833f1dd1115c8bc201f610b546e2b5 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -42,13 +42,11 @@ typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_gr
//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;
+typedef struct
+{
+ unsigned short int avg_time_empty;
+ unsigned short int avg_time_full;
+ unsigned short int bat_status;
}TChargerData;
#pragma pack (pop)
@@ -64,9 +62,10 @@ class CDarwinRobot
unsigned char manager_status;
/* action status */
unsigned char action_status;
- //smart charger status
+ //smart charger variables
unsigned char smart_charger_status;
- double MIN_limit_current, MAX_limit_current;
+ double min_limit_current;
+ double max_limit_current;
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
@@ -205,9 +204,9 @@ class CDarwinRobot
*/
bool is_smart_charger_detected(void);
/**
- * \brief Function to check if smart charger module is detected and enabled
+ * \brief Function to check if smart charger module is enabled
*/
- bool is_smart_charger_det_and_en(void);
+ bool is_smart_charger_enabled(void);
/**
* \brief Function to enable smart charger module
*/
@@ -227,12 +226,16 @@ class CDarwinRobot
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
+ *
+ * Battery status: indicates whether if AC and/or battery are connected or not
+ * Avg time to full
+ * Avg time to empty
*/
TChargerData smart_charger_get_data(void);
/**
*
*/
- void smart_charger_range_current(double min_current, double max_current);
+ void smart_charger_set_range_current(double min_current, double max_current);
/**
* \brief Function to get limit current in A
*/
@@ -242,19 +245,6 @@ class CDarwinRobot
* \param limit_current Value of limit current in A
*/
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/darwin_smart_charger_test.cpp b/src/examples/darwin_smart_charger_test.cpp
index 44e266d754c9e0b859d90acd0af8ed56e20b53ab..34295316a437f7a114b5be5dad6a53ad7990a272 100644
--- a/src/examples/darwin_smart_charger_test.cpp
+++ b/src/examples/darwin_smart_charger_test.cpp
@@ -39,15 +39,15 @@ int main(int argc, char *argv[])
std::cout << "found darwin controller" << std::endl;
darwin.gpio_clear_led(LED_TX);
- darwin.gpio_clear_led(LED_4);
+ // 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 << "Changing smart charger period to 1400ms..." << std::endl;
+ darwin.smart_charger_set_period(1.4);
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;
@@ -56,10 +56,8 @@ int main(int argc, char *argv[])
*/
//DETECTING AND ENABLING SMART CHARGER
-/* if(darwin.smart_charger_is_detected())
- std::cout << "smart charger detected" << std::endl;
-*/
- control=darwin.smart_charger_read_control();
+
+/* control=darwin.is_smart_charger_read_control();
if(control==1)
std::cout << "smart charger detected" << std::endl;
else if(control==0)
@@ -70,6 +68,16 @@ int main(int argc, char *argv[])
control=darwin.smart_charger_read_control();
if(control==3)
std::cout << "smart charger detected and enabled" << std::endl;
+ */
+ if(darwin.is_smart_charger_detected())
+ std::cout << "smart charger detected" << std::endl;
+ else
+ std::cout << "smart charger not detected" << std::endl;
+ darwin.smart_charger_enable();
+ if(darwin.is_smart_charger_enabled())
+ std::cout << "smart charger enabled" << std::endl;
+ else
+ std::cout << "smart charger disabled" << std::endl;
//START MOTION MANAGER
darwin.mm_start();
@@ -85,12 +93,14 @@ int main(int argc, char *argv[])
darwin.smart_charger_set_limit_current(limit_current);
//current=darwin.smart_charger_get_limit_current();
- std::cout << "LIMIT CURRENT: " << darwin.smart_charger_get_limit_current() << "A" << std::endl;
+ //std::cout << "LIMIT CURRENT: " << darwin.smart_charger_get_limit_current() << "A" << std::endl;
//READ SMART CHARGER DATA
while(1){
+ std::cout << "LIMIT CURRENT: " << darwin.smart_charger_get_limit_current() << "A" << std::endl;
+
charger_data=darwin.smart_charger_get_data();
if(charger_data.bat_status==0)
std::cout << "battery status: disconnected " << std::endl;