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
51a3cc73
Commit
51a3cc73
authored
Sep 29, 2021
by
gxiang
Browse files
fixed controller
parent
2131759f
Changes
4
Hide whitespace changes
Inline
Side-by-side
common/mb_controller.c
View file @
51a3cc73
...
...
@@ -19,18 +19,20 @@ 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
kdL
=
0
.
15
;
float
kiL
=
40
;
float
kdL
=
0
.
2
;
float
kpR
=
3
.
5
;
float
kiR
=
15
;
float
kdR
=
0
.
15
;
float
kiR
=
40
;
float
kdR
=
0
.
2
;
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
);
rc_filter_enable_saturation
(
&
left_wheel_velocity_pid
,
-
1
.
0
,
1
.
0
);
rc_filter_enable_saturation
(
&
right_wheel_velocity_pid
,
-
1
.
0
,
1
.
0
);
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_wheel_low_pass
,
DT
,
0
.
08
);
rc_filter_first_order_lowpass
(
&
right_wheel_low_pass
,
DT
,
0
.
08
);
rc_filter_first_order_lowpass
(
&
left_set_low_pass
,
DT
,
0
.
08
);
rc_filter_first_order_lowpass
(
&
right_set_low_pass
,
DT
,
0
.
08
);
return
0
;
}
...
...
@@ -80,11 +82,18 @@ int mb_load_controller_config(){
*******************************************************************************/
int
mb_controller_update
(
mb_state_t
*
mb_state
,
mb_setpoints_t
*
mb_setpoints
,
int
*
reset
){
float
kL
=
1
.
5275
;
float
bL
=
0
;
float
kR
=
1
.
479313
;
float
bR
=
0
;
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
/*if (left_prev != left_vel){
left_prev = left_vel;
...
...
@@ -94,11 +103,19 @@ 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_cmd_calibration
=
(
left_vel
-
bL
)
/
kL
;
float
right_cmd_calibration
=
(
right_vel
-
bR
)
/
kR
;
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
);
mb_state
->
left_cmd
=
mb_state
->
left_cmd
+
left_cmd_calibration
;
mb_state
->
right_cmd
=
mb_state
->
right_cmd
+
right_cmd_calibration
;
if
(
*
reset
==
1
)
{
*
reset
=
0
;
...
...
common/mb_controller.h
View file @
51a3cc73
...
...
@@ -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_defs.h
View file @
51a3cc73
...
...
@@ -15,7 +15,7 @@
#define BEAGLEBONE_BLUE
#define DEFAULT_PWM_FREQ 25000 // period of motor drive pwm
#define LEFT_MOTOR 1 // id of left motor
#define RIGHT_MOTOR 3 // id of right motor
#define RIGHT_MOTOR 3
// id of right motor
// TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_MOTOR_POL 1
...
...
@@ -27,7 +27,7 @@
#define GEAR_RATIO 78.0 // gear ratio of motor
#define ENCODER_RES 20.0 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.084 // diameter of wheel in meters
#define WHEEL_BASE 0.15
8
// wheel separation distance in meters
#define WHEEL_BASE 0.15
9
// wheel separation distance in meters
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
...
...
common/mb_odometry.c
View file @
51a3cc73
...
...
@@ -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_thresh
=
0
.
3
;
//0.125/DT;
float
del_thresh
=
0
.
1
;
//0.125/DT;
boolean
Gyro
=
T
;
float
del_left
=
mb_state
->
left_encoder_delta
*
1
.
0
*
enc2meters
;
...
...
@@ -54,8 +54,11 @@ 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_go
=
del_theta_gyro
-
del_theta
;
float
theta_gyro_now
=
mb_state
->
tb_angles
[
2
];
//mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0));
float
del_theta_gyro
=
theta_gyro_now
-
mb_state
->
last_yaw
;
float
del_go
=
abs
(
del_theta_gyro
-
del_theta
);
if
(
Gyro
==
T
){
if
(
abs
(
del_go
)
>=
del_thresh
){
mb_odometry
->
theta
+=
del_theta_gyro
;}
else
{
mb_odometry
->
theta
+=
del_theta
;}
...
...
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