def run(): arm = arm_container.create_3_dof() state = State(arm) from threading import Thread cmd_thread = Thread(target=command_proc, name='Command Thread', args=(state,)) cmd_thread.start() print_and_cr("Press 'b2' to add waypoint ('b3' for stopping at this waypoint), 'b4' to clear waypoints, 'b5' to playback, and 'b1' to quit.") print_and_cr("When in playback mode, 'b6' resumes training, and 'b1' quits.") # Mobile device setup phone_family = 'HEBI' phone_name = "mobileIO" lookup = hebi.Lookup() sleep(2) print('Waiting for Mobile IO device to come online...') m = create_mobile_io(lookup, phone_family, phone_name) if m is None: raise RuntimeError("Could not find Mobile IO device") m.update() while not m.get_button_state(1): # Update MobileIO state if not m.update(): print("Failed to get feedback from MobileIO") continue state.lock() current_mode = state.mode if current_mode == 'training': m.set_led_color("blue") if m.get_button_state(2): add_waypoint(state, False) elif m.get_button_state(3): add_waypoint(state, True) elif m.get_button_state(4): clear_waypoints(state) elif m.get_button_state(5): if state.number_of_waypoints > 1: state._mode = 'playback' else: print_and_cr('Need at least two waypoints to enter playback mode!') elif current_mode == 'playback': m.set_led_color("green") if m.get_button_state(6): state._mode = 'training' state.unlock() m.set_led_color("red") state._quit = True print_and_cr('')
if __name__ == "__main__": from .tready_utils import set_mobile_io_instructions, setup_base lookup = hebi.Lookup() sleep(2) family = "Tready" # Base setup base = setup_base(lookup, family) # mobileIO setup phone_name = "mobileIO" print('Waiting for mobileIO device to come online...') m = create_mobile_io(lookup, family, phone_name) while m is None: m = create_mobile_io(lookup, family, phone_name) print("Could not find mobileIO device, waiting...") sleep(0.5) print("mobileIO device found.") input_parser = config_mobile_io(m) demo_controller = TreadyControl(base) def update_mobile_io(controller: TreadyControl, new_state: TreadyControlState): if controller.state == new_state: return
sleep(1) zoom_group = lookup.get_group_from_names('C10', ['C10-0001']) cam_group = lookup.get_group_from_names('CW1', ['CW1-0004']) while cam_group is None: print('Looking for camera...') sleep(1) cam_group = lookup.get_group_from_names('CW1', ['CW1-0004']) mast = HebiCameraMast(group) zoom_camera = HebiCamera(zoom_group) camera = HebiCamera(cam_group) # Setup MobileIO print('Looking for Mobile IO...') m = create_mobile_io(lookup, family) while m is None: print('Waiting for Mobile IO device to come online...') sleep(1) m = create_mobile_io(lookup, family) m.update() setup_mobile_io(m) mast_control = MastControl(mast, zoom_camera) ####################### ## Main Control Loop ## ####################### while mast_control.running:
# Create base group base_group = lookup.get_group_from_names('Tready', wheel_names + flipper_names) while base_group is None: print('Looking for Tready modules...') sleep(1) base_group = lookup.get_group_from_names('Tready', wheel_names + flipper_names) base = TreadedBase(base_group, 0.25, 0.33) base_control = TreadyControl(base) # Setup MobileIO print('Looking for Mobile IO...') m = create_mobile_io(lookup, 'Tready') while m is None: print('Waiting for Mobile IO device to come online...') sleep(1) m = create_mobile_io(lookup, 'Tready') m.update() setup_mobile_io(m) def update_mobile_ui(controller: TreadyControl, new_state: TreadyControlState): if controller.state == TreadyControlState.DISCONNECTED and new_state == TreadyControlState.TELEOP: setup_mobile_io(m) base_control._transition_handlers.append(update_mobile_ui)
if __name__ == "__main__": from .tready_utils import set_mobile_io_instructions, setup_base lookup = hebi.Lookup() sleep(2) family = "Tready" # Base setup base = setup_base(lookup, family) # mobileIO setup phone_name = "mobileIO" print('Waiting for mobileIO device to come online...') m_tready = create_mobile_io(lookup, family, phone_name) while m_tready is None: m_tready = create_mobile_io(lookup, family, phone_name) print("Could not find mobileIO device, waiting...") sleep(0.5) print("mobileIO device found.") input_parser = config_mobile_io(m_tready) demo_controller = TreadyControl(base) def update_mobile_io(controller: TreadyControl, new_state: TreadyControlState): if controller.state == new_state: return
base_family = "HEBI" module_names = ['W1_left', 'W2_right'] # Create group group = lookup.get_group_from_names([base_family], module_names) if group is None: raise RuntimeError(f"Could not find Diff Drive modules: {module_names} in family '{base_family}'") load_gains(group, "gains/diff_drive.xml") base = DiffDrive(group) # mobileIO setup phone_name = "mobileIO" print('Waiting for Mobile IO device to come online...') m = create_mobile_io(lookup, base_family, phone_name) if m is None: raise RuntimeError("Could not find Mobile IO device") m.set_led_color("blue") m.update() # Demo Variables abort_flag = False # Print Instructions instructions = "B8 - Quit" m.clear_text() m.add_text(instructions) #######################
m.set_button_mode(4, 0) m.set_axis_label(3, 'grip', blocking=False) m.set_axis_value(3, -1.0) m.set_button_label(8, 'Reset', blocking=False) if __name__ == "__main__": base_dir, _ = os.path.split(__file__) lookup = hebi.Lookup() sleep(2) # Setup MobileIO print('Looking for Mobile IO...') m = create_mobile_io(lookup, 'Arm') while m is None: print('Waiting for Mobile IO device to come online...') sleep(1) m = create_mobile_io(lookup, 'Arm') setup_mobile_io(m) maps_modules = ['J1-A', 'J2-B', 'J2-A', 'J3-B', 'J3-A', 'J4-B', 'J4-A'] maps_group = lookup.get_group_from_names(['MAPS'], maps_modules) while maps_group is None: m.clear_text(blocking=False) m.add_text( 'MAPS arm not found: Check connection and make sure all modules are blinking green', blocking=False) sleep(1)
] hrdf_file = "hrdf/A-2085-06.hrdf" gains_file = "gains/A-2085-06.xml" # Create Arm object arm = hebi.arm.create([arm_family], names=module_names, hrdf_file=hrdf_file, lookup=lookup) arm.load_gains(gains_file) arm_control = ArmMobileIOControl(arm) # Setup MobileIO print('Looking for Mobile IO...') m = create_mobile_io(lookup, arm_family) while m is None: print('Waiting for Mobile IO device to come online...') sleep(1) m = create_mobile_io(lookup, arm_family) m.update() setup_mobile_io(m) ####################### ## Main Control Loop ## ####################### while arm_control.running: t = time() arm_inputs = parse_mobile_feedback(m)