diff --git a/diffbot_base/include/diffbot_base/diffbot_hw_interface.h b/diffbot_base/include/diffbot_base/diffbot_hw_interface.h index 6dd90b8e..2f1536c9 100644 --- a/diffbot_base/include/diffbot_base/diffbot_hw_interface.h +++ b/diffbot_base/include/diffbot_base/diffbot_hw_interface.h @@ -181,4 +181,4 @@ namespace diffbot_base } // namespace -#endif // DIFFBOT_HW_INTERFACE_H \ No newline at end of file +#endif // DIFFBOT_HW_INTERFACE_H diff --git a/diffbot_base/src/diffbot_hw_interface.cpp b/diffbot_base/src/diffbot_hw_interface.cpp index 7afcd058..2b8ad0fd 100644 --- a/diffbot_base/src/diffbot_hw_interface.cpp +++ b/diffbot_base/src/diffbot_hw_interface.cpp @@ -79,6 +79,10 @@ namespace diffbot_base joint_velocity_commands_[i] = 0.0; + // Initialize encoder_ticks_ to zero because receiving meaningful + // tick values from the microcontroller might take some time + encoder_ticks_[i] = 0.0; + // Initialize the pid controllers for the motors using the robot namespace std::string pid_namespace = "pid/" + motor_names[i]; ROS_INFO_STREAM("pid namespace: " << pid_namespace); @@ -311,4 +315,4 @@ namespace diffbot_base return angle * wheel_diameter_ / 2.0; } -}; \ No newline at end of file +};