Exemple #1
0
    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")
Exemple #2
0
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}))
Exemple #3
0
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()
Exemple #4
0
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()
Exemple #5
0
 def init_ros(self, ros):
     self.ros = ros
     self.ros_pub = Topic(ros, "hadabot/wheel_radps_{}".format(self.name),
                          "std_msgs/Float32")
Exemple #6
0
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