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
cc8103ed
Commit
cc8103ed
authored
Sep 15, 2021
by
jxchenlu
Browse files
Merge branch 'withlivecode' into odometry
parents
9d9d3930
138d20dd
Changes
7
Hide whitespace changes
Inline
Side-by-side
common/mb_controller.c
View file @
cc8103ed
...
...
@@ -14,14 +14,19 @@ int mb_initialize_controller(){
//mb_load_controller_config();
left_wheel_velocity_pid
=
rc_filter_empty
();
right_wheel_velocity_pid
=
rc_filter_empty
();
left_wheel_low_pass
=
rc_filter_empty
();
right_wheel_low_pass
=
rc_filter_empty
();
//put variable for left pid and right pid
float
kp
=
1
.
0
;
float
ki
=
0
.
0
;
float
kd
=
0
.
0
;
float
kp
=
0
.
65
;
float
ki
=
0
.
0
2
;
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
);
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
.
1
);
rc_filter_first_order_lowpass
(
&
right_wheel_low_pass
,
DT
,
0
.
1
);
return
0
;
}
...
...
@@ -70,11 +75,15 @@ 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
;
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_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_controller.h
View file @
cc8103ed
...
...
@@ -18,6 +18,8 @@ int mb_destroy_controller();
*************/
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
;
/***********
* 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 @
cc8103ed
...
...
@@ -23,10 +23,17 @@
#define LEFT_ENC_POL 1
#define RIGHT_ENC_POL -1
// TODO: Fill in physical propeties of robot
<<<<<<<
HEAD
#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.158 // wheel separation distance in meters
=======
#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
>>>>>>>
withlivecode
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
...
...
@@ -49,6 +56,7 @@
#define MBOT_ENCODER_CHANNEL "MBOT_ENCODERS"
#define MBOT_MOTOR_COMMAND_CHANNEL "MBOT_MOTOR_COMMAND"
#define MBOT_TIMESYNC_CHANNEL "MBOT_TIMESYNC"
#define MBOT_WHEEL_CTRL_CHANNEL "MBOT_CTRL_COMMAND"
#define LCM_ADDRESS "udpm://239.255.76.67:7667?ttl=1"
#define LED_OFF 1
...
...
measure_motor_params/measure_motors.c
View file @
cc8103ed
...
...
@@ -20,11 +20,11 @@
#include <rc/motor.h>
#include "../common/mb_motor.h"
#include "../common/mb_defs.h"
#include "../common/mb_structs.h"
float
enc2meters
=
(
WHEEL_DIAMETER
*
M_PI
)
/
(
GEAR_RATIO
*
ENCODER_RES
);
void
test_speed
(
float
du
,
float
dtime_s
);
void
test_speed
(
float
du
ty
,
float
dtime_s
,
mb_state_t
*
mb
);
/*******************************************************************************
* int main()
...
...
@@ -59,11 +59,10 @@ int main(){
}
#endif
if
(
rc_encoder_
eqep_
init
()
<
0
){
if
(
rc_encoder_init
()
<
0
){
fprintf
(
stderr
,
"ERROR: failed to initialze encoders
\n
"
);
return
-
1
;
}
// make PID file to indicate your project is running
// due to the check made on the call to rc_kill_existing_process() above
// we can be fairly confident there is no PID file already and we can
...
...
@@ -75,15 +74,43 @@ int main(){
if
(
rc_get_state
()
==
RUNNING
){
rc_nanosleep
(
1E9
);
//sleep for 1s
//TODO: write routine here
}
// TODO: Plase exit routine here
rc_encoder_eqep_write
(
1
,
0
);
rc_encoder_eqep_write
(
2
,
0
);
mb_state_t
mb
;
mb
.
left_encoder_delta
=
0
;
mb
.
right_encoder_delta
=
0
;
//set up file for output
FILE
*
fpL
;
FILE
*
fpR
;
fpL
=
fopen
(
"left_speed.txt"
,
"w+"
);
fpR
=
fopen
(
"right_speed.txt"
,
"w+"
);
for
(
float
pwm
=
0
.
0
;
pwm
<=
1
.
0
;
pwm
+=
0
.
05
){
rc_encoder_eqep_write
(
1
,
0
);
rc_encoder_eqep_write
(
2
,
0
);
test_speed
(
pwm
,
1E9
,
&
mb
);
fprintf
(
fpL
,
"%f
\t
%f
\n
"
,
pwm
,
mb
.
left_velocity
);
fprintf
(
fpR
,
"%f
\t
%f
\n
"
,
pwm
,
mb
.
right_velocity
);
}
// TODO: Plase exit routine here
fclose
(
fpL
);
fclose
(
fpR
);
rc_encoder_cleanup
();
rc_motor_cleanup
();
rc_set_state
(
EXITING
);
// remove pid file LAST
rc_remove_pid_file
();
return
0
;
}
void
test_speed
(
float
duty
,
float
dtime_s
){
}
\ No newline at end of file
void
test_speed
(
float
duty
,
float
dtime_s
,
mb_state_t
*
mb
){
rc_motor_set
(
1
,
duty
);
rc_motor_set
(
2
,
-
duty
);
rc_nanosleep
(
2E8
);
rc_nanosleep
(
dtime_s
);
mb
->
left_encoder_delta
=
rc_encoder_eqep_read
(
1
);
mb
->
right_encoder_delta
=
rc_encoder_eqep_read
(
2
);
mb
->
left_velocity
=
mb
->
left_encoder_delta
*
1
.
0
*
enc2meters
/
(
dtime_s
/
1E9
);
mb
->
right_velocity
=
mb
->
right_encoder_delta
*
1
.
0
*
enc2meters
/
(
dtime_s
/
1E9
);
}
mobilebot/mobilebot.c
View file @
cc8103ed
...
...
@@ -195,10 +195,11 @@ void publish_mb_msgs(){
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
;
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
;
//TODO: Create Odometry LCM message
odo_msg
.
utime
=
now
;
...
...
@@ -283,6 +284,7 @@ void motor_command_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const
mbot_motor_command_t
*
msg
,
void
*
user
){
mb_setpoints
.
fwd_velocity
=
msg
->
trans_v
;
mb_setpoints
.
turn_velocity
=
msg
->
angular_v
;
}
...
...
mobilebot/mobilebot.h
View file @
cc8103ed
...
...
@@ -36,6 +36,7 @@
#include "../lcmtypes/oled_message_t.h"
#include "../lcmtypes/timestamp_t.h"
#include "../lcmtypes/reset_odometry_t.h"
#include "../lcmtypes/mbot_wheel_ctrl_t.h"
#include "../common/mb_defs.h"
#include "../common/mb_structs.h"
...
...
python/plot_step.py
View file @
cc8103ed
...
...
@@ -33,6 +33,7 @@ for event in log:
cmd
=
data
[:,
3
]
start
=
np
.
where
(
cmd
>=
0.1
)[
0
][
0
]
-
5
end
=
start
+
500
#plot 10s worth of data
plt
.
figure
(
0
)
p1
=
plt
.
plot
(
data
[
start
:
end
,
0
],
data
[
start
:
end
,
3
],
'g'
,
label
=
"vel cmd"
)
p2
=
plt
.
plot
(
data
[
start
:
end
,
0
],
data
[
start
:
end
,
1
],
'r'
,
label
=
"pwm cmd"
)
p3
=
plt
.
plot
(
data
[
start
:
end
,
0
],
data
[
start
:
end
,
5
],
'b'
,
label
=
"velocity"
)
...
...
@@ -40,3 +41,8 @@ plt.legend(loc="upper right")
filename
,
file_extension
=
os
.
path
.
splitext
(
file
)
plt
.
savefig
(
filename
+
'.png'
)
plt
.
show
()
plt
.
figure
(
1
)
p1
=
plt
.
plot
(
data
[
start
:
end
,
0
],
data
[
start
:
end
,
4
],
'g'
,
label
=
"vel cmd"
)
p2
=
plt
.
plot
(
data
[
start
:
end
,
0
],
data
[
start
:
end
,
2
],
'r'
,
label
=
"pwm cmd"
)
p3
=
plt
.
plot
(
data
[
start
:
end
,
0
],
data
[
start
:
end
,
6
],
'b'
,
label
=
"velocity"
)
plt
.
show
()
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