x = rand[0]*300
y = rand[1]*600
y=y-300
z_const = 100
print("aimed positions are",x,y,z_const)
arm.set_position(x= x,y= y,z=z_const,speed=4000,wait=True,timeout=100)
a = arm.get_is_moving()
print("the status is", a, "\n")
pos = arm.get_position() # float array of the format [x, y, z] of the robots current location
print("x values " , pos[0])
print("y values" , pos[1])
print("z values", pos [2])
if(pos[2]!= z_const):
print("invalid data height misconception")
sys.stdout.write('/a')
sys.stdout.flush()
sleep(2.0)
continue
else:
print("valid data with desired height")
servo_angles=arm.get_servo_angle()
print("angles all", servo_angles)
servo_angles=servo_angles[0:3]
print("angles desired", servo_angles)
print("the position is ", pos, "\n")
position_arr.append(pos)
sleep(2.0)
angles_arr.append(servo_angles)
sleep(2)
print('\nset X350 Y0 Z50 F500 ...')
swift.set_position(230, 0, 70, speed=1500, wait=True)
# 下降到桌面
swift.set_position(230, 0, z=1, speed=1500, wait=True)
# 气泵
swift.set_pump(on=True)
#升起
# swift.set_position(230, 0, 70, speed=1500,wait=True)
swift.set_position(230, 0, 2, speed=1500, wait=True) #原地旋转
#旋转90度
print('角度：', swift.get_servo_angle()) #[90.09, 74.76, 39.27, 93.0]
print('旋转90度')
ret = swift.set_wrist(angle=90 - 90, wait=True) #默认是90度
print('角度：', swift.get_servo_angle(), ret) #[90.09, 74.94, 39.01, 104.0]
sleep(1)
ret = swift.set_wrist(angle=90 - 60, wait=True) #
print('角度：', swift.get_servo_angle(), ret) #[90.09, 75.03, 38.92, 15.0]
sleep(1)
ret = swift.set_wrist(angle=90 - 30, wait=True) #
print('角度：', swift.get_servo_angle(), ret) #[90.09, 75.03, 38.92, 52.0]
sleep(1)
ret = swift.set_wrist(angle=90 + 0, wait=True) #
print('角度：', swift.get_servo_angle(), ret) #[90.09, 75.03, 38.92, 65.0]
sleep(1)
ret = swift.set_wrist(angle=90 + 30, wait=True) #
print('角度：', swift.get_servo_angle(), ret) #[90.09, 75.03, 38.92, 94.0]
