166 lines
5.7 KiB
Python
166 lines
5.7 KiB
Python
import queue
|
||
import threading
|
||
import requests
|
||
import Robot
|
||
import time
|
||
|
||
|
||
# todo: 1. 添加软限位
|
||
# todo: 2. 添加回零点功能(完成)
|
||
# todo: 3. 将抓取流程封装成一个函数(完成)
|
||
# todo: 4. 将物品移至指定位置(完成)
|
||
|
||
class FRRobot:
|
||
|
||
def __init__(self):
|
||
# 机械臂相关
|
||
self.robotCTL = Robot.RPC('192.168.3.102')
|
||
self.speed = 100 # 速度百分比,[0~100]默认20
|
||
self.home = [] # 回零点
|
||
self.tool = 1 # 工具坐标系编号
|
||
self.user = 1 # 工件坐标系编号
|
||
# 100% 60mm/s; 50% 30mm/s; 20% 12mm/s
|
||
self.velocity = 60 # 速度(mm/s)
|
||
self.is_arm_moving = False # 是否正在执行动作
|
||
# 线程相关
|
||
self.command_queue = queue.Queue()
|
||
self.listener_thread = threading.Thread(target=self.process_commands)
|
||
self.listener_thread.daemon = True # 设置为守护线程,确保主程序退出时线程也会退出
|
||
self.listener_thread.start()
|
||
# 夹爪相关
|
||
self.is_open = False # 夹爪是否打开
|
||
self.catch_moving = False # 是否在执行抓取动作
|
||
|
||
speed_error_code = self.robotCTL.SetSpeed(self.speed)
|
||
if speed_error_code != 0:
|
||
print("初始化全局速度错误:", speed_error_code)
|
||
|
||
# 将移动命令添加到队列中
|
||
def add_move_command(self, command):
|
||
self.command_queue.put(command)
|
||
|
||
def add_catch_commands(self, command):
|
||
self.command_queue.put(command)
|
||
|
||
def process_commands(self):
|
||
while True:
|
||
move_commands = self.command_queue.get()
|
||
if move_commands is None:
|
||
continue
|
||
print("接收到指令: ", move_commands)
|
||
if move_commands[0] == "catch":
|
||
self.catch(move_commands[1])
|
||
continue
|
||
if move_commands == "home":
|
||
self.run_script("/fruser/Home.lua")
|
||
continue
|
||
if move_commands == "stop":
|
||
self.robotCTL.ImmStopJOG()
|
||
break
|
||
# 停止线程
|
||
# axis, distance, direction = move_commands
|
||
|
||
# 机械臂移动
|
||
# todo:这地方写的不好,其实不用for,直接和上面一样判断就行
|
||
for command in move_commands:
|
||
self.is_arm_moving = True
|
||
axis, distance = command
|
||
if axis == "x":
|
||
print("x移动距离: ", distance)
|
||
direction = 1 if distance > 0 else 0
|
||
self.move_x(distance, direction)
|
||
|
||
elif axis == "y":
|
||
print("y移动距离: ", distance)
|
||
direction = 1 if distance < 0 else 0
|
||
self.move_y(distance, direction)
|
||
|
||
self.command_queue.task_done()
|
||
|
||
def move_x(self, distance=30, direction=1):
|
||
# direction为0代表负方向,为1代表正方向
|
||
move_time = abs(distance) / self.velocity
|
||
self.robotCTL.StartJOG(ref=4, nb=1, dir=direction, max_dis=100, vel=20.0, acc=100.0)
|
||
time.sleep(move_time)
|
||
print("x移动时间: ", move_time)
|
||
self.robotCTL.ImmStopJOG()
|
||
self.is_arm_moving = False
|
||
|
||
def move_y(self, distance=30, direction=1):
|
||
move_time = abs(distance) / self.velocity
|
||
self.robotCTL.StartJOG(ref=4, nb=2, dir=direction, max_dis=100, vel=20.0, acc=100.0)
|
||
time.sleep(move_time)
|
||
print("y移动时间: ", move_time)
|
||
self.robotCTL.ImmStopJOG()
|
||
self.is_arm_moving = False
|
||
|
||
def move_z(self, distance=30, direction=1):
|
||
move_time = (abs(distance - 100)) / self.velocity
|
||
self.robotCTL.StartJOG(ref=4, nb=3, dir=direction, max_dis=300, vel=20.0, acc=100.0)
|
||
time.sleep(move_time)
|
||
self.robotCTL.ImmStopJOG()
|
||
|
||
def run_script(self, script_name):
|
||
self.is_arm_moving = True
|
||
# 0:自动模式,1:手动模式
|
||
self.robotCTL.Mode(0)
|
||
self.robotCTL.ProgramLoad(script_name)
|
||
self.robotCTL.ProgramRun()
|
||
while self.robotCTL.GetProgramState() == 2:
|
||
time.sleep(0.1)
|
||
|
||
self.is_arm_moving = False
|
||
self.robotCTL.Mode(1)
|
||
|
||
def catch(self, distance):
|
||
if not self.is_open:
|
||
self.catch_moving = True
|
||
self.open_hand()
|
||
# 1向上,0向下
|
||
direction = 1 if distance < 0 else 0
|
||
self.is_arm_moving = True
|
||
self.move_z(distance, direction)
|
||
self.close_hand()
|
||
# 抬起物品
|
||
self.move_z(distance, 1)
|
||
self.run_script("/fruser/home2.lua")
|
||
time.sleep(0.5)
|
||
self.open_hand()
|
||
self.run_script("/fruser/Home.lua")
|
||
self.close_hand()
|
||
self.catch_moving = False
|
||
self.is_arm_moving = False
|
||
|
||
# 夹爪控制
|
||
def open_hand(self):
|
||
params = {
|
||
'direction': 'cw',
|
||
'speed': 2000
|
||
}
|
||
|
||
response = requests.get('http://192.168.3.200/motor', params=params)
|
||
|
||
print(response.text) # 打印响应内容
|
||
print(response.status_code) # 打印响应状态码
|
||
time.sleep(1)
|
||
if response.status_code == 200:
|
||
self.is_open = True
|
||
|
||
def close_hand(self):
|
||
params = {
|
||
'direction': 'ccw',
|
||
'speed': 2000
|
||
}
|
||
|
||
response = requests.get('http://192.168.3.200/motor', params=params)
|
||
|
||
print(response.text) # 打印响应内容
|
||
print(response.status_code) # 打印响应状态码
|
||
time.sleep(1)
|
||
if response.status_code == 200:
|
||
self.is_open = False
|
||
|
||
# if __name__ == '__main__':
|
||
# robot_controller = FRRobot()
|
||
# robot_controller.run_script("/fruser/home2.lua")
|