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
9d9d3930
Commit
9d9d3930
authored
Sep 15, 2021
by
jxchenlu
Browse files
try merging
parents
25e7c674
a0185e26
Changes
5
Hide whitespace changes
Inline
Side-by-side
common/mb_controller.c
View file @
9d9d3930
...
...
@@ -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 @
9d9d3930
...
...
@@ -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 @
9d9d3930
...
...
@@ -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 78.0 // gear ratio of motor
#define ENCODER_RES 20.0 // encoder counts per motor shaft revolution
...
...
common/mb_odometry.c
View file @
9d9d3930
...
...
@@ -19,9 +19,13 @@
*
*******************************************************************************/
void
mb_initialize_odometry
(
mb_odometry_t
*
mb_odometry
,
float
x
,
float
y
,
float
theta
){
<<<<<<<
HEAD
mb_odometry
->
x
=
x
;
mb_odometry
->
y
=
y
;
mb_odometry
->
theta
=
theta
;
=======
>>>>>>>
origin
/
withlivecode
}
...
...
mobilebot/mobilebot.c
View file @
9d9d3930
...
...
@@ -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,7 +168,12 @@ void read_mb_sensors(){
void
publish_mb_msgs
(){
mbot_imu_t
imu_msg
;
mbot_encoder_t
encoder_msg
;
<<<<<<<
HEAD
odometry_t
odo_msg
;
=======
mbot_wheel_ctrl_t
wheel_ctrl_msg
;
// odometry_t odo_msg;
>>>>>>>
origin
/
withlivecode
//Create IMU LCM Message
imu_msg
.
utime
=
now
;
...
...
@@ -185,6 +192,14 @@ 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
odo_msg
.
utime
=
now
;
odo_msg
.
x
=
mb_odometry
.
x
;
...
...
@@ -194,7 +209,12 @@ void publish_mb_msgs(){
//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
);
<<<<<<<
HEAD
odometry_t_publish
(
lcm
,
ODOMETRY_CHANNEL
,
&
odo_msg
);
=======
mbot_wheel_ctrl_t_publish
(
lcm
,
"MBOT_WHEEL_CTRL"
,
&
wheel_ctrl_msg
);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
>>>>>>>
origin
/
withlivecode
}
/*******************************************************************************
...
...
@@ -213,10 +233,16 @@ void publish_mb_msgs(){
void
mobilebot_controller
(){
update_now
();
read_mb_sensors
();
<<<<<<<
HEAD
// 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_controller_update
(
&
mb_state
,
&
mb_setpoints
);
>>>>>>>
origin
/
withlivecode
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
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