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

Added new functions and modifications

Added two functions to check if smart charger if detected and enabled.
Modified functions to access them with doubles (time in seconds and Amperes).
parent 45e24e8f
No related branches found
No related tags found
1 merge request!1Darwin robot driver smart charger
This commit is part of merge request !1. Comments created here will be created in the context of that merge request.
......@@ -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()
{
......
......@@ -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
*
......
......@@ -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;
......
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