Skip to content
Snippets Groups Projects
Commit e026b57f authored by Irene Garcia Camacho's avatar Irene Garcia Camacho
Browse files

Made some modifications, deleted comments and unused functions, improved coding style

parent ac9b99cc
No related branches found
No related tags found
No related merge requests found
...@@ -8,7 +8,10 @@ ...@@ -8,7 +8,10 @@
#include "xml/darwin_config.hxx" #include "xml/darwin_config.hxx"
#endif #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"), const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
std::string("j_shoulder_pitch_r"), std::string("j_shoulder_pitch_r"),
...@@ -82,8 +85,8 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s ...@@ -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); this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
/* get the current smart charger status (detected or not)*/ /* get the current smart charger status (detected or not)*/
this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status); this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status);
this->MIN_limit_current=0.255; this->min_limit_current=MIN_LIMIT_CURRENT;
this->MAX_limit_current=1.0; this->max_limit_current=MAX_LIMIT_CURRENT;
} }
else else
...@@ -1781,7 +1784,8 @@ void CDarwinRobot::head_get_current_target(double *pan,double *tilt) ...@@ -1781,7 +1784,8 @@ void CDarwinRobot::head_get_current_target(double *pan,double *tilt)
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
} }
//Smart charger interface /* Smart charger interface */
// Enable smart charger module
void CDarwinRobot::smart_charger_enable(void) void CDarwinRobot::smart_charger_enable(void)
{ {
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
...@@ -1793,8 +1797,7 @@ void CDarwinRobot::smart_charger_enable(void) ...@@ -1793,8 +1797,7 @@ void CDarwinRobot::smart_charger_enable(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
} }
/* Disables the smart charger module // Disable smart charger module
*/
void CDarwinRobot::smart_charger_disable(void) void CDarwinRobot::smart_charger_disable(void)
{ {
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
...@@ -1806,6 +1809,7 @@ void CDarwinRobot::smart_charger_disable(void) ...@@ -1806,6 +1809,7 @@ void CDarwinRobot::smart_charger_disable(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
} }
// Returns true if smart charger is detected
bool CDarwinRobot::is_smart_charger_detected(void) bool CDarwinRobot::is_smart_charger_detected(void)
{ {
unsigned char status; unsigned char status;
...@@ -1821,14 +1825,16 @@ bool CDarwinRobot::is_smart_charger_detected(void) ...@@ -1821,14 +1825,16 @@ bool CDarwinRobot::is_smart_charger_detected(void)
else else
throw CDarwinRobotException(_HERE_,"Invalid robot device"); 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; unsigned char status;
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
{ {
this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&status); 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; return true;
else else
return false; return false;
...@@ -1845,11 +1851,10 @@ void CDarwinRobot::smart_charger_set_period(double period) ...@@ -1845,11 +1851,10 @@ void CDarwinRobot::smart_charger_set_period(double period)
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
{ {
period_ms=period*1000; 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); this->robot_device->write_word_register(DARWIN_SMART_CHARGER_PERIOD_L,period_ms);
else else
std::cout <<" Invalid period value" << std::endl; throw CDarwinRobotException(_HERE_,"Invalid period value");
//throw CDarwinRobotException(_HERE_,"Invalid period value");
} }
else else
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
...@@ -1863,37 +1868,20 @@ double CDarwinRobot::smart_charger_get_period(void) ...@@ -1863,37 +1868,20 @@ double CDarwinRobot::smart_charger_get_period(void)
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
{ {
this->robot_device->read_word_register(DARWIN_SMART_CHARGER_PERIOD_L,&period_ms); this->robot_device->read_word_register(DARWIN_SMART_CHARGER_PERIOD_L,&period_ms);
return ((double)period_ms)/1000; return ((double)period_ms)/1000;
} }
else else
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
} }
/* // Obtain time to full, to empty and battery status
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
*/
TChargerData CDarwinRobot::smart_charger_get_data(void) TChargerData CDarwinRobot::smart_charger_get_data(void)
{ {
TChargerData smart_charger_data; TChargerData smart_charger_data;
if(this->robot_device!=NULL) 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; return smart_charger_data;
} }
else else
...@@ -1901,10 +1889,10 @@ TChargerData CDarwinRobot::smart_charger_get_data(void) ...@@ -1901,10 +1889,10 @@ TChargerData CDarwinRobot::smart_charger_get_data(void)
} }
//Set limit input current //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->max_limit_current=max_current;
this->MIN_limit_current=min_current; this->min_limit_current=min_current;
} }
//Get limit current in A //Get limit current in A
...@@ -1915,8 +1903,7 @@ double CDarwinRobot::smart_charger_get_limit_current(void) ...@@ -1915,8 +1903,7 @@ double CDarwinRobot::smart_charger_get_limit_current(void)
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
{ {
this->robot_device->read_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,&limit_current_ma); this->robot_device->read_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,&limit_current_ma);
return ((double)(limit_current_ma*2))/1000;
return ((double)limit_current_ma)/1000;
} }
else else
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
...@@ -1929,52 +1916,13 @@ void CDarwinRobot::smart_charger_set_limit_current(double limit_current) ...@@ -1929,52 +1916,13 @@ void CDarwinRobot::smart_charger_set_limit_current(double limit_current)
if(this->robot_device!=NULL) if(this->robot_device!=NULL)
{ {
//if(limit_current > MIN_limit_current && limit_current < MAX_limit_current){ if(limit_current > min_limit_current && limit_current < max_limit_current)
if(limit_current > 0.255 && limit_current < 1.025){ {
limit_current_ma=(limit_current/2)*1000; limit_current_ma=(limit_current/2)*1000;
this->robot_device->write_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,limit_current_ma); this->robot_device->write_word_register(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,limit_current_ma);
}else{
std::cout <<" Invalid current value" << std::endl;
} }
} else
else throw CDarwinRobotException(_HERE_,"Invalid current value");
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 else
throw CDarwinRobotException(_HERE_,"Invalid robot device"); throw CDarwinRobotException(_HERE_,"Invalid robot device");
......
...@@ -42,13 +42,11 @@ typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_gr ...@@ -42,13 +42,11 @@ typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_gr
//smart charger read data //smart charger read data
#pragma pack (push, 1) #pragma pack (push, 1)
typedef struct{ typedef struct
// uint16_t limit_current; {
// uint8_t rel_soc; unsigned short int avg_time_empty;
// uint8_t abs_soc; unsigned short int avg_time_full;
unsigned short int avg_time_empty; unsigned short int bat_status;
unsigned short int avg_time_full;
unsigned short int bat_status;
}TChargerData; }TChargerData;
#pragma pack (pop) #pragma pack (pop)
...@@ -64,9 +62,10 @@ class CDarwinRobot ...@@ -64,9 +62,10 @@ class CDarwinRobot
unsigned char manager_status; unsigned char manager_status;
/* action status */ /* action status */
unsigned char action_status; unsigned char action_status;
//smart charger status //smart charger variables
unsigned char smart_charger_status; unsigned char smart_charger_status;
double MIN_limit_current, MAX_limit_current; double min_limit_current;
double max_limit_current;
public: public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id); CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface // GPIO interface
...@@ -205,9 +204,9 @@ class CDarwinRobot ...@@ -205,9 +204,9 @@ class CDarwinRobot
*/ */
bool is_smart_charger_detected(void); 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 * \brief Function to enable smart charger module
*/ */
...@@ -227,12 +226,16 @@ class CDarwinRobot ...@@ -227,12 +226,16 @@ class CDarwinRobot
double 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 * \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); 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 * \brief Function to get limit current in A
*/ */
...@@ -242,19 +245,6 @@ class CDarwinRobot ...@@ -242,19 +245,6 @@ class CDarwinRobot
* \param limit_current Value of limit current in A * \param limit_current Value of limit current in A
*/ */
void smart_charger_set_limit_current(double 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(); ~CDarwinRobot();
}; };
......
...@@ -39,15 +39,15 @@ int main(int argc, char *argv[]) ...@@ -39,15 +39,15 @@ int main(int argc, char *argv[])
std::cout << "found darwin controller" << std::endl; std::cout << "found darwin controller" << std::endl;
darwin.gpio_clear_led(LED_TX); darwin.gpio_clear_led(LED_TX);
darwin.gpio_clear_led(LED_4); // darwin.gpio_clear_led(LED_4);
darwin.gpio_clear_led(LED_3); darwin.gpio_clear_led(LED_3);
std::cout << "Holaaaaa! " << std::endl; std::cout << "Holaaaaa! " << std::endl;
//Read/write smart charger period //Read/write smart charger period
std::cout << "Default smart charger period: " << darwin.smart_charger_get_period() << " seg" << 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; std::cout << "Changing smart charger period to 1400ms..." << std::endl;
darwin.smart_charger_set_period(1.5); darwin.smart_charger_set_period(1.4);
std::cout << "New smart charger period: " << darwin.smart_charger_get_period() << " seg" << std::endl; std::cout << "New smart charger period: " << darwin.smart_charger_get_period() << " seg" << std::endl;
//Write invalid period value //Write invalid period value
/* std::cout << "Changing smart charger period to 1000ms" << std::endl; /* std::cout << "Changing smart charger period to 1000ms" << std::endl;
...@@ -56,10 +56,8 @@ int main(int argc, char *argv[]) ...@@ -56,10 +56,8 @@ int main(int argc, char *argv[])
*/ */
//DETECTING AND ENABLING SMART CHARGER //DETECTING AND ENABLING SMART CHARGER
/* if(darwin.smart_charger_is_detected())
std::cout << "smart charger detected" << std::endl; /* control=darwin.is_smart_charger_read_control();
*/
control=darwin.smart_charger_read_control();
if(control==1) if(control==1)
std::cout << "smart charger detected" << std::endl; std::cout << "smart charger detected" << std::endl;
else if(control==0) else if(control==0)
...@@ -70,6 +68,16 @@ int main(int argc, char *argv[]) ...@@ -70,6 +68,16 @@ int main(int argc, char *argv[])
control=darwin.smart_charger_read_control(); control=darwin.smart_charger_read_control();
if(control==3) if(control==3)
std::cout << "smart charger detected and enabled" << std::endl; 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 //START MOTION MANAGER
darwin.mm_start(); darwin.mm_start();
...@@ -85,12 +93,14 @@ int main(int argc, char *argv[]) ...@@ -85,12 +93,14 @@ int main(int argc, char *argv[])
darwin.smart_charger_set_limit_current(limit_current); darwin.smart_charger_set_limit_current(limit_current);
//current=darwin.smart_charger_get_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 //READ SMART CHARGER DATA
while(1){ while(1){
std::cout << "LIMIT CURRENT: " << darwin.smart_charger_get_limit_current() << "A" << std::endl;
charger_data=darwin.smart_charger_get_data(); charger_data=darwin.smart_charger_get_data();
if(charger_data.bat_status==0) if(charger_data.bat_status==0)
std::cout << "battery status: disconnected " << std::endl; std::cout << "battery status: disconnected " << std::endl;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment