def __init__(self, ros, name, pin):
self.name = name
self.en_pin = pin
self.count = 0
self.prev_pin_val = pin.value()
self.ros = ros
self.ros_pub = Topic(ros, "hadabot/wheel_radps_{}".format(self.name),
"std_msgs/Float32")
class Encoder:
def __init__(self, ros, name, pin):
self.name = name
self.en_pin = pin
self.count = 0
self.prev_pin_val = pin.value()
self.ros = ros
self.ros_pub = Topic(ros, "hadabot/wheel_radps_{}".format(self.name),
"std_msgs/Float32")
def publish_radps(self, radps):
# logger.info("Encoder publish {}".format(self.name))
self.ros_pub.publish(Message({"data": radps}))
def main(argv):
ros = None
try:
boot_button = Pin(0, Pin.IN)
builtin_led = Pin(BUILTIN_LED, Pin.OUT)
ros = Ros(CONFIG["ros2_web_bridge_ip_addr"])
log_info = Topic(ros, "/hadabot/log/info", "std_msgs/String")
twist_cmd = Topic(ros, "/hadabot/cmd_vel", "geometry_msgs/Twist")
twist_cmd.subscribe(twist_cmd_cb)
cnt = 0
# Loop forever
while(boot_button.value() == 1):
ros.run_once()
if (cnt % 1000) == 0:
log_info.publish(Message(
"Hadabot heartbeat {}".format(cnt * 0.001)))
cnt += 1
time.sleep(0.01)
logger.info("Boot button pushed... exiting")
except KeyboardInterrupt:
logger.info("Keyboard control-c hit... stopping")
except Exception as ex:
logger.error("Some other exception hit {}".format(str(ex)))
if ros is not None:
ros.terminate()
# Blink 5 times
blink_led(5, end_off=True)
raise ex
# Shutdown
ros.terminate()
builtin_led.off()
def main(argv):
ros = None
try:
hadabot_ip_address = CONFIG["network"]["hadabot_ip_address"]
boot_button = Pin(0, Pin.IN)
builtin_led = Pin(BUILTIN_LED, Pin.OUT)
ros = Ros(CONFIG["ros2_web_bridge_ip_addr"])
# Publish out log info
log_info = Topic(ros, "hadabot/log/info", "std_msgs/String")
# Controller
controller = Controller(ros)
# Subscribe to twist topics
# twist_cmd = Topic(ros, "hadabot/cmd_vel", "geometry_msgs/Twist")
# twist_cmd.subscribe(twist_cmd_cb)
# Subscribe to wheel turn callbacks
rw_sub_topic = Topic(
ros, "hadabot/wheel_power_right", "std_msgs/Float32")
rw_sub_topic.subscribe(controller.right_wheel_cb)
lw_sub_topic = Topic(
ros, "hadabot/wheel_power_left", "std_msgs/Float32")
lw_sub_topic.subscribe(controller.left_wheel_cb)
# Loop forever
last_hb_ms = time.ticks_ms()
last_controller_ms = time.ticks_ms()
while(boot_button.value() == 1):
ros.run_once()
cur_ms = time.ticks_ms()
# logger.info("cur ms - {}".format(cur_ms))
# logger.info("last_controller ms - {}".format(last_controller_ms))
# logger.info("last_hb ms - {}".format(last_hb_ms))
# Run the controller
if time.ticks_diff(cur_ms, last_controller_ms) > 100:
controller.run_once()
last_controller_ms = cur_ms
# Publish heartbeat message
if time.ticks_diff(cur_ms, last_hb_ms) > 5000:
# logger.info("Encoder left - {}".format(en_count_left))
# logger.info("Encoder right - {}".format(en_count_right))
log_info.publish(Message(
"Hadabot heartbeat - IP {}".format(hadabot_ip_address)))
last_hb_ms = cur_ms
time.sleep(0.01)
logger.info("Boot button pushed... exiting")
except KeyboardInterrupt:
logger.info("Keyboard control-c hit... stopping")
except Exception as ex:
logger.error("Some other exception hit {}".format(str(ex)))
if ros is not None:
ros.terminate()
# Blink 5 times
blink_led(5, end_off=True)
raise ex
# Shutdown
ros.terminate()
builtin_led.off()
def init_ros(self, ros):
self.ros = ros
self.ros_pub = Topic(ros, "hadabot/wheel_radps_{}".format(self.name),
"std_msgs/Float32")
class Encoder:
TICKS_PER_REVOLUTION = 40.0
DEBOUNCE_US = 8000
def __init__(self, name):
self.name = name
# Used by IRQ handler, need semaphore?
self.en_count = 0
self.en_count_us = time.ticks_us()
# Used for debouncing code
self.en_debounce_us = 0
# END Used by IRQ handler
self.ros = None
self.ros_pub = None
# Used for when we publish out the tick count
self.last_pub_us = 0
self.last_pub_en_count = 0
self.last_pub_en_count_us = self.en_count_us
self.last_pub_en_count_diff = 0
def irq_handler(self, pin):
t_us = time.ticks_us()
if time.ticks_diff(t_us, self.en_debounce_us) > self.DEBOUNCE_US:
self.en_count += 1
self.en_count_us = t_us
# We assume real signal is less than debounce interval
# so always update the last bounce time
self.en_debounce_us = t_us
def init_ros(self, ros):
self.ros = ros
self.ros_pub = Topic(ros, "hadabot/wheel_radps_{}".format(self.name),
"std_msgs/Float32")
def run_once(self):
cur_us = time.ticks_us()
# Do we need to disable IRQ to avoid race conditions??
en_count = self.en_count
en_count_us = self.en_count_us
# If the rotation changed, then publish more frequently. Even more
# frequently if stopped from a moving velocity
nochange_pub_freq_us = 2000000
# The number of ticks between current tick count versus what we
# published last time
en_count_diff = en_count - self.last_pub_en_count
# If stopped from a moving velocity then publish that asap
changed_pub_freq_us = 100000 if en_count_diff == 0 else 200000
# Was the last published en_count delta same as what we published last
# time?
last_pub_same_as_current = self.last_pub_en_count_diff == en_count_diff
# If last published en_count delta is different from current delta,
# and enough time has elapsed, publish out
need_to_pub_change = ((time.ticks_diff(cur_us, self.last_pub_us) >
(changed_pub_freq_us))
and (last_pub_same_as_current is False))
# If last published en_count delta is equal to current delta,
# and a longer time has elapsed, then publish out as well
need_to_pub_unchanged = time.ticks_diff(
cur_us, self.last_pub_us) > (nochange_pub_freq_us)
# Do we need to publish out?
if need_to_pub_unchanged or need_to_pub_change:
# Publish out radians per sec
time_delta_us = time.ticks_diff(en_count_us,
self.last_pub_en_count_us)
if self.last_pub_en_count_diff == 0:
# If the wheels were not spinning, the last encoder update
# could have happened aeons ago, so use last published
# time for time delta to get more accurate rad per sec.
# Only significant if need_to_pub_change is true
time_delta_us = time.ticks_diff(en_count_us, self.last_pub_us)
rps = 0.0
# Could be less than zero because our last encoder update could
# have happened before the last published update of unchanged
# state.
if time_delta_us > 0:
# Radians per second
radians = 2 * math.pi * ((en_count - self.last_pub_en_count) /
self.TICKS_PER_REVOLUTION)
rps = radians * 1000000.0 / float(time_delta_us)
self.ros_pub.publish(Message({"data": rps}))
# Update values published
self.last_pub_en_count = en_count
self.last_pub_en_count_us = en_count_us
self.last_pub_us = cur_us
self.last_pub_en_count_diff = en_count_diff