Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
D
darwin_robot_driver
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
humanoides
darwin
darwin_robot_driver
Commits
e026b57f
Commit
e026b57f
authored
8 years ago
by
Irene Garcia Camacho
Browse files
Options
Downloads
Patches
Plain Diff
Made some modifications, deleted comments and unused functions, improved coding style
parent
ac9b99cc
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
src/darwin_robot.cpp
+26
-78
26 additions, 78 deletions
src/darwin_robot.cpp
src/darwin_robot.h
+15
-25
15 additions, 25 deletions
src/darwin_robot.h
src/examples/darwin_smart_charger_test.cpp
+18
-8
18 additions, 8 deletions
src/examples/darwin_smart_charger_test.cpp
with
59 additions
and
111 deletions
src/darwin_robot.cpp
+
26
−
78
View file @
e026b57f
...
@@ -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
>=
1
4
00
&&
period_ms
<=
16
00
)
if
(
period_ms
>=
1
0
00
&&
period_ms
<=
50
00
)
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"
);
...
...
This diff is collapsed.
Click to expand it.
src/darwin_robot.h
+
15
−
25
View file @
e026b57f
...
@@ -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
statu
s
//smart charger
variable
s
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
();
};
};
...
...
This diff is collapsed.
Click to expand it.
src/examples/darwin_smart_charger_test.cpp
+
18
−
8
View file @
e026b57f
...
@@ -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 1
5
00ms..."
<<
std
::
endl
;
std
::
cout
<<
"Changing smart charger period to 1
4
00ms..."
<<
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
;
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment