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
837959cc
Commit
837959cc
authored
Sep 25, 2021
by
gxiang
Browse files
add filter for set points, increase I, changed Odometry
parent
2131759f
Changes
6
Hide whitespace changes
Inline
Side-by-side
common/mb_controller.c
View file @
837959cc
...
...
@@ -19,10 +19,10 @@ int mb_initialize_controller(){
right_wheel_low_pass
=
rc_filter_empty
();
//put variable for left pid and right pid
float
kpL
=
3
;
float
kiL
=
15
;
float
kiL
=
20
;
float
kdL
=
0
.
15
;
float
kpR
=
3
.
5
;
float
kiR
=
1
5
;
float
kiR
=
2
5
;
float
kdR
=
0
.
15
;
rc_filter_pid
(
&
left_wheel_velocity_pid
,
kpL
,
kiL
,
kdL
,
0
.
1
,
DT
);
rc_filter_pid
(
&
right_wheel_velocity_pid
,
kpR
,
kiR
,
kdR
,
0
.
1
,
DT
);
...
...
@@ -31,6 +31,8 @@ int mb_initialize_controller(){
rc_filter_first_order_lowpass
(
&
left_wheel_low_pass
,
DT
,
0
.
06
);
rc_filter_first_order_lowpass
(
&
right_wheel_low_pass
,
DT
,
0
.
06
);
rc_filter_first_order_lowpass
(
&
left_set_low_pass
,
DT
,
0
.
1
);
rc_filter_first_order_lowpass
(
&
right_set_low_pass
,
DT
,
0
.
1
);
return
0
;
}
...
...
@@ -83,6 +85,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
mb_state
->
left_velocity
=
rc_filter_march
(
&
left_wheel_low_pass
,
mb_state
->
left_velocity
);
mb_state
->
right_velocity
=
rc_filter_march
(
&
right_wheel_low_pass
,
mb_state
->
right_velocity
);
float
left_vel
=
(
2
*
mb_setpoints
->
fwd_velocity
-
mb_setpoints
->
turn_velocity
*
WHEEL_BASE
)
/
2
;
float
right_vel
=
(
2
*
mb_setpoints
->
fwd_velocity
+
mb_setpoints
->
turn_velocity
*
WHEEL_BASE
)
/
2
;
//reset filter with a new point
...
...
@@ -94,8 +97,11 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
right_prev = right_vel;
rc_filter_reset(&right_wheel_velocity_pid);
}*/
left_vel
=
rc_filter_march
(
&
left_set_low_pass
,
left_vel
);
right_vel
=
rc_filter_march
(
&
right_set_low_pass
,
right_vel
);
float
left_error
=
left_vel
-
mb_state
->
left_velocity
;
float
right_error
=
right_vel
-
mb_state
->
right_velocity
;
mb_state
->
left_cmd
=
rc_filter_march
(
&
left_wheel_velocity_pid
,
left_error
);
mb_state
->
right_cmd
=
rc_filter_march
(
&
right_wheel_velocity_pid
,
right_error
);
...
...
common/mb_controller.h
View file @
837959cc
...
...
@@ -20,6 +20,8 @@ rc_filter_t left_wheel_velocity_pid;
rc_filter_t
right_wheel_velocity_pid
;
rc_filter_t
left_wheel_low_pass
;
rc_filter_t
right_wheel_low_pass
;
rc_filter_t
left_set_low_pass
;
rc_filter_t
right_set_low_pass
;
int
left_prev
;
int
right_prev
;
/***********
...
...
common/mb_odometry.c
View file @
837959cc
...
...
@@ -33,13 +33,13 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
* publish new odometry to lcm ODOMETRY_CHANNEL
*
*******************************************************************************/
void
mb_update_odometry
(
mb_odometry_t
*
mb_odometry
,
mb_state_t
*
mb_state
){
void
mb_update_odometry
(
mb_odometry_t
*
mb_odometry
,
mb_state_t
*
mb_state
,
float
*
prev
){
// mb_odometry is the input of previenc2metersous veh's position and orientation
// Reference: https://www.hmc.edu/lair/ARW/ARW-Lecture01-Odometry.pdf P28
//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_thresh
=
0
.
3
;
//0.125/DT;
float
del_thresh
=
100
;
//0.125/DT;
boolean
Gyro
=
T
;
float
del_left
=
mb_state
->
left_encoder_delta
*
1
.
0
*
enc2meters
;
...
...
@@ -54,7 +54,10 @@ 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
theta_gyro_now
=
mb_clamp_radians
(
mb_state
->
gyro
[
2
]
*
(
PI
/
180
.
0
));
float
del_theta_gyro
=
theta_gyro_now
-
*
prev
;
*
prev
=
theta_gyro_now
;
float
del_go
=
del_theta_gyro
-
del_theta
;
if
(
Gyro
==
T
){
if
(
abs
(
del_go
)
>=
del_thresh
){
mb_odometry
->
theta
+=
del_theta_gyro
;}
...
...
common/mb_odometry.h
View file @
837959cc
...
...
@@ -8,7 +8,7 @@
#include "mb_structs.h"
void
mb_initialize_odometry
(
mb_odometry_t
*
mb_odometry
,
float
x
,
float
y
,
float
theta
);
void
mb_update_odometry
(
mb_odometry_t
*
mb_odometry
,
mb_state_t
*
mb_state
);
void
mb_update_odometry
(
mb_odometry_t
*
mb_odometry
,
mb_state_t
*
mb_state
,
float
*
prev
);
float
mb_clamp_radians
(
float
angle
);
float
mb_angle_diff_radians
(
float
angle1
,
float
angle2
);
...
...
mobilebot/mobilebot.c
View file @
837959cc
...
...
@@ -15,7 +15,7 @@
*
*******************************************************************************/
int
main
(){
prev
=
0
;
rc_led_set
(
RC_LED_GREEN
,
LED_OFF
);
rc_led_set
(
RC_LED_RED
,
LED_ON
);
//set cpu freq to max performance
...
...
@@ -233,7 +233,7 @@ void mobilebot_controller(){
read_mb_sensors
();
// calculate odometry after update reading and defining sensors readings during each call-back
// mb_odometry x,y,theta will be updated in mb_odoemtry.c
mb_update_odometry
(
&
mb_odometry
,
&
mb_state
)
;
mb_update_odometry
(
&
mb_odometry
,
&
mb_state
,
&
prev
);
mb_controller_update
(
&
mb_state
,
&
mb_setpoints
,
&
reset
);
publish_mb_msgs
();
rc_motor_set
(
LEFT_MOTOR
,
LEFT_MOTOR_POL
*
mb_state
.
left_cmd
);
...
...
mobilebot/mobilebot.h
View file @
837959cc
...
...
@@ -52,6 +52,7 @@ mb_state_t mb_state;
mb_setpoints_t
mb_setpoints
;
mb_odometry_t
mb_odometry
;
int
reset
;
float
prev
;
int64_t
now
;
int64_t
time_offset
;
int
time_offset_initialized
;
...
...
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