rest_angles = (30, 30, 30)
rest_position = robot.forward_k(30, 30, 30)
center_angles = (40, 40, 40)
center_position = robot.forward_k(40, 40, 40)
top_angles1 = robot.inverse_k(0, rest_position[2] + 100, rest_position[3])
top_angles2 = robot.inverse_k(0, center_position[2] + 100, center_position[3])
bottom_angles1 = robot.inverse_k(0, rest_position[2] - 100, rest_position[3])
bottom_angles2 = robot.inverse_k(0, center_position[2] - 100,
center_position[3])
right_angles1 = robot.inverse_k(rest_position[2] + 100, 0, rest_position[3])
right_angles2 = robot.inverse_k(center_position[2] + 100, 0,
center_position[3])
left_angles1 = robot.inverse_k(rest_position[2] - 100, 0, rest_position[3])
left_angles2 = robot.inverse_k(center_position[2] - 100, 0, center_position[3])
robot.set_position(rest_angles)
sleep(.5)
print 'Center'
robot.set_position(center_angles)
sleep(.5)
robot.set_position(rest_angles)
sleep(2)
print 'Top'
robot.set_position(top_angles1)
sleep(.5)
robot.set_position(top_angles2)
sleep(.5)
robot.set_position(rest_angles)
sleep(2)
origin_point = bot.forward_k(starting_position, starting_position, starting_position)
if origin_point[0] is not 0:
raise Exception('Could not detect origin point')
origin_point = (origin_point[1], origin_point[2], origin_point[3])
# add a mapping to the screen center
mappings.append((screen_center, (bot.a.angle, bot.b.angle, bot.c.angle)))
# return the device to the original position
bot.move(starting_position,starting_position,starting_position)
# touch to the left
left50pt1 = bot.inverse_k(contact_point[0]-50, 0, origin_point[2])
left50pt2 = bot.inverse_k(contact_point[0]-50, 0, contact_point[2])
bot.set_position(left50pt1)
sleep(1.0)
bot.set_position(left50pt2)
left_coords = get_coords()
left_position = bot.position()
left_point = bot.forward_k(left_position[0], left_position[1], left_position[2])
if left_point[0] is not 0:
raise Exception('Could not detect origin point')
left_point = (left_point[1], left_point[2], left_point[3])
mappings.append((left_coords, left_point))
# touch to the right
right50pt1 = bot.inverse_k(contact_point[0]+50, 0, origin_point[2])
right50pt2 = bot.inverse_k(contact_point[0]+50, 0, contact_point[2])
bot.set_position(right50pt1)
sleep(1.0)
robot = Bot('/dev/tty.usbmodem5d11')
rest_angles = (30,30,30)
rest_position = robot.forward_k(30,30,30)
center_angles = (40,40,40)
center_position = robot.forward_k(40,40,40)
top_angles1 = robot.inverse_k(0, rest_position[2]+100, rest_position[3])
top_angles2 = robot.inverse_k(0, center_position[2]+100, center_position[3])
bottom_angles1 = robot.inverse_k(0, rest_position[2]-100, rest_position[3])
bottom_angles2 = robot.inverse_k(0, center_position[2]-100, center_position[3])
right_angles1 = robot.inverse_k(rest_position[2]+100, 0, rest_position[3])
right_angles2 = robot.inverse_k(center_position[2]+100, 0, center_position[3])
left_angles1 = robot.inverse_k(rest_position[2]-100, 0, rest_position[3])
left_angles2 = robot.inverse_k(center_position[2]-100, 0, center_position[3])
robot.set_position(rest_angles)
sleep(.5)
print 'Center'
robot.set_position(center_angles)
sleep(.5)
robot.set_position(rest_angles)
sleep(2)
print 'Top'
robot.set_position(top_angles1)
sleep(.5)
robot.set_position(top_angles2)
sleep(.5)
robot.set_position(rest_angles)
sleep(2)
origin_point = bot.forward_k(starting_position, starting_position,
starting_position)
if origin_point[0] is not 0:
raise Exception('Could not detect origin point')
origin_point = (origin_point[1], origin_point[2], origin_point[3])
# add a mapping to the screen center
mappings.append((screen_center, (bot.a.angle, bot.b.angle, bot.c.angle)))
# return the device to the original position
bot.move(starting_position, starting_position, starting_position)
# touch to the left
left50pt1 = bot.inverse_k(contact_point[0] - 50, 0, origin_point[2])
left50pt2 = bot.inverse_k(contact_point[0] - 50, 0, contact_point[2])
bot.set_position(left50pt1)
sleep(1.0)
bot.set_position(left50pt2)
left_coords = get_coords()
left_position = bot.position()
left_point = bot.forward_k(left_position[0], left_position[1],
left_position[2])
if left_point[0] is not 0:
raise Exception('Could not detect origin point')
left_point = (left_point[1], left_point[2], left_point[3])
mappings.append((left_coords, left_point))
# touch to the right
right50pt1 = bot.inverse_k(contact_point[0] + 50, 0, origin_point[2])
right50pt2 = bot.inverse_k(contact_point[0] + 50, 0, contact_point[2])
bot.set_position(right50pt1)
