Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
labrobotica
drivers
dynamixel_motor_cont
Commits
dec7c6aa
Commit
dec7c6aa
authored
Sep 02, 2019
by
Sergi Hernandez
Browse files
Implemented the current control and the position and current control modes.
parent
8e7efa29
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/dynamixel_motor.cpp
View file @
dec7c6aa
...
...
@@ -25,6 +25,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this
->
info
.
max_angle
=-
1
;
this
->
info
.
center_angle
=-
1
;
this
->
info
.
max_speed
=-
1
;
this
->
info
.
max_current
=-
1
;
this
->
info
.
baudrate
=
(
unsigned
int
)
-
1
;
this
->
info
.
id
=
(
unsigned
char
)
-
1
;
this
->
info
.
ext_memory_map
=
false
;
...
...
@@ -55,6 +56,11 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this
->
config
.
max_temperature
=
this
->
get_temperature_limit
();
this
->
get_voltage_limits
(
&
this
->
config
.
min_voltage
,
&
this
->
config
.
max_voltage
);
this
->
config
.
max_pwm
=
this
->
get_pwm_limit
();
try
{
this
->
config
.
max_current
=
this
->
get_current_limit
();
}
catch
(
CDynamixelMotorUnsupportedFeatureException
&
e
){
std
::
cout
<<
"No current limit feature"
<<
std
::
endl
;
}
this
->
get_compliance_control
(
this
->
compliance
);
this
->
get_pid_control
(
this
->
pid
);
this
->
alarms
=
this
->
get_turn_off_alarms
();
...
...
@@ -157,6 +163,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
300.0
;
this
->
info
.
center_angle
=
150.0
;
this
->
info
.
max_speed
=
354
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
1023
;
this
->
info
.
gear_ratio
=
254
;
this
->
registers
=
ax_reg
;
...
...
@@ -168,6 +175,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
300.0
;
this
->
info
.
center_angle
=
150.0
;
this
->
info
.
max_speed
=
2830
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
1023
;
this
->
info
.
gear_ratio
=
32
;
this
->
registers
=
ax_reg
;
...
...
@@ -179,6 +187,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
300.0
;
this
->
info
.
center_angle
=
150.0
;
this
->
info
.
max_speed
=
582
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
1023
;
this
->
info
.
gear_ratio
=
254
;
this
->
registers
=
ax_reg
;
...
...
@@ -190,6 +199,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
360.0
;
this
->
info
.
center_angle
=
180.0
;
this
->
info
.
max_speed
=
2820
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
4095
;
this
->
info
.
gear_ratio
=
32
;
this
->
registers
=
mx_1_0_reg
;
...
...
@@ -201,6 +211,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
360.0
;
this
->
info
.
center_angle
=
180.0
;
this
->
info
.
max_speed
=
330
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
4095
;
this
->
info
.
gear_ratio
=
193
;
this
->
registers
=
mx_1_0_reg
;
...
...
@@ -212,6 +223,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
360.0
;
this
->
info
.
center_angle
=
180.0
;
this
->
info
.
max_speed
=
378
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
4095
;
this
->
info
.
gear_ratio
=
200
;
this
->
registers
=
mx_1_0_reg
;
...
...
@@ -223,6 +235,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
360.0
;
this
->
info
.
center_angle
=
180.0
;
this
->
info
.
max_speed
=
270
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
4095
;
this
->
info
.
gear_ratio
=
225
;
this
->
registers
=
mx_106_1_0_reg
;
...
...
@@ -234,6 +247,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
300.0
;
this
->
info
.
center_angle
=
150
;
this
->
info
.
max_speed
=
684
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
1023
;
this
->
info
.
gear_ratio
=
238
;
this
->
registers
=
xl_reg
;
...
...
@@ -245,6 +259,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
max_angle
=
360.0
;
this
->
info
.
center_angle
=
180
;
this
->
info
.
max_speed
=
4620
;
this
->
info
.
max_current
=
2.3
;
this
->
info
.
encoder_resolution
=
4095
;
this
->
info
.
gear_ratio
=
213
;
this
->
registers
=
xm_reg
;
...
...
@@ -753,12 +768,21 @@ void CDynamixelMotor::set_pwm_limit(double torque_ratio)
double
CDynamixelMotor
::
get_current_limit
(
void
)
{
double
current
;
unsigned
int
data
;
this
->
read_register
(
this
->
registers
[
current_limit
],
data
);
current
=
((
signed
short
int
)
data
)
*
2.69
/
1000.0
;
return
current
;
}
void
CDynamixelMotor
::
set_current_limit
(
double
current
)
{
unsigned
int
data
;
data
=
(
signed
short
int
)(
current
*
1000.0
/
2.69
);
this
->
write_register
(
this
->
registers
[
current_limit
],
data
);
}
double
CDynamixelMotor
::
get_speed_limit
(
void
)
...
...
@@ -931,19 +955,42 @@ void CDynamixelMotor::disable(void)
void
CDynamixelMotor
::
move_absolute_angle
(
double
angle
,
double
speed
,
double
current
)
{
unsigned
int
cmd
[
2
]
;
unsigned
int
data
;
this
->
set_control_mode
(
position_ctrl
);
if
(
angle
>
this
->
info
.
center_angle
)
angle
=
this
->
info
.
center_angle
;
else
if
(
angle
<-
this
->
info
.
center_angle
)
angle
=-
this
->
info
.
center_angle
;
cmd
[
0
]
=
this
->
from_angles
(
angle
);
this
->
write_register
(
this
->
registers
[
goal_pos
],
cmd
[
0
]);
if
(
speed
>
this
->
info
.
max_speed
)
speed
=
this
->
info
.
max_speed
;
cmd
[
1
]
=
this
->
from_speeds
(
speed
);
this
->
write_register
(
this
->
registers
[
goal_speed
],
cmd
[
1
]);
if
(
current
==
std
::
numeric_limits
<
double
>::
infinity
())
{
this
->
set_control_mode
(
position_ctrl
);
if
(
angle
>
this
->
info
.
center_angle
)
angle
=
this
->
info
.
center_angle
;
else
if
(
angle
<-
this
->
info
.
center_angle
)
angle
=-
this
->
info
.
center_angle
;
data
=
this
->
from_angles
(
angle
);
this
->
write_register
(
this
->
registers
[
goal_pos
],
data
);
if
(
speed
>
this
->
info
.
max_speed
)
speed
=
this
->
info
.
max_speed
;
data
=
this
->
from_speeds
(
speed
);
this
->
write_register
(
this
->
registers
[
goal_speed
],
data
);
}
else
{
this
->
set_control_mode
(
current_pos_ctrl
);
if
(
current
>
this
->
info
.
max_current
)
current
=
this
->
info
.
max_current
;
else
if
(
current
<-
this
->
info
.
max_current
)
current
=-
this
->
info
.
max_current
;
data
=
(
signed
short
int
)(
current
*
1000.0
/
2.69
);
this
->
write_register
(
this
->
registers
[
goal_current
],
data
);
if
(
angle
>
this
->
info
.
center_angle
)
angle
=
this
->
info
.
center_angle
;
else
if
(
angle
<-
this
->
info
.
center_angle
)
angle
=-
this
->
info
.
center_angle
;
data
=
this
->
from_angles
(
angle
);
this
->
write_register
(
this
->
registers
[
goal_pos
],
data
);
if
(
speed
>
this
->
info
.
max_speed
)
speed
=
this
->
info
.
max_speed
;
data
=
this
->
from_speeds
(
speed
);
this
->
write_register
(
this
->
registers
[
goal_speed
],
data
);
}
}
void
CDynamixelMotor
::
move_relative_angle
(
double
angle
,
double
speed
,
double
current
)
...
...
@@ -996,7 +1043,11 @@ void CDynamixelMotor::move_pwm(double pwm_ratio)
void
CDynamixelMotor
::
move_current
(
double
current
)
{
unsigned
int
data
;
this
->
set_control_mode
(
current_ctrl
);
data
=
(
signed
short
int
)(
current
*
1000.0
/
2.69
);
this
->
write_register
(
this
->
registers
[
goal_current
],
data
);
}
void
CDynamixelMotor
::
stop
(
void
)
...
...
@@ -1085,7 +1136,13 @@ double CDynamixelMotor::get_current_pwm(void)
double
CDynamixelMotor
::
get_current_current
(
void
)
{
double
current
;
unsigned
int
data
;
this
->
read_register
(
this
->
registers
[
current_load
],
data
);
current
=
((
signed
short
int
)
data
)
*
2.69
/
1000.0
;
return
current
;
}
unsigned
int
CDynamixelMotor
::
get_punch
(
void
)
...
...
@@ -1109,18 +1166,26 @@ control_mode CDynamixelMotor::get_control_mode(void)
{
unsigned
int
value
;
if
(
this
->
info
.
model
==
"XL-320"
)
if
(
this
->
info
.
ext_memory_map
)
{
this
->
read_register
(
this
->
registers
[
op_mode
],
value
);
this
->
mode
=
(
control_mode
)
value
;
}
else
{
this
->
get_position_range
(
&
this
->
config
.
min_angle
,
&
this
->
config
.
max_angle
);
if
(
this
->
config
.
min_angle
==-
this
->
info
.
center_angle
&&
this
->
config
.
max_angle
==-
this
->
info
.
center_angle
)
this
->
mode
=
pwm_ctrl
;
if
(
this
->
info
.
model
==
"XL-320"
)
{
this
->
read_register
(
this
->
registers
[
op_mode
],
value
);
this
->
mode
=
(
control_mode
)
value
;
}
else
this
->
mode
=
position_ctrl
;
{
this
->
get_position_range
(
&
this
->
config
.
min_angle
,
&
this
->
config
.
max_angle
);
if
(
this
->
config
.
min_angle
==-
this
->
info
.
center_angle
&&
this
->
config
.
max_angle
==-
this
->
info
.
center_angle
)
this
->
mode
=
pwm_ctrl
;
else
this
->
mode
=
position_ctrl
;
}
}
return
this
->
mode
;
...
...
src/dynamixel_motor.h
View file @
dec7c6aa
...
...
@@ -12,6 +12,7 @@
#include
<string>
#include
<vector>
#include
<memory>
#include
<limits>
typedef
enum
{
...
...
@@ -35,6 +36,7 @@ typedef struct
double
max_angle
;
double
center_angle
;
double
max_speed
;
double
max_current
;
unsigned
int
baudrate
;
unsigned
char
id
;
bool
ext_memory_map
;
...
...
@@ -63,6 +65,7 @@ typedef struct
double
max_voltage
;
double
min_voltage
;
double
max_pwm
;
double
max_current
;
unsigned
short
int
punch
;
}
TDynamixel_config
;
...
...
@@ -366,12 +369,12 @@ class CDynamixelMotor
* \brief
*
*/
void
move_absolute_angle
(
double
angle
,
double
speed
,
double
current
=
-
1.0
);
void
move_absolute_angle
(
double
angle
,
double
speed
,
double
current
=
std
::
numeric_limits
<
double
>::
infinity
()
);
/**
* \brief
*
*/
void
move_relative_angle
(
double
angle
,
double
speed
,
double
current
=
-
1.0
);
void
move_relative_angle
(
double
angle
,
double
speed
,
double
current
=
std
::
numeric_limits
<
double
>::
infinity
()
);
/**
* \brief
*
...
...
src/examples/test_dynamixel_motor.cpp
View file @
dec7c6aa
...
...
@@ -131,25 +131,30 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
////MOVE ABSOLUTE ANGLE
/////////////////////////////////////////////////////////////////////////
/*
double
time_interval
=
0.1
;
//time in secs between checks
int
max_time_sec
=
10.0
;
//max time to wait until timeout
int
t
;
double
uperiod
=
time_interval
*
1000000.0
;
double
timeout
=
max_time_sec
/
(
uperiod
/
1000000.0
);
double
desired_angle
,
absolute_angle
=
0.0
,
desired_speed
=
1.0
,
current_angle
,
max_angle_error
=
0.5
,
desired_current
=
0.1
;
std
::
cout
<<
"-----------------------------------------------------------"
<<
std
::
endl
;
double absolute_angle=0.0;
double current_abs_angle;
std::cout << "MOVE ABSOLUTE ANGLE: " << absolute_angle << std::endl;
cont
->
enable
();
current_abs_angle = cont->get_current_angle();
desired_angle
=
absolute_angle
;
std
::
cout
<<
"Desired angle: "
<<
desired_angle
<<
std
::
endl
;
std::cout << "Current angle: " << current_abs_angle << std::endl;
current_angle
=
cont
->
get_current_angle
();
std
::
cout
<<
"Current angle: "
<<
current_angle
<<
std
::
endl
;
cont->move_absolute_angle(absolute_angle, desired_speed);
cont
->
move_absolute_angle
(
absolute_angle
,
desired_speed
,
desired_current
);
std
::
cout
<<
"Moving..."
<<
std
::
endl
;
t
=
0
;
while(fabs(current_
abs_
angle)>max_angle_error && t<timeout)
while
(
fabs
(
current_angle
)
>
max_angle_error
&&
t
<
timeout
)
{
current_abs_angle = cont->get_current_angle();
current_angle
=
cont
->
get_current_angle
();
std
::
cout
<<
current_angle
<<
std
::
endl
;
usleep
(
uperiod
);
t
++
;
}
...
...
@@ -158,16 +163,16 @@ int main(int argc, char *argv[])
std
::
cout
<<
"Reached "
<<
max_time_sec
<<
"sec timeout"
<<
std
::
endl
;
std
::
cout
<<
"Desired angle: "
<<
desired_angle
<<
std
::
endl
;
std::cout << "Reached angle: " << current_
abs_
angle << std::endl;
std::cout << "Error angle: " << current_
abs_
angle-desired_angle << std::endl;
std
::
cout
<<
"Reached angle: "
<<
current_angle
<<
std
::
endl
;
std
::
cout
<<
"Error angle: "
<<
current_angle
-
desired_angle
<<
std
::
endl
;
std
::
cout
<<
"Done"
<<
std
::
endl
;
sleep
(
1
);
cont
->
disable
();
*/
/////////////////////////////////////////////////////////////////////////
////MOVE PWM
/////////////////////////////////////////////////////////////////////////
/*
double time_interval = 0.1; //time in secs between checks
int max_time_sec = 10.0; //max time to wait until timeout
int t;
...
...
@@ -203,7 +208,7 @@ int main(int argc, char *argv[])
std::cout << "----------------------------" << std::endl;
desired_pwm=-1.0*max_pwm;
std
::
cout
<<
"MOVE
EFFORT
: "
<<
desired_pwm
<<
std
::
endl
;
std::cout << "MOVE
PWM
: " << desired_pwm << std::endl;
cont->move_pwm(desired_pwm);
std::cout << "Moving..." << std::endl;
...
...
@@ -221,8 +226,66 @@ int main(int argc, char *argv[])
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
cont->disable();
*/
/////////////////////////////////////////////////////////////////////////
////MOVE Current
/////////////////////////////////////////////////////////////////////////
/*
double time_interval = 0.1; //time in secs between checks
int max_time_sec = 10.0; //max time to wait until timeout
int t;
double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0);
double max_current,desired_current;
cont->enable();
std::cout << "-----------------------------------------------------------" << std::endl;
std::cout << "Current limit " << cont->get_current_limit() << std::endl;
if(argc==2)
max_current = atof(argv[1]);
else
max_current = 0.5;
desired_current=max_current;
std::cout << "MOVE current: " << desired_current << std::endl;
cont->move_current(desired_current);
std::cout << "Moving..." << std::endl;
t=0;
while(t<timeout)
{
std::cout << "Current current: " << cont->get_current_current() << std::endl;
usleep(uperiod);
t++;
}
cont->move_current(0);
std::cout << "Done" << std::endl;
sleep(1);
std::cout << "----------------------------" << std::endl;
desired_current=-1.0*max_current;
std::cout << "MOVE current: " << desired_current << std::endl;
cont->move_current(desired_current);
std::cout << "Moving..." << std::endl;
t=0;
while(t<timeout)
{
std::cout << "Current current: " << cont->get_current_current() << std::endl;
usleep(uperiod);
t++;
}
cont->move_current(0);
std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
cont->disable();
*/
/////////////////////////////////////////////////////////////////////////
////MOVE RANDOM TORQUES AND OUTPUT SPEEDS
/////////////////////////////////////////////////////////////////////////
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment