class set_arm_control():
def __init__(self,RELAY_PIN=12):
self.arm = SwiftAPI()
self.arm.get_power_status()
self.RELAY_PIN = RELAY_PIN
self.arm.reset()
#機械手臂進行初始化動作
def set_arm_origin(self):
self.arm.waiting_ready()
self.arm.set_position(200, 0, 170, speed=100000)
# 機械手臂設定(x, y, z)座標移動
def set_arm_move(self, x, y, z):
self.arm.set_position(x, y, z, speed=100000)
# 機械手臂切斷連線
def arm_disconnect(self):
self.arm.disconnect()
def get_position(self):
print(self.arm.get_position())
#初始設定繼電器
def origin_pump(self):
GPIO.setmode(GPIO.BOARD) # 指定模式BOARD 啟用RPi板子相對應之腳位編號
GPIO.setwarnings(False) # 避免出現警告
GPIO.setup(self.RELAY_PIN, GPIO.OUT) # 設定pin腳為輸出
#打開幫浦=開啟繼電器(使電流流通)
def start_pump(self):
GPIO.output(self.RELAY_PIN, 0)
#關閉幫浦=關閉繼電器(使電流不能流通)
def close_pump(self):
GPIO.output(self.RELAY_PIN, 1)
from uarm.wrapper import SwiftAPI
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift.waiting_ready(timeout=3)
device_info = swift.get_device_info()
print(device_info)
firmware_version = device_info['firmware_version']
if firmware_version and not firmware_version.startswith(
('0.', '1.', '2.', '3.')):
swift.set_speed_factor(0.0005)
swift.set_mode(0)
swift.reset(wait=True, speed=10000)
swift.set_position(x=200, speed=10000 * 20)
swift.set_position(y=100)
swift.set_position(z=100)
swift.flush_cmd(wait_stop=True)
swift.set_polar(stretch=200, speed=10000 * 20)
swift.set_polar(rotation=90)
swift.set_polar(height=150)
print(swift.set_polar(stretch=200, rotation=90, height=150, wait=True))
swift.flush_cmd()
# time.sleep(1)
# print('mv 1')
# swift.set_position(x=0, y=0, z=0, speed=10000*20)
device_info = swift.get_device_info()
print(swift.port, device_info)
firmware_version = device_info['firmware_version']
if firmware_version and not firmware_version.startswith(('0.', '1.', '2.', '3.')):
swift.set_speed_factor(0.00001)
swift.set_servo_detach()
input('A4')
swift.set_servo_attach()
pos = swift.get_position()
print(pos);
bias = 23;
swift.reset(speed=10000000);
seqs = ["F", "G", "E", "A", "D", "G", "C", "C"]
# seqs = ["C", "C", "D", "D", "E", "E", "D", "D"]
count = 0;
while True:
count = count + 1
seq = seqs[count % len(seqs)]
print(seq);
print(time.time())
# move
swift.set_position(x=pos[X], y=pos[Y] + (-d[seq] * bias), z=pos[Z] + 5, speed=10000000)
# push
from uarm.wrapper import SwiftAPI
sio = socketio.AsyncClient(reconnection=True)
try:
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'},
callback_thread_pool_size=2)
swift.waiting_ready(timeout=10)
device_info = swift.get_device_info()
print(device_info)
# swift.set_speed_factor(100)
swift.set_mode(3)
swift.reset(wait=True, speed=250)
# firmware_version = device_info['firmware_version']
# if firmware_version and not firmware_version.startswith(('0.', '1.', '2.', '3.')):
# # swift.set_speed_factor(0.0005)
except Exception:
swift = False
print('swift uArm not connected')
@sio.event
async def connect():
print('connected to server')
@sio.event
import os
import sys
import time
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
from uarm.wrapper import SwiftAPI
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift.waiting_ready(timeout=3)
device_info = swift.get_device_info()
print(device_info)
firmware_version = device_info['firmware_version']
#if firmware_version and not firmware_version.startswith(('0.', '1.', '2.', '3.')):
# swift.set_speed_factor(0.0005)
#set for general mode
swift.set_mode(0)
'''
swift.reset(wait=True, speed=100000)
swift.set_position(x=200, speed=100000)
swift.set_position(y=100)
swift.set_position(z=100)
swift.flush_cmd(wait_stop=True)
'''
'''
swift.set_polar(stretch=150, speed=100000)
swift.set_polar(rotation=0)
swift.set_polar(height=80)
print(swift.set_polar(stretch=150, rotation=0, height=80, wait=True))
swift.flush_cmd()
'''
'''
swift.set_polar(stretch=150, rotation=90, height=150, speed=100000, wait=True)
class UArm_SDK(object):
def __init__(self):
'''
connect to UArm
'''
self.swift = SwiftAPI()
self.swift.connect()
self.swift.get_power_status()
print(self.swift.get_device_info())
self.swift.reset(wait=True) # back to home position
print('init complete')
self.gripper_temp = 0 # keep track of gripper state
def __del__(self):
'''
disconnect UArm
'''
self.swift.disconnect()
print('uarm disconnected')
def set_servo_angle(self, joint_angles, dt):
'''
set servo angle via SDK
input:
joint_angles, 5-vector: [theta1, theta2, theta3, theta4, pump state] in degrees
dt, time step
'''
wait = True
self.swift.set_servo_angle(servo_id=0,
angle=joint_angles[0] + 90,
speed=5000,
wait=wait)
time.sleep(dt / 4)
self.swift.set_servo_angle(servo_id=1,
angle=joint_angles[1],
speed=5000,
wait=wait)
time.sleep(dt / 4)
self.swift.set_servo_angle(servo_id=2,
angle=joint_angles[2] - joint_angles[1],
speed=5000,
wait=wait)
time.sleep(dt / 4)
self.swift.set_servo_angle(servo_id=3,
angle=180 - joint_angles[3],
speed=5000,
wait=wait)
time.sleep(dt / 4)
if joint_angles[4] > 0:
self.swift.set_pump(on=True)
elif joint_angles[4] == 0:
self.swift.set_pump(on=False)
else:
print("ERROR")
def control_uarm_via_traj(self, position, wrist_angle, pump_state, dt):
'''
set end effector position, wrist angle and pump state via SDK
input:
position, 3-vector: [px, py, pz]
wrist_angle: wrist angle in rad
pump_state: bool, 0 - off, 1 - on
'''
px, py, pz = position[0], position[1], position[2]
# conver m to mm
px *= 1000
py *= 1000
pz *= 1000
# change end effector position
e = self.swift.set_position(x=px, y=py, z=pz, speed=100000, wait=True)
print(e)
# change wrist angle
self.swift.set_wrist(90 - wrist_angle * 180 / PI)
if self.gripper_temp == 0 and pump_state == 1:
# enable suction cup
self.swift.set_pump(on=True, wait=True)
print('pump on')
self.gripper_temp = 1
if self.gripper_temp == 1 and pump_state == 0:
# disable suction cup
self.swift.set_pump(on=False, wait=True)
print('pump off')
self.gripper_temp = 0
time.sleep(dt)
import sys
import time
import functools
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
from uarm.wrapper import SwiftAPI
from uarm.utils.log import logger
logger.setLevel(logger.DEBUG)
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
# swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_handle_thread=False)
# swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_write_thread=True)
# swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_report_thread=True)
# swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_handle_thread=True, enable_write_thread=True, enable_report_thread=True)
swift.reset(speed=50000, wait=True)
swift.set_position(x=250, y=0, z=10, speed=50000, wait=True)
swift.flush_cmd()
swift.set_pump(True)
swift.set_position(x=250, y=0, z=170, speed=50000, wait=True)
swift.flush_cmd()
swift.set_position(x=220, y=70, z=170, speed=50000, wait=True)
swift.flush_cmd()
swift.set_position(x=200, y=140, z=170, speed=50000, wait=True)
swift.flush_cmd()
swift.set_position(x=180, y=210, z=170, speed=50000, wait=True)
swift.flush_cmd()
swift.set_position(x=250, y=210, z=10, speed=50000, wait=True)
swift.flush_cmd()
swift.set_pump(False)
swift.flush_cmd()
swift.get_power_status()
print(swift.get_device_info())
T = 2
dt = 0.01 # best we can get: 0.01
N = int(T / dt)
#swift.set_servo_angle(servo_id=0, angle=90)
##time.sleep(3)
#swift.set_servo_angle(servo_id=1, angle=90)
##time.sleep(3)
#swift.set_servo_angle(servo_id=2, angle=0)
##time.sleep(3)
#swift.set_servo_angle(servo_id=3, angle=90)
swift.reset()
time.sleep(3)
swift.set_speed_factor(2)
'''generate configs'''
joint_angles = np.zeros((N, 5))
for i in range(N):
joint_angles[i, :] = np.array(
[90, 90 - 90 / T * dt * i, 90 / T * dt * i, 90, 0])
# step motor step?
for i in range(N):
swift.set_servo_angle(servo_id=0, angle=joint_angles[i, 0])
# time.sleep(dt/4)
swift.set_servo_angle(servo_id=1, angle=joint_angles[i, 1])
class Arm:
def __init__(self):
self.arm = SwiftAPI()
self.arm.reset()
sleep(2)
def rest(self):
self.arm.set_position(x=0,
y=180,
z=146,
wait=True,
timeout=10,
speed=10000)
print("moving to rest pos!")
def correction(self):
print("=================check start!=================\n")
self.arm.set_position(x=227, y=0, z=60, wait=True, speed=10000)
sleep(1)
self.arm.set_position(x=227,
y=0,
z=47,
wait=True,
timeout=10,
speed=10000)
print("check center\n")
sleep(1)
input()
self.arm.set_position(x=292, y=75, z=60, wait=True, speed=10000)
sleep(1)
self.arm.set_position(x=292,
y=75,
z=47,
wait=True,
timeout=10,
speed=10000)
print("check left-up\n")
sleep(1)
input()
self.arm.set_position(x=293, y=-75, z=60, wait=True, speed=10000)
sleep(1)
self.arm.set_position(x=293,
y=-75,
z=47,
wait=True,
timeout=10,
speed=10000)
print("check right-up\n")
sleep(1)
input()
self.arm.set_position(x=160, y=75, z=60, wait=True, speed=10000)
sleep(1)
self.arm.set_position(x=160,
y=75,
z=47,
wait=True,
timeout=10,
speed=10000)
print("check left-down\n")
sleep(1)
input()
self.arm.set_position(x=160, y=-75, z=60, wait=True, speed=10000)
sleep(1)
self.arm.set_position(x=160,
y=-75,
z=47,
wait=True,
timeout=10,
speed=10000)
print("check right-down\n")
sleep(1)
input()
self.arm.set_position(x=0,
y=180,
z=146,
wait=True,
timeout=10,
speed=10000)
print("=================check finish!=================\n")
def move(self, color, coords):
global count
count += 1
if coords == "pass" or coords == "resign":
return None
y, x = coords
y = int(ord(y) - ord('A'))
x = int(x) - 1
if count == 1:
x_take = 309
elif count == 2:
x_take = 280
elif count == 3:
x_take = 254
elif count == 4:
x_take = 223
elif count == 5:
x_take = 200
elif count == 6:
x_take = 170
elif count == 7:
x_take = 144
count = 0
if color == "B":
xp = x_take
yp = -145
zp = 100
elif color == "W":
xp = x_take
yp = 157
zp = 100
else:
print("Wrong color input!")
self.arm.set_position(x=xp, y=yp, z=zp, wait=True, speed=10000)
self.arm.flush_cmd(wait_stop=True)
self.arm.set_position(z=54, wait=True, speed=10000)
sleep(1.5)
self.arm.set_pump(True)
sleep(2)
self.arm.set_position(z=100, wait=True, speed=10000)
self.arm.set_position(x=294 - x * 22,
y=-66 + y * 24,
z=100,
wait=True,
speed=10000)
self.arm.flush_cmd(wait_stop=True)
print("moving to:", coords)
self.arm.set_position(z=54, wait=True,
speed=10000) # original z is -26
sleep(1)
self.arm.set_pump(False)
sleep(0.5)
self.arm.set_position(z=100, wait=True, speed=10000)
sleep(0.5)
self.arm.flush_cmd()
def remove(self, coords, remove_list):
self.arm.set_position(x=1, y=2, z=45, wait=True, speed=10000)
x, y = coords
remove_list
def remove_chess(self, color, coords):
y, x = coords
y = int(y) - 1
x = int(x) - 1
self.arm.set_position(x=294 - x * 22,
y=-66 + y * 24,
z=100,
wait=True,
speed=10000)
self.arm.flush_cmd(wait_stop=True)
self.arm.set_position(z=54, wait=True, speed=10000)
sleep(1.5)
self.arm.set_pump(True)
sleep(2)
print("removing :", coords)
self.arm.set_position(z=100, wait=True, speed=10000)
self.arm.set_position(x=342, y=0, z=90, wait=True, speed=10000)
self.arm.flush_cmd(wait_stop=True)
self.arm.set_pump(False)
sleep(0.5)
def take_photo(self):
self.arm.set_position(x=166,
y=4,
z=220,
wait=True,
timeout=10,
speed=10000)
print("Take Picture place!")
def game_over(self):
self.arm.set_position(x=0,
y=170,
z=100,
wait=True,
timeout=10,
speed=10000)
print("Game over!")
import os
import sys
import time
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
from uarm.wrapper import SwiftAPI
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift.waiting_ready(timeout=3)
device_info = swift.get_device_info()
print(device_info)
firmware_version = device_info['firmware_version']
if firmware_version and not firmware_version.startswith(
('0.', '1.', '2.', '3.')):
swift.set_speed_factor(0.0005)
swift.set_mode(0)
'''
swift.reset(wait=True, speed=100000)
swift.set_position(x=200, speed=100000)
swift.set_position(y=100)
swift.set_position(z=100)
swift.flush_cmd(wait_stop=True)
'''
#set_digital_output(pin=None, value=None, wait=True, timeout=None, callback=None)
swift.set_digital_output(pin=50, value=1) # red led
swift.set_digital_output(pin=49, value=0) # green led
swift.flush_cmd()
i = 0
while i < 5:
swift.set_polar(stretch=150, speed=100000)
class MinionArm:
def __init__(self):
self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
self.swift.waiting_ready()
device_info = self.swift.get_device_info()
print(device_info)
firmware_version = device_info['firmware_version']
if firmware_version and not firmware_version.startswith(
('0.', '1.', '2.', '3.')):
self.swift.set_speed_factor(0.0005)
self.speed = 110000
self.test()
self.board = "right"
def test(self):
# self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
self.swift.reset(wait=True, speed=self.speed)
self.move(1250, 0, 150, self.speed)
print(self.swift.get_position())
# self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
def home(self):
print("going home")
self.swift.set_position(x=150, y=0, speed=self.speed * 0.5, wait=True)
self.move(145, 0, 100, self.speed * 1.5)
time.sleep(2)
# self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
def rest(self):
print("going to rest position")
self.swift.set_position(x=150, speed=self.speed * 0.5, wait=True)
self.swift.set_position(x=125, y=0, speed=self.speed * 0.5, wait=True)
self.move(125, 0, 40, self.speed * 0.5)
time.sleep(2)
self.swift.set_buzzer(frequency=1000, duration=0.5)
def move(self, x, y, z, speed):
self.swift.set_position(x, y, z, speed, wait=True)
def flip(self):
print("flipping")
# self.home()
self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
if self.board == "right":
self.flip_left()
self.board = "left"
else:
self.flip_right()
self.board = "right"
self.home()
self.swift.set_buzzer(frequency=500, duration=1.0, wait=True)
def flip_left(self):
self.move(180, 0, 150, self.speed)
self.move(240, 0, 150, self.speed)
# self.move(220, 10, 140, self.speed*.2)
self.move(220, 40, 150, self.speed * .2)
# self.move(220, 40, 110, self.speed*.2)
# self.move(220, 60, 100, self.speed*.2)
self.move(220, 50, 30, self.speed * .2)
time.sleep(1)
self.move(180, 50, 20, self.speed * .2)
time.sleep(2)
def flip_right(self):
self.move(180, 0, 150, self.speed)
self.move(240, -10, 150, self.speed)
# self.move(220, -20, 140, self.speed*.2)
self.move(220, -40, 150, self.speed * .2)
# self.move(220, -50, 110, self.speed*.2)
# self.move(220, -60, 100, self.speed*.2)
self.move(220, -60, 20, self.speed * .2)
self.move(180, -50, 20, self.speed * .2)
time.sleep(2)
def move_to_galton_cal_pos(self):
self.move(180, 0, 150, self.speed)
self.move(240, 0, 150, self.speed)
wait = input("waiting")
self.home()
def disconnect(self):
self.swift.disconnect()
