# mobileIO setup
phone_name = "mobileIO"
lookup = hebi.Lookup()
sleep(2)
# Setup MobileIO
print('Waiting for Mobile IO device to come online...')
m = create_mobile_io(lookup, arm_family, phone_name)
if m is None:
raise RuntimeError("Could not find Mobile IO device")
m.update()
# Demo Variables
abort_flag = False
run_mode = "softstart"
goal = arm_api.Goal(arm.size)
home_position = [0, np.pi / 3, 2 * np.pi / 3, 5 * np.pi / 6, -np.pi / 2, 0]
# Command the softstart to the home position
softstart = arm_api.Goal(arm.size)
softstart.add_waypoint(t=4, position=home_position)
arm.update()
arm.set_goal(softstart)
arm.send()
# Get the cartesian position and rotation matrix @ home position
xyz_home = np.zeros(3)
rot_home = np.zeros((3, 3))
# arm.FK(home_position, xyz_home, ori_home)
arm.FK(home_position, xyz_out=xyz_home, orientation_out=rot_home)
"""
phone_name = "mobileIO"
print('Waiting for Mobile IO device to come online...')
m = create_mobile_io(lookup, arm_family, phone_name)
if m is None:
raise RuntimeError("Could not find Mobile IO device")
m.set_led_color("blue") # as we start in grav comp
m.clear_text() # Clear any garbage on screen
m.update()
"""
# Demo Variables
abort_flag = False
pending_goal = False
run_mode = "training"
goal = arm_api.Goal(arm.size)
# Printing Instructions
instructions = """B1 - Add waypoint (stop)
B2 - Add waypoint (stop) and toggle the gripper
B3 - Add waypoint (flow)
B5 - Toggle training/playback
B6 - Clear waypoints
A3 - Up/down for longer/shorter time to waypoint
B8 - Quit
"""
print(instructions)
#######################
## Main Control Loop ##
#######################
