graduation-design/robot_control/FRRobot.py
2024-04-25 20:10:52 +08:00

166 lines
5.7 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

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")