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
humanoides
tools
stm32_libraries
Commits
fb208d0c
Commit
fb208d0c
authored
Apr 18, 2020
by
Sergi Hernandez
Browse files
Changed the long int variables to long long int to avoid undesired overflow.
parent
1a306833
Changes
2
Hide whitespace changes
Inline
Side-by-side
dynamixel_manager/include/modules/joint_motion.h
View file @
fb208d0c
...
...
@@ -18,15 +18,15 @@ typedef struct{
TMemory
*
memory
;
TMemModule
mem_module
;
unsigned
short
int
ram_base_address
;
long
int
target_angles
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
long
int
target_speeds
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
long
int
target_accels
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
long
int
target_stop_angles
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
long
int
current_angles
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
long
int
current_speeds
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
char
dir
[
DYN_MANAGER_MAX_NUM_DEVICES
];
// joint_direction
long
int
motion_period
;
short
int
motion_period_ms
;
signed
long
long
int
target_angles
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
signed
long
long
int
target_speeds
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
signed
long
long
int
target_accels
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
signed
long
long
int
target_stop_angles
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
signed
long
long
int
current_angles
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
signed
long
long
int
current_speeds
[
DYN_MANAGER_MAX_NUM_DEVICES
];
//48|16
signed
char
dir
[
DYN_MANAGER_MAX_NUM_DEVICES
];
// joint_direction
unsigned
long
long
int
motion_period
;
unsigned
short
int
motion_period_ms
;
unsigned
char
stop
[
NUM_JOINT_GROUPS
];
unsigned
char
moving
[
NUM_JOINT_GROUPS
];
unsigned
int
group_servos
[
NUM_JOINT_GROUPS
];
...
...
dynamixel_manager/src/modules/joint_motion.c
View file @
fb208d0c
...
...
@@ -88,48 +88,48 @@ void joint_motion_pre_process(void *module)
if
(
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
])
>=
65
)
{
moving
=
0x01
;
if
(
joint
->
current_speeds
[
i
]
!=
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
])
// it is necessary to change the current speed
if
(
joint
->
current_speeds
[
i
]
!=
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
])
// it is necessary to change the current speed
{
joint
->
current_angles
[
i
]
+=
((
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
>>
17
);
if
(
joint
->
current_speeds
[
i
]
>
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
])
joint
->
current_angles
[
i
]
+=
((
(
signed
long
long
int
)(
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
)
>>
17
);
if
(
joint
->
current_speeds
[
i
]
>
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
])
{
joint
->
current_speeds
[
i
]
-=
((
joint
->
target_accels
[
i
]
*
joint
->
motion_period
)
>>
16
);
// reduce speed
if
(
joint
->
current_speeds
[
i
]
<
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
])
joint
->
current_speeds
[
i
]
=
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
];
joint
->
current_speeds
[
i
]
-=
((
(
signed
long
long
int
)(
joint
->
target_accels
[
i
]
*
joint
->
motion_period
)
)
>>
16
);
// reduce speed
if
(
joint
->
current_speeds
[
i
]
<
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
])
joint
->
current_speeds
[
i
]
=
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
];
}
else
{
joint
->
current_speeds
[
i
]
+=
((
joint
->
target_accels
[
i
]
*
joint
->
motion_period
)
>>
16
);
// increase speed
if
(
joint
->
current_speeds
[
i
]
>
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
])
joint
->
current_speeds
[
i
]
=
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
];
joint
->
current_speeds
[
i
]
+=
((
(
signed
long
long
int
)(
joint
->
target_accels
[
i
]
*
joint
->
motion_period
)
)
>>
16
);
// increase speed
if
(
joint
->
current_speeds
[
i
]
>
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
])
joint
->
current_speeds
[
i
]
=
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
];
}
joint
->
current_angles
[
i
]
+=
((
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
>>
17
);
if
(
joint
->
dir
[
i
]
==
1
&&
joint
->
current_angles
[
i
]
>
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
+=
((
(
signed
long
long
int
)(
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
)
>>
17
);
if
(
((
signed
char
)
joint
->
dir
[
i
]
)
==
1
&&
joint
->
current_angles
[
i
]
>
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
=
joint
->
target_angles
[
i
];
if
(
joint
->
dir
[
i
]
==-
1
&&
joint
->
current_angles
[
i
]
<
joint
->
target_angles
[
i
])
if
(
((
signed
char
)
joint
->
dir
[
i
]
)
==-
1
&&
joint
->
current_angles
[
i
]
<
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
=
joint
->
target_angles
[
i
];
}
else
{
if
(
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
])
>
joint
->
target_stop_angles
[
i
])
// constant speed phase
{
joint
->
current_angles
[
i
]
+=
((
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
>>
16
);
if
(
joint
->
dir
[
i
]
==
1
&&
joint
->
current_angles
[
i
]
>
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
+=
((
(
signed
long
long
int
)(
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
)
>>
16
);
if
(
((
signed
char
)
joint
->
dir
[
i
]
)
==
1
&&
joint
->
current_angles
[
i
]
>
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
=
joint
->
target_angles
[
i
];
if
(
joint
->
dir
[
i
]
==-
1
&&
joint
->
current_angles
[
i
]
<
joint
->
target_angles
[
i
])
if
(
((
signed
char
)
joint
->
dir
[
i
]
)
==-
1
&&
joint
->
current_angles
[
i
]
<
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
=
joint
->
target_angles
[
i
];
}
else
// deceleration phase
{
joint
->
current_angles
[
i
]
=
joint
->
current_angles
[
i
]
+
((
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
>>
17
);
joint
->
target_speeds
[
i
]
=
joint
->
target_speeds
[
i
]
-
((
joint
->
target_accels
[
i
]
*
joint
->
motion_period
)
>>
16
);
joint
->
current_angles
[
i
]
=
joint
->
current_angles
[
i
]
+
((
(
signed
long
long
int
)(
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
)
>>
17
);
joint
->
target_speeds
[
i
]
=
joint
->
target_speeds
[
i
]
-
((
(
signed
long
long
int
)(
joint
->
target_accels
[
i
]
*
joint
->
motion_period
)
)
>>
16
);
if
(
joint
->
target_speeds
[
i
]
<
0
)
joint
->
target_speeds
[
i
]
=
0
;
joint
->
current_speeds
[
i
]
=
joint
->
dir
[
i
]
*
joint
->
target_speeds
[
i
];
joint
->
current_angles
[
i
]
=
joint
->
current_angles
[
i
]
+
((
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
>>
17
);
if
(
joint
->
dir
[
i
]
==
1
&&
joint
->
current_angles
[
i
]
>
joint
->
target_angles
[
i
])
joint
->
current_speeds
[
i
]
=
((
signed
char
)
joint
->
dir
[
i
]
)
*
joint
->
target_speeds
[
i
];
joint
->
current_angles
[
i
]
=
joint
->
current_angles
[
i
]
+
((
(
signed
long
long
int
)(
joint
->
current_speeds
[
i
]
*
joint
->
motion_period
)
)
>>
17
);
if
(
((
signed
char
)
joint
->
dir
[
i
]
)
==
1
&&
joint
->
current_angles
[
i
]
>
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
=
joint
->
target_angles
[
i
];
if
(
joint
->
dir
[
i
]
==-
1
&&
joint
->
current_angles
[
i
]
<
joint
->
target_angles
[
i
])
if
(
((
signed
char
)
joint
->
dir
[
i
]
)
==-
1
&&
joint
->
current_angles
[
i
]
<
joint
->
target_angles
[
i
])
joint
->
current_angles
[
i
]
=
joint
->
target_angles
[
i
];
}
}
...
...
@@ -139,14 +139,6 @@ void joint_motion_pre_process(void *module)
joint
->
current_speeds
[
i
]
=
0
;
}
joint
->
mmodule
.
manager
->
servo_values
[
i
].
target_angle
=
joint
->
current_angles
[
i
];
if
(
joint
->
mmodule
.
manager
->
servo_configs
[
i
]
->
pid
)
{
}
else
{
joint
->
mmodule
.
manager
->
servo_values
[
i
].
cw_compliance
=
5
;
joint
->
mmodule
.
manager
->
servo_values
[
i
].
ccw_compliance
=
5
;
}
}
}
}
...
...
@@ -224,7 +216,7 @@ unsigned int joint_motion_get_group_servos(TJointMModule *joint,unsigned char gr
void
joint_motion_start
(
TJointMModule
*
joint
,
unsigned
char
group_id
)
{
unsigned
char
i
;
s
hort
int
angle
;
s
igned
long
long
int
angle
;
unsigned
int
servo_index
;
/* initialize the internal variables */
...
...
@@ -245,12 +237,13 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
// current values
joint
->
current_angles
[
i
]
=
joint
->
mmodule
.
manager
->
servo_values
[
i
].
target_angle
;
// target angle
angle
=
joint
->
memory
->
data
[
joint
->
ram_base_address
+
JOINT_MOTION_SERVO_ANGLES_OFFSET
+
i
*
2
]
+
(
joint
->
memory
->
data
[
joint
->
ram_base_address
+
JOINT_MOTION_SERVO_ANGLES_OFFSET
+
i
*
2
+
1
]
<<
8
);
angle
=
(
signed
long
long
int
)((
signed
short
int
)(
joint
->
memory
->
data
[
joint
->
ram_base_address
+
JOINT_MOTION_SERVO_ANGLES_OFFSET
+
i
*
2
]
+
(
joint
->
memory
->
data
[
joint
->
ram_base_address
+
JOINT_MOTION_SERVO_ANGLES_OFFSET
+
i
*
2
+
1
]
<<
8
)));
angle
=
angle
<<
9
;
if
(
angle
>
joint
->
mmodule
.
manager
->
servo_configs
[
i
]
->
ccw_angle_limit
)
angle
=
joint
->
mmodule
.
manager
->
servo_configs
[
i
]
->
ccw_angle_limit
;
else
if
(
angle
<
joint
->
mmodule
.
manager
->
servo_configs
[
i
]
->
cw_angle_limit
)
angle
=
joint
->
mmodule
.
manager
->
servo_configs
[
i
]
->
c
cw_angle_limit
;
joint
->
target_angles
[
i
]
=
angle
<<
9
;
angle
=
joint
->
mmodule
.
manager
->
servo_configs
[
i
]
->
cw_angle_limit
;
joint
->
target_angles
[
i
]
=
angle
;
// target speed
joint
->
target_speeds
[
i
]
=
joint
->
memory
->
data
[
joint
->
ram_base_address
+
JOINT_MOTION_SERVO_SPEEDS_OFFSET
+
i
]
<<
16
;
// target accel
...
...
@@ -263,14 +256,14 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
// check the parameters
if
(
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
])
>=
65
)
{
if
((
joint
->
target_speeds
[
i
]
*
joint
->
target_speeds
[
i
])
/
joint
->
target_accels
[
i
]
>
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
]))
if
((
(
unsigned
long
long
int
)
joint
->
target_speeds
[
i
]
*
joint
->
target_speeds
[
i
])
/
joint
->
target_accels
[
i
]
>
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
]))
{
joint
->
target_accels
[
i
]
=
(
joint
->
target_speeds
[
i
]
*
joint
->
target_speeds
[
i
])
/
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
]);
joint
->
target_accels
[
i
]
=
(
(
unsigned
long
long
int
)
joint
->
target_speeds
[
i
]
*
joint
->
target_speeds
[
i
])
/
abs
(
joint
->
target_angles
[
i
]
-
joint
->
current_angles
[
i
]);
joint
->
memory
->
data
[
joint
->
ram_base_address
+
JOINT_MOTION_SERVO_ACCELS_OFFSET
+
i
]
=
joint
->
target_accels
[
i
]
>>
16
;
}
}
// stop angles
joint
->
target_stop_angles
[
i
]
=
joint
->
target_speeds
[
i
]
*
joint
->
target_speeds
[
i
]
/
(
2
*
joint
->
target_accels
[
i
]);
joint
->
target_stop_angles
[
i
]
=
((
signed
long
long
int
)
joint
->
target_speeds
[
i
]
*
joint
->
target_speeds
[
i
]
)
/
(
2
*
joint
->
target_accels
[
i
]);
// the current angles, speeds and accelerations are in RAM
if
(
joint
->
target_angles
[
i
]
>=
joint
->
current_angles
[
i
])
joint
->
dir
[
i
]
=
1
;
...
...
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