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)
bot.set_position(right50pt2)
right_coords = get_coords()
right_position = bot.position()
right_point = bot.forward_k(right_position[0], right_position[1], right_position[2])
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)
bot.set_position(right50pt2)
right_coords = get_coords()
right_position = bot.position()
