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)
