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)