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
63a15082
Commit
63a15082
authored
Sep 17, 2021
by
gxiang
Browse files
added controller
parent
138d20dd
Changes
3
Hide whitespace changes
Inline
Side-by-side
common/mb_controller.c
View file @
63a15082
...
...
@@ -17,11 +17,14 @@ int mb_initialize_controller(){
left_wheel_low_pass
=
rc_filter_empty
();
right_wheel_low_pass
=
rc_filter_empty
();
//put variable for left pid and right pid
float
kp
=
0
.
65
;
float
ki
=
0
.
02
;
float
kd
=
0
.
15
;
rc_filter_pid
(
&
left_wheel_velocity_pid
,
kp
,
ki
,
kd
,
0
.
1
,
DT
);
rc_filter_pid
(
&
right_wheel_velocity_pid
,
kp
,
ki
,
kd
,
0
.
1
,
DT
);
float
kpL
=
2
;
float
kiL
=
7
;
float
kdL
=
0
.
2
;
float
kpR
=
2
;
float
kiR
=
7
;
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
);
...
...
@@ -76,14 +79,15 @@ int mb_load_controller_config(){
*******************************************************************************/
int
mb_controller_update
(
mb_state_t
*
mb_state
,
mb_setpoints_t
*
mb_setpoints
){
//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_error
=
mb_setpoints
->
fwd_velocity
-
LEFT_MOTOR_POL
*
mb_state
->
left_velocity
;
float
right_error
=
mb_setpoints
->
fwd_velocity
-
RIGHT_MOTOR_POL
*
mb_state
->
right_velocity
;
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
;
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
=
rc_filter_march
(
&
left_wheel_low_pass
,
mb_state
->
left_cmd
);
mb_state
->
right_cmd
=
rc_filter_march
(
&
right_wheel_low_pass
,
mb_state
->
right_cmd
);
return
0
;
}
...
...
common/mb_defs.h
View file @
63a15082
...
...
@@ -20,13 +20,13 @@
// TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_MOTOR_POL 1
#define RIGHT_MOTOR_POL -1
#define LEFT_ENC_POL 1
#define RIGHT_ENC_POL
-
1
#define LEFT_ENC_POL
-
1
#define RIGHT_ENC_POL 1
// TODO: Fill in physical propeties of robot
#define GEAR_RATIO 78 // gear ratio of motor
#define ENCODER_RES 20 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.08 // diameter of wheel in meters
#define WHEEL_BASE 0.
0
// wheel separation distance in meters
#define WHEEL_BASE 0.
157
// 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)
...
...
mobilebot/mobilebot.c
View file @
63a15082
...
...
@@ -145,14 +145,14 @@ void read_mb_sensors(){
}
// Read encoders
mb_state
.
left_encoder_delta
=
rc_encoder_read
(
LEFT_MOTOR
);
mb_state
.
right_encoder_delta
=
rc_encoder_read
(
RIGHT_MOTOR
);
mb_state
.
left_encoder_delta
=
LEFT_ENC_POL
*
rc_encoder_read
(
LEFT_MOTOR
);
mb_state
.
right_encoder_delta
=
RIGHT_ENC_POL
*
rc_encoder_read
(
RIGHT_MOTOR
);
mb_state
.
left_encoder_total
+=
mb_state
.
left_encoder_delta
;
mb_state
.
right_encoder_total
+=
mb_state
.
right_encoder_delta
;
rc_encoder_write
(
LEFT_MOTOR
,
0
);
rc_encoder_write
(
RIGHT_MOTOR
,
0
);
float
enc2meters
=
(
WHEEL_DIAMETER
*
3
.
1415
)
/
(
GEAR_RATIO
*
ENCODER_RES
);
mb_state
.
left_velocity
=
enc2meters
*
mb_state
.
left_encoder_delta
/
DT
;
mb_state
.
left_velocity
=
enc2meters
*
mb_state
.
left_encoder_delta
/
DT
;
mb_state
.
right_velocity
=
enc2meters
*
mb_state
.
right_encoder_delta
/
DT
;
//unlock state mutex
pthread_mutex_unlock
(
&
state_mutex
);
...
...
@@ -193,8 +193,8 @@ void publish_mb_msgs(){
wheel_ctrl_msg
.
right_motor_pwm_cmd
=
mb_state
.
right_cmd
;
wheel_ctrl_msg
.
left_motor_vel_cmd
=
mb_setpoints
.
fwd_velocity
;
wheel_ctrl_msg
.
right_motor_vel_cmd
=
mb_setpoints
.
fwd_velocity
;
wheel_ctrl_msg
.
left_motor_vel
=
-
1
*
mb_state
.
left_velocity
;
wheel_ctrl_msg
.
right_motor_vel
=
mb_state
.
right_velocity
;
wheel_ctrl_msg
.
left_motor_vel
=
LEFT_MOTOR_POL
*
mb_state
.
left_velocity
;
wheel_ctrl_msg
.
right_motor_vel
=
RIGHT_ENC_POL
*
mb_state
.
right_velocity
;
//TODO: Create Odometry LCM message
...
...
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