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
a0185e26
Commit
a0185e26
authored
Sep 12, 2021
by
gxiang
Browse files
add live code result
parent
0ace834f
Changes
5
Hide whitespace changes
Inline
Side-by-side
common/mb_controller.c
View file @
a0185e26
...
...
@@ -11,7 +11,17 @@
*******************************************************************************/
int
mb_initialize_controller
(){
mb_load_controller_config
();
//mb_load_controller_config();
left_wheel_velocity_pid
=
rc_filter_empty
();
right_wheel_velocity_pid
=
rc_filter_empty
();
//put variable for left pid and right pid
float
kp
=
1
.
0
;
float
ki
=
0
.
0
;
float
kd
=
0
.
0
;
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
);
rc_filter_enable_saturation
(
&
left_wheel_velocity_pid
,
-
1
.
0
,
1
.
0
);
rc_filter_enable_saturation
(
&
right_wheel_velocity_pid
,
-
1
.
0
,
1
.
0
);
return
0
;
}
...
...
@@ -61,6 +71,10 @@ int mb_load_controller_config(){
*******************************************************************************/
int
mb_controller_update
(
mb_state_t
*
mb_state
,
mb_setpoints_t
*
mb_setpoints
){
float
left_error
=
mb_setpoints
->
fwd_velocity
-
mb_state
->
left_velocity
;
float
right_error
=
mb_setpoints
->
fwd_velocity
-
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
);
return
0
;
}
...
...
@@ -75,5 +89,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
*******************************************************************************/
int
mb_destroy_controller
(){
rc_filter_free
(
&
left_wheel_velocity_pid
);
rc_filter_free
(
&
right_wheel_velocity_pid
);
return
0
;
}
common/mb_controller.h
View file @
a0185e26
...
...
@@ -16,7 +16,8 @@ int mb_destroy_controller();
* rc_filter_t left_wheel_speed_pid;
* rc_filter_t fwd_vel_sp_lpf;
*************/
rc_filter_t
left_wheel_velocity_pid
;
rc_filter_t
right_wheel_velocity_pid
;
/***********
* For each PID filter you want to load from settings
* add a pid_parameter_t or filter_parameter_t
...
...
common/mb_defs.h
View file @
a0185e26
...
...
@@ -18,7 +18,10 @@
#define RIGHT_MOTOR 2 // id of right motor
// 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
// TODO: Fill in physical propeties of robot
#define GEAR_RATIO 0.0 // gear ratio of motor
#define ENCODER_RES 0.0 // encoder counts per motor shaft revolution
...
...
common/mb_odometry.c
View file @
a0185e26
...
...
@@ -19,6 +19,7 @@
*
*******************************************************************************/
void
mb_initialize_odometry
(
mb_odometry_t
*
mb_odometry
,
float
x
,
float
y
,
float
theta
){
}
...
...
mobilebot/mobilebot.c
View file @
a0185e26
...
...
@@ -151,7 +151,9 @@ void read_mb_sensors(){
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
.
right_velocity
=
enc2meters
*
mb_state
.
right_encoder_delta
/
DT
;
//unlock state mutex
pthread_mutex_unlock
(
&
state_mutex
);
...
...
@@ -166,6 +168,7 @@ void read_mb_sensors(){
void
publish_mb_msgs
(){
mbot_imu_t
imu_msg
;
mbot_encoder_t
encoder_msg
;
mbot_wheel_ctrl_t
wheel_ctrl_msg
;
// odometry_t odo_msg;
//Create IMU LCM Message
...
...
@@ -185,11 +188,20 @@ void publish_mb_msgs(){
encoder_msg
.
leftticks
=
mb_state
.
left_encoder_total
;
encoder_msg
.
rightticks
=
mb_state
.
right_encoder_total
;
wheel_ctrl_msg
.
utime
=
now
;
wheel_ctrl_msg
.
left_motor_pwm_cmd
=
mb_state
.
left_cmd
;
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
=
mb_state
.
left_velocity
;
wheel_ctrl_msg
.
right_motor_vel
=
mb_state
.
right_velocity
;
//TODO: Create Odometry LCM message
//publish IMU & Encoder Data to LCM
mbot_imu_t_publish
(
lcm
,
MBOT_IMU_CHANNEL
,
&
imu_msg
);
mbot_encoder_t_publish
(
lcm
,
MBOT_ENCODER_CHANNEL
,
&
encoder_msg
);
mbot_wheel_ctrl_t_publish
(
lcm
,
"MBOT_WHEEL_CTRL"
,
&
wheel_ctrl_msg
);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
}
...
...
@@ -209,7 +221,10 @@ void publish_mb_msgs(){
void
mobilebot_controller
(){
update_now
();
read_mb_sensors
();
mb_controller_update
(
&
mb_state
,
&
mb_setpoints
);
publish_mb_msgs
();
rc_motor_set
(
LEFT_MOTOR
,
LEFT_MOTOR_POL
*
mb_state
.
left_cmd
);
rc_motor_set
(
RIGHT_MOTOR
,
RIGHT_MOTOR_POL
*
mb_state
.
right_cmd
);
}
...
...
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