def moveRandom():
SERVO_BOTTOM = 0
SERVO_LEFT = 1
SERVO_RIGHT = 2
SERVO_HAND = 3
position_arr = []
pixel_position = []
angles_arr = []
arm = SwiftAPI()
sleep(2.0)
rand = np.random.rand(3)
angles = rand * 180
print("angles are", angles)
arm.flush_cmd()
arm.reset()
arm.set_servo_angle_speed(SERVO_RIGHT,
angles[0],
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_BOTTOM,
angles[1],
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_LEFT,
angles[2],
wait=True,
timeout=100,
speed=5000)
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
cap = cv2.VideoCapture(0)
cap1 = cv2.VideoCapture(0)
red = detect_w_avg(cap, 600, 166, 84, 80, 286, 255, 255, "red")
print("red", red)
blue = detect_w_avg(cap1, 600, 97, 100, 117, 117, 255, 255, "blue")
print("blue", blue)
pixel_position.append(red)
pixel_position.append(blue)
cv2.destroyAllWindows()
cap.release()
cap1.release()
# pixelpostion= get the pixel position here
print("the position is ", pos, "\n")
position_arr.append(pos)
angles = angles.tolist()
angles_arr.append(angles) # [right bottom left]
sleep(2.0)
return angles, pixel_position
def moveRandom():
SERVO_BOTTOM = 0
SERVO_LEFT = 1
SERVO_RIGHT = 2
SERVO_HAND = 3
position_arr=[]
angles_arr=[]
arm = SwiftAPI()
sleep(2.0)
rand = np.random.rand(3)
angles = rand * 180
print("angles are", angles)
arm.flush_cmd()
arm.reset()
arm.set_servo_angle_speed(SERVO_RIGHT, angles[0], wait=True, timeout=100, speed=5000)
arm.set_servo_angle_speed(SERVO_BOTTOM, angles[1], wait=True, timeout=100, speed=5000)
arm.set_servo_angle_speed(SERVO_LEFT, angles[2], wait=True, timeout=100, speed=5000)
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
# pixelpostion= get the pixel position here
print("the position is ", pos, "\n")
position_arr.append(pos)
angles = angles.tolist()
angles_arr.append(angles) # [right bottom left]
sleep(2.0)
def main():
print('setup swift ...')
swift = SwiftAPI()
print('sleep 2 sec ...')
sleep(2)
print('device info: ')
print(swift.get_device_info())
movements = [
{
'x': 180,
'y': 180,
'z': 30
},
{
'x': 180,
'y': -180,
'z': 30
},
{
'x': 250,
'y': 0,
'z': 150
},
{
'x': 150,
'y': 0,
'z': 30
},
]
for movement in movements:
print('\nset ' + ' '.join(
["%s%d" % (key.upper(), value)
for key, value in movement.items()]))
swift.set_position(wait=True, **movement)
sleep(0.1)
while swift.get_is_moving():
sleep(0.1)
swift.set_buzzer()
print('done ...')
while True:
sleep(1)
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_BOTTOM,
angles[1],
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_LEFT,
angles[2],
wait=True,
timeout=100,
speed=5000)
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
# pixelpostion= get the pixel position here
print("the position is ", pos, "\n")
position_arr.append(pos)
angles = angles.tolist()
angles_arr.append(angles) # [right bottom left]
sleep(2.0)
DAT = np.row_stack((position_arr, angles_arr))
print("try final ", DAT)
swift.set_position(350, 0, 50, speed=1500)
print('set X340 ...')
swift.set_position(x=340)
print('set X320 ...')
swift.set_position(x=320)
print('set X300 ...')
swift.set_position(x=300)
print('set X200 ...')
swift.set_position(x=200)
print('set X190 ...')
swift.set_position(x=190)
# make sure at least one position was sent to device before start checking
sleep(0.1)
# make sure corresponding topics was empty before start service call
# data through different topics and services does not guarantee the order
while swift.get_is_moving():
sleep(0.1)
print('set Z100 ...')
swift.set_position(z=100, wait=True)
print('set Z150 ...')
swift.set_position(z=150, wait=True)
swift.set_buzzer()
print('done ...')
while True:
sleep(1)
