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
gxiang
Mobilebot
Commits
267d66f5
Commit
267d66f5
authored
Sep 25, 2021
by
jxchenlu
Browse files
update delta gyro
parent
9bb053ff
Changes
1
Hide whitespace changes
Inline
Side-by-side
common/mb_odometry.c
View file @
267d66f5
...
...
@@ -39,7 +39,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
//del_thresh Needs to be tuned
//IF DON'T WANT GYRO, SET IT AS F!
float
enc2meters
=
(
WHEEL_DIAMETER
*
PI
)
/
(
GEAR_RATIO
*
ENCODER_RES
);
float
del_thesh
=
0
.
125
/
DT
;
float
del_thesh
=
0
.
3
;
//
0.125/DT;
boolean
Gyro
=
T
;
float
del_left
=
mb_state
->
left_encoder_delta
*
1
.
0
*
enc2meters
;
...
...
@@ -54,7 +54,8 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
// Gyro: we only need to use yaw
// Since each time step is small, no need to integrate
float
del_theta_gyro
=
mb_clamp_radians
(
mb_state
->
gyro
[
2
]
*
(
PI
/
180
.
0
)
*
DT
);
//float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT);
float
del_theta_gyro
=
mb_clamp_radians
(
mb_state
->
tb_angles
[
2
]
-
mb_state
->
last_yaw
);
float
del_go
=
del_theta_gyro
-
del_theta
;
if
(
Gyro
==
T
){
if
(
abs
(
del_go
)
>
del_thesh
){
mb_odometry
->
theta
+=
del_theta_gyro
*
DT
;}
...
...
Write
Preview
Markdown
is supported
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