diff --git a/omnidrive_rppico/hardware/omnibot_pico_system.cpp b/omnidrive_rppico/hardware/omnibot_pico_system.cpp index 9de356f..4c48e47 100644 --- a/omnidrive_rppico/hardware/omnibot_pico_system.cpp +++ b/omnidrive_rppico/hardware/omnibot_pico_system.cpp @@ -249,10 +249,10 @@ hardware_interface::return_type omnidrive_rppico ::OmniDriveRpPicoHardware::writ return hardware_interface::return_type::ERROR; } - int motor_f_counts_per_loop = wheel_f_.cmd / wheel_f_.rads_per_count / cfg_.loop_rate; - int motor_l_counts_per_loop = wheel_l_.cmd / wheel_l_.rads_per_count / cfg_.loop_rate; - int motor_b_counts_per_loop = wheel_b_.cmd / wheel_b_.rads_per_count / cfg_.loop_rate; - int motor_r_counts_per_loop = wheel_r_.cmd / wheel_r_.rads_per_count / cfg_.loop_rate; + int motor_f_counts_per_loop = wheel_f_.cmd / wheel_f_.rads_per_count; + int motor_l_counts_per_loop = wheel_l_.cmd / wheel_l_.rads_per_count; + int motor_b_counts_per_loop = wheel_b_.cmd / wheel_b_.rads_per_count; + int motor_r_counts_per_loop = wheel_r_.cmd / wheel_r_.rads_per_count; diff --git a/src/ezbot_robot/config/omnidirectional_controller.yaml b/src/ezbot_robot/config/omnidirectional_controller.yaml index 7880db6..5d9fa81 100644 --- a/src/ezbot_robot/config/omnidirectional_controller.yaml +++ b/src/ezbot_robot/config/omnidirectional_controller.yaml @@ -20,7 +20,7 @@ omnidirectional_controller: robot_radius: 0.15 wheel_radius: 0.03 - publish_rate: 50.0 + publish_rate: 30.0 odom_frame_id: odom base_frame_id: base_link pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]