def cup_appeared(self, coords):
if self.cup:
return
else:
self.cup = True
angles = get_angles(coords[0]-10, -coords[1]+50)
self.l.setAngles(*angles) # shouldn't there be a time here as well?
self.sleep(1)
self.nxt.grasp()
self.nxt.moveUp(1000)
self.moveOnTray()
self.sleep(0)
def moveOnTray(self):
# Positions for cups, in the order to avoid collisions
positions = [(-20, -270),
(80, -200) # TODO: test
]
cup_pos = positions[self.cups_got]
a, b, c = get_angles(*cup_pos)
self.l.setAngles(*self.init_angles)
self.sleep(2)
self.l.setAngles(a, b, c, time=2)
self.sleep(2)
self.nxt.moveDown(3000)
self.nxt.release()
self.nxt.moveUp(2000)
self.cup_released()
