Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
D
dynamixel_motor_cont
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
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
labrobotica
drivers
dynamixel_motor_cont
Commits
a0303f56
Commit
a0303f56
authored
1 year ago
by
Sergi Hernandez
Browse files
Options
Downloads
Patches
Plain Diff
Added support for the XM430 servo model.
Checked the validity of the registers variable before trying to access it.
parent
71895cb7
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/dynamixel_motor.cpp
+111
-3
111 additions, 3 deletions
src/dynamixel_motor.cpp
with
111 additions
and
3 deletions
src/dynamixel_motor.cpp
+
111
−
3
View file @
a0303f56
...
...
@@ -197,6 +197,8 @@ void CDynamixelMotor::reset_motor(void)
unsigned
int
current_position
;
unsigned
int
maximum_pwm
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
try
{
this
->
read_register
(
this
->
registers
[
current_pos
],
current_position
);
}
catch
(
CDynamixelAlarmException
&
e
){
...
...
@@ -238,6 +240,7 @@ void CDynamixelMotor::get_model(void)
{
try
{
this
->
dynamixel_dev
->
read_word_register
(
0x0000
,
&
model
);
this
->
registers
=
NULL
;
switch
(
model
)
{
case
0x000C
:
this
->
info
.
model
=
"AX-12A"
;
...
...
@@ -373,7 +376,7 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
ext_memory_map
=
true
;
break
;
case
0x460
:
this
->
info
.
model
=
"XM540-W270"
;
this
->
info
.
max_angle
=
3
5
0.0
;
this
->
info
.
max_angle
=
3
6
0.0
;
this
->
info
.
center_angle
=
180.0
;
this
->
info
.
max_speed
=
1023
*
0.229
;
this
->
info
.
max_current
=
0.0
;
...
...
@@ -384,6 +387,18 @@ void CDynamixelMotor::get_model(void)
this
->
info
.
multi_turn
=
true
;
this
->
info
.
ext_memory_map
=
true
;
break
;
case
0x3FC
:
this
->
info
.
model
=
"XM430-W350"
;
this
->
info
.
max_angle
=
360.0
;
this
->
info
.
center_angle
=
180.0
;
this
->
info
.
max_speed
=
2760
;
this
->
info
.
max_current
=
0.0
;
this
->
info
.
encoder_resolution
=
4095
;
this
->
info
.
gear_ratio
=
353.5
;
this
->
registers
=
xm_reg
;
this
->
info
.
pid_control
=
true
;
this
->
info
.
multi_turn
=
true
;
this
->
info
.
ext_memory_map
=
true
;
break
;
default:
this
->
info
.
model
=
"unknown"
;
break
;
}
...
...
@@ -393,8 +408,11 @@ void CDynamixelMotor::get_model(void)
this
->
reset_motor
();
throw
;
}
this
->
read_register
(
this
->
registers
[
firmware_version
],
value
);
this
->
info
.
firmware_ver
=
value
;
if
(
this
->
registers
!=
NULL
)
{
this
->
read_register
(
this
->
registers
[
firmware_version
],
value
);
this
->
info
.
firmware_ver
=
value
;
}
}
}
...
...
@@ -405,6 +423,8 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
if
(
this
->
mode
!=
mode
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
this
->
enabled
)
this
->
disable
();
if
(
this
->
info
.
ext_memory_map
)
...
...
@@ -459,6 +479,8 @@ void CDynamixelMotor::set_drive_mode(bool time_based,bool reverse)
}
else
this
->
reverse_mode
=
false
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
write_register
(
this
->
registers
[
drive_mode
],
value
);
}
...
...
@@ -704,6 +726,8 @@ void CDynamixelMotor::set_position_range(double min, double max)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
config
.
max_angle
=
max
;
this
->
config
.
min_angle
=
min
;
this
->
write_register
(
this
->
registers
[
max_angle_limit
],
max_pos
);
...
...
@@ -719,6 +743,8 @@ void CDynamixelMotor::get_position_range(double *min, double *max)
{
unsigned
int
angle_limit
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
max_angle_limit
],
angle_limit
);
(
*
max
)
=
this
->
to_angles
(
angle_limit
);
this
->
read_register
(
this
->
registers
[
min_angle_limit
],
angle_limit
);
...
...
@@ -729,6 +755,8 @@ int CDynamixelMotor::get_temperature_limit(void)
{
unsigned
int
limit
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
temp_limit
],
limit
);
return
limit
;
...
...
@@ -745,6 +773,8 @@ void CDynamixelMotor::set_temperature_limit(int limit)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
config
.
max_temperature
=
limit
;
value
=
limit
;
this
->
write_register
(
this
->
registers
[
temp_limit
],
value
);
...
...
@@ -755,6 +785,8 @@ void CDynamixelMotor::get_voltage_limits(double *min, double *max)
{
unsigned
int
value
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
min_voltage_limit
],
value
);
*
min
=
((
double
)
value
/
10.0
);
this
->
read_register
(
this
->
registers
[
max_voltage_limit
],
value
);
...
...
@@ -779,6 +811,8 @@ void CDynamixelMotor::set_voltage_limits(double min, double max)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
config
.
min_voltage
=
min
;
this
->
config
.
max_voltage
=
max
;
value
=
min
*
10
;
...
...
@@ -793,6 +827,8 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
{
unsigned
int
led_alarms
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
alarm_led
],
led_alarms
);
return
led_alarms
;
...
...
@@ -809,6 +845,8 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
value
=
alarms
;
this
->
write_register
(
this
->
registers
[
alarm_led
],
value
);
}
...
...
@@ -818,6 +856,8 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
{
unsigned
int
shutdown_alarms
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
alarm_shtdwn
],
shutdown_alarms
);
return
shutdown_alarms
;
...
...
@@ -834,6 +874,8 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
alarms
=
alarms
;
value
=
alarms
;
this
->
write_register
(
this
->
registers
[
alarm_shtdwn
],
value
);
...
...
@@ -845,6 +887,8 @@ double CDynamixelMotor::get_max_pwm(void)
unsigned
int
load
;
double
torque
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
max_pwm
],
load
);
torque
=
(
load
&
0x3FF
)
*
100.0
/
1023.0
;
if
(
load
>
0x3FF
)
...
...
@@ -858,6 +902,8 @@ double CDynamixelMotor::get_pwm_limit(void)
unsigned
int
load
;
double
torque
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
pwm_limit
],
load
);
if
(
this
->
info
.
ext_memory_map
)
torque
=
((
signed
short
int
)
load
)
*
100.0
/
885.0
;
...
...
@@ -881,6 +927,8 @@ void CDynamixelMotor::set_max_pwm(double torque_ratio)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
load
=
torque_ratio
*
1023.0
/
100.0
;
this
->
config
.
max_pwm
=
torque_ratio
;
this
->
write_register
(
this
->
registers
[
max_pwm
],
load
);
...
...
@@ -897,6 +945,8 @@ void CDynamixelMotor::set_pwm_limit(double torque_ratio)
}
else
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
this
->
info
.
ext_memory_map
)
load
=
torque_ratio
*
885.0
/
100.0
;
else
...
...
@@ -910,6 +960,8 @@ double CDynamixelMotor::get_current_limit(void)
double
current
;
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_limit
],
data
);
current
=
((
signed
short
int
)
data
)
*
2.69
/
1000.0
;
...
...
@@ -920,6 +972,8 @@ void CDynamixelMotor::set_current_limit(double current)
{
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
data
=
(
signed
short
int
)(
current
*
1000.0
/
2.69
);
this
->
write_register
(
this
->
registers
[
current_limit
],
data
);
}
...
...
@@ -929,6 +983,8 @@ double CDynamixelMotor::get_speed_limit(void)
unsigned
int
data
;
double
speed
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
velocity_limit
],
data
);
speed
=
this
->
to_speeds
(
data
);
...
...
@@ -939,6 +995,8 @@ void CDynamixelMotor::set_speed_limit(double speed)
{
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
data
=
this
->
from_speeds
(
speed
);
this
->
write_register
(
this
->
registers
[
velocity_limit
],
data
);
}
...
...
@@ -950,6 +1008,8 @@ void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
if
(
!
this
->
info
.
pid_control
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
cw_comp_margin
],
value
);
config
.
cw_compliance_margin
=
value
;
this
->
read_register
(
this
->
registers
[
ccw_comp_margin
],
value
);
...
...
@@ -977,6 +1037,8 @@ void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config)
/* handle exceptions */
throw
CDynamixelMotorException
(
_HERE_
,
"Invalid counterclockwise compliance margin."
);
}
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
config
.
cw_compliance_slope
>=
1
&&
config
.
cw_compliance_slope
<=
5
)
config
.
cw_compliance_slope
=
4
;
else
if
(
config
.
cw_compliance_slope
>=
6
&&
config
.
cw_compliance_slope
<=
11
)
config
.
cw_compliance_slope
=
8
;
else
if
(
config
.
cw_compliance_slope
>=
12
&&
config
.
cw_compliance_slope
<=
23
)
config
.
cw_compliance_slope
=
16
;
...
...
@@ -1022,6 +1084,8 @@ void CDynamixelMotor::get_pid_control(TDynamixel_pid &config)
if
(
this
->
info
.
pid_control
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
pos_pid_p
],
value
);
config
.
p
=
value
;
this
->
read_register
(
this
->
registers
[
pos_pid_i
],
value
);
...
...
@@ -1037,6 +1101,8 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
if
(
this
->
info
.
pid_control
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
pid
.
p
=
config
.
p
;
this
->
pid
.
i
=
config
.
i
;
this
->
pid
.
d
=
config
.
d
;
...
...
@@ -1071,11 +1137,15 @@ void CDynamixelMotor::set_feedfwd_control(double acc_gain,double vel_gain)
void
CDynamixelMotor
::
turn_led_on
(
void
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
write_register
(
this
->
registers
[
led
],
1
);
}
void
CDynamixelMotor
::
turn_led_off
(
void
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
write_register
(
this
->
registers
[
led
],
0
);
}
...
...
@@ -1091,12 +1161,16 @@ void CDynamixelMotor::lock(void)
void
CDynamixelMotor
::
enable
(
void
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
write_register
(
this
->
registers
[
torque_en
],
1
);
this
->
enabled
=
true
;
}
void
CDynamixelMotor
::
disable
(
void
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
write_register
(
this
->
registers
[
torque_en
],
0
);
this
->
enabled
=
false
;
}
...
...
@@ -1105,6 +1179,8 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed,double curre
{
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
current
==
std
::
numeric_limits
<
double
>::
infinity
())
{
this
->
set_control_mode
(
position_ctrl
);
...
...
@@ -1146,6 +1222,8 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed,double curre
unsigned
int
data
,
pos
;
double
abs_angle
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
current
==
std
::
numeric_limits
<
double
>::
infinity
())
{
this
->
set_control_mode
(
position_ctrl
);
...
...
@@ -1190,6 +1268,8 @@ void CDynamixelMotor::move_speed(double speed)
{
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
set_control_mode
(
velocity_ctrl
);
if
(
speed
>
this
->
info
.
max_speed
)
speed
=
this
->
info
.
max_speed
;
...
...
@@ -1203,6 +1283,8 @@ void CDynamixelMotor::move_pwm(double pwm_ratio)
{
unsigned
int
torque
=
0
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
set_control_mode
(
pwm_ctrl
);
if
(
pwm_ratio
>
100.0
)
pwm_ratio
=
100.0
;
...
...
@@ -1226,6 +1308,8 @@ void CDynamixelMotor::move_current(double current)
{
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
set_control_mode
(
current_ctrl
);
if
(
current
>
this
->
info
.
max_current
)
current
=
this
->
info
.
max_current
;
...
...
@@ -1241,6 +1325,8 @@ void CDynamixelMotor::stop(void)
if
(
this
->
mode
==
position_ctrl
||
this
->
mode
==
ext_position_ctrl
||
this
->
mode
==
current_pos_ctrl
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_pos
],
current_position
);
if
(
this
->
to_angles
(
current_position
)
>
this
->
config
.
max_angle
)
current_position
=
from_angles
(
this
->
config
.
max_angle
);
...
...
@@ -1261,6 +1347,8 @@ double CDynamixelMotor::get_current_angle(void)
unsigned
int
data
;
double
current_position
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_pos
],
data
);
current_position
=
this
->
to_angles
(
data
);
...
...
@@ -1272,6 +1360,8 @@ double CDynamixelMotor::get_current_speed(void)
unsigned
int
data
;
double
c_speed
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_speed
],
data
);
c_speed
=
this
->
to_speeds
(
data
&
0x03FF
);
if
(
data
&
0x0400
)
...
...
@@ -1285,6 +1375,8 @@ double CDynamixelMotor::get_current_temperature(void)
unsigned
int
data
;
double
c_temp
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_temp
],
data
);
c_temp
=
(
double
)
data
;
...
...
@@ -1296,6 +1388,8 @@ double CDynamixelMotor::get_current_voltage(void)
unsigned
int
data
;
double
c_voltage
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_voltage
],
data
);
c_voltage
=
(
double
)
data
/
10.0
;
...
...
@@ -1307,6 +1401,8 @@ double CDynamixelMotor::get_current_pwm(void)
unsigned
int
data
;
double
c_effort
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
this
->
info
.
ext_memory_map
)
{
this
->
read_register
(
this
->
registers
[
current_pwm
],
data
);
...
...
@@ -1328,6 +1424,8 @@ double CDynamixelMotor::get_current_current(void)
double
current
;
unsigned
int
data
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
current_load
],
data
);
current
=
((
signed
short
int
)
data
)
*
2.69
/
1000.0
;
...
...
@@ -1338,6 +1436,8 @@ unsigned int CDynamixelMotor::get_punch(void)
{
unsigned
int
value
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
read_register
(
this
->
registers
[
punch
],
value
);
return
value
;
...
...
@@ -1347,6 +1447,8 @@ void CDynamixelMotor::set_punch(unsigned int punch_value)
{
if
(
punch_value
<
0x0020
||
punch_value
>
0x03FF
)
throw
CDynamixelMotorException
(
_HERE_
,
"Invalid Punch value"
);
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
this
->
config
.
punch
=
punch_value
;
this
->
write_register
(
this
->
registers
[
punch
],
punch_value
);
}
...
...
@@ -1355,6 +1457,8 @@ control_mode CDynamixelMotor::get_control_mode(void)
{
unsigned
int
value
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
if
(
this
->
info
.
ext_memory_map
)
{
this
->
read_register
(
this
->
registers
[
op_mode
],
value
);
...
...
@@ -1384,6 +1488,8 @@ void CDynamixelMotor::get_drive_mode(bool &time_based,bool &reverse)
{
unsigned
int
value
;
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
try
{
this
->
read_register
(
this
->
registers
[
drive_mode
],
value
);
if
(
value
&
0x04
)
...
...
@@ -1416,6 +1522,8 @@ void CDynamixelMotor::get_drive_mode(bool &time_based,bool &reverse)
unsigned
short
int
CDynamixelMotor
::
get_register_address
(
reg_id
id
)
{
if
(
this
->
registers
==
NULL
)
throw
CDynamixelMotorException
(
_HERE_
,
"Unknown dynamixel model."
);
return
this
->
registers
[
id
].
address
;
}
...
...
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