def GetAndThrowiT(pos):
if (pos == "h"):
arm.SetPosition(-1, -1, -1, -1, 85, -1)
WaitForMove()
else:
pos = arm.GetArmPosition()
arm.SetPosition(-1, float(pos[1]) + 5, -1, -1, 160, -1)
WaitForMove()
arm.SetPosition(-1, -1, -1, -1, 160, -1)
WaitForMove()
arm.SetPosition(-1, float(pos[1]), -1, -1, 160, -1)
WaitForMove()
if (arm.GetTriggerStatus != "1"):
arm.SetPosition(-1, float(pos[1]) - 2, -1, -1, 160, -1)
WaitForMove()
arm.Gripper(90)
WaitForMove()
arm.MovingSpeed(2)
arm.SetPosition(-1, 80, -1, -1, -1, -1)
WaitForMove()
arm.SetPosition(6, 50, 80, 27, 85, -1)
WaitForMove()
arm.Gripper(20)
WaitForMove()
arm.SetPosition(lastpos[0], lastpos[1], lastpos[2], lastpos[3], lastpos[4],
lastpos[5])
arm.MovingSpeed(5)
def FirstGum1():
arm.SetPosition(91,43.3,99.5,16,170,22)
WaitForMove()
arm.SetPosition(92.66,45.96,105.58,9.49,12.91,-1)
WaitForMove()
arm.Gripper(85)
WaitForMove()
arm.SetPosition(91,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,64.81,77,40,93,-1)
WaitForMove()
arm.Gripper(34)
WaitForMove()
def SecondGum1():
time.sleep(0.5)
WaitForMove()
arm.SetPosition(91,43.3,99.5,16,170,22)
WaitForMove()
arm.SetPosition(91.43,41,99.5,15.24,171.14,-1)
WaitForMove()
arm.Gripper(85)
WaitForMove()
arm.SetPosition(91,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,64.81,77,40,93,-1)
WaitForMove()
arm.Gripper(34)
def FirstGum2():
time.sleep(0.5)
WaitForMove()
arm.SetPosition(90.19,77.97,65.9,65.64,93.34,30)
WaitForMove()
arm.SetPosition(90.88,28.56,30.45,63.08,177.49,35)
WaitForMove()
arm.Gripper(85)
WaitForMove()
arm.SetPosition(90.88,35.22,30.85,63.08,177.49,-1)
WaitForMove()
arm.SetPosition(90.67,38.51,29.02,65.84,171.6,-1)
WaitForMove()
arm.SetPosition(91,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,64.81,77,40,93,-1)
WaitForMove()
arm.Gripper(34)
def Secondtic():
WaitForMove()
arm.SetPosition(91.19,77.97,65.9,65.64,93.34,32)
WaitForMove()
arm.SetPosition(91.62,42.35,48.6,63.81,171.11,32)
WaitForMove()
arm.SetPosition(91.62,36.08,63.07,35.64,171.11,32)
WaitForMove()
arm.Gripper(85)
WaitForMove()
arm.SetPosition(92.62,36.48,57.99,46.59,171.11,-1)
time.sleep(1)
WaitForMove()
arm.SetPosition(91.62,42.35,48.6,63.81,171.11,-1)
WaitForMove()
arm.SetPosition(91,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,64.81,77,40,93,-1)
WaitForMove()
arm.Gripper(34)
def throw():
WaitForMove()
arm.Gripper(80)
WaitForMove()
arm.MovingSpeed(4)
thro = [4, 46, 118, 24]
WaitForMove()
arm.SetPosition(4, 69, 66, 55, -1, -1)
WaitForMove()
arm.SetPosition(thro[0], thro[1], thro[2], thro[3], -1, -1)
WaitForMove()
arm.SetPosition(-1, -1, -1, -1, 85, 20)
WaitForMove()
arm.MovingSpeed(5)
WaitForMove()
arm.SetPosition(-1, f_pos[0], -1, -1, -1, -1)
WaitForMove()
time.sleep(0.500)
trigger = arm.GetTriggerStatus()
if (trigger == "0"):
arm.SetPosition(-1, -1, -1, f_pos[2] + 10, -1, -1)
WaitForMove()
time.sleep(0.500)
trigger = arm.GetTriggerStatus()
if (trigger == "0"):
arm.SetPosition(-1, -1, -1, f_pos[2] - 10, -1, -1)
WaitForMove()
time.sleep(0.500)
arm.Gripper(70)
WaitForMove()
else:
arm.Gripper(70)
WaitForMove()
else:
act_pos = arm.GetArmPosition()
arm.SetPosition(-1, float(act_pos[1]) - 3, -1, -1, -1, -1)
WaitForMove()
arm.Gripper(70)
WaitForMove()
arm.MovingSpeed(3)
if (id == "alpha"):
arm.SetPosition(-1, 70, -1, -1, -1, -1)
WaitForMove()