yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

Added optional encoder jump detection

Browse Source
This commit is contained in:
Patrick Goebel 2016-11-22 07:05:12 -08:00
parent 9d29c18769
commit 6c77321d66
1 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ros_arduino_python/src/ros_arduino_python/base_controller.py
View File

@ -59,6 +59,8 @@ class BaseController:
self.accel_limit = rospy.get_param('~accel_limit', 1.0)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
self.detect_enc_jump_error = rospy.get_param("~detect_enc_jump_error", False)
self.enc_jump_error_threshold = rospy.get_param("~enc_jump_error_threshold", 1000)
# Default error threshold (percent) before getting a diagnostics warning
self.base_diagnotics_error_threshold = rospy.get_param("~base_diagnotics_error_threshold", 10)
@ -152,6 +154,30 @@ class BaseController:
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
# Check for jumps in encoder readings
if self.detect_enc_jump_error:
try:
#rospy.loginfo("Left: %d LEFT: %d Right: %d RIGHT: %d", left_enc, self.enc_left, right_enc, self.enc_right)
enc_jump_error = False
if abs(right_enc - self.enc_right) > self.enc_jump_error_threshold:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("RIGHT encoder jump error from %d to %d", self.enc_right, right_enc)
self.enc_right = right_enc
enc_jump_error = True
if abs(left_enc - self.enc_left) > self.enc_jump_error_threshold:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("LEFT encoder jump error from %d to %d", self.enc_left, left_enc)
self.enc_left = left_enc
enc_jump_error = True
if enc_jump_error:
return
except:
pass
dt = now - self.then
self.then = now
dt = dt.to_sec()