From 887a690292e7472ddf0b9d0bfbfef8f25e1d865c Mon Sep 17 00:00:00 2001 From: weijian Date: Tue, 9 May 2023 09:31:39 +0800 Subject: [PATCH] add 320 gripper slider control --- mycobot_320/new_mycobot_320/CMakeLists.txt | 1 + .../launch/mycobot_320_gripper_slider.launch | 31 +++++++++ .../scripts/mycobot_320_gripper_slider.py | 56 +++++++++++++++ mycobot_320/new_mycobot_320/scripts/test.py | 69 ++++++++++++++----- 4 files changed, 139 insertions(+), 18 deletions(-) create mode 100644 mycobot_320/new_mycobot_320/launch/mycobot_320_gripper_slider.launch create mode 100755 mycobot_320/new_mycobot_320/scripts/mycobot_320_gripper_slider.py diff --git a/mycobot_320/new_mycobot_320/CMakeLists.txt b/mycobot_320/new_mycobot_320/CMakeLists.txt index 73640da..4b1e0a0 100644 --- a/mycobot_320/new_mycobot_320/CMakeLists.txt +++ b/mycobot_320/new_mycobot_320/CMakeLists.txt @@ -66,6 +66,7 @@ include_directories(include${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) catkin_install_python(PROGRAMS scripts/mycobot_320_follow_display.py scripts/mycobot_320_slider.py + scripts/mycobot_320_gripper_slider.py scripts/mycobot_320_teleop_keyboard.py scripts/mycobot_320_listen_real.py scripts/mycobot_320_listen_real_of_topic.py diff --git a/mycobot_320/new_mycobot_320/launch/mycobot_320_gripper_slider.launch b/mycobot_320/new_mycobot_320/launch/mycobot_320_gripper_slider.launch new file mode 100644 index 0000000..1dc7b33 --- /dev/null +++ b/mycobot_320/new_mycobot_320/launch/mycobot_320_gripper_slider.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_320/new_mycobot_320/scripts/mycobot_320_gripper_slider.py b/mycobot_320/new_mycobot_320/scripts/mycobot_320_gripper_slider.py new file mode 100755 index 0000000..b3a3110 --- /dev/null +++ b/mycobot_320/new_mycobot_320/scripts/mycobot_320_gripper_slider.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +"""[summary] +This file obtains the joint angle of the manipulator in ROS, +and then sends it directly to the real manipulator using `pymycobot` API. +This file is [slider_control.launch] related script. +Passable parameters: + port: serial prot string. Defaults is '/dev/ttyUSB0' + baud: serial prot baudrate. Defaults is 115200. +""" + +import rospy +import time +from sensor_msgs.msg import JointState + +from pymycobot.mycobot import MyCobot + + +mc = None +gripper_value = [] + +def callback(data): + global mc + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + data_list = [] + for index, value in enumerate(data.position): + data_list.append(round(value, 3)) + + data_list = data_list[:7] + print("radians:%s"%data_list[:6]) + mc.send_radians(data_list[:6], 80) + gripper_value = int(abs(-0.7-data_list[6])* 117) + print("gripper_value:%s"%gripper_value) + # mc.set_gripper_mode(0) + # time.sleep(1) + mc.set_gripper_value(gripper_value, 100) + +def listener(): + global mc + global gripper_value + + rospy.init_node("control_slider", anonymous=True) + + rospy.Subscriber("joint_states", JointState, callback) + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + print(port, baud) + mc = MyCobot(port, baud) + + # spin() simply keeps python from exiting until this node is stopped + print("spin ...") + rospy.spin() + + +if __name__ == "__main__": + listener() \ No newline at end of file diff --git a/mycobot_320/new_mycobot_320/scripts/test.py b/mycobot_320/new_mycobot_320/scripts/test.py index 6d945d9..6f9a7d3 100755 --- a/mycobot_320/new_mycobot_320/scripts/test.py +++ b/mycobot_320/new_mycobot_320/scripts/test.py @@ -4,36 +4,36 @@ from pymycobot.genre import Angle from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 import time -mc = MyCobot("/dev/ttyUSB0", 115200) +mc = MyCobot("/dev/ttyACM0", 115200) # 通过传递角度参数，让机械臂每个关节移动到对应[0, 0, 0, 0, 0, 0]的位置 mc.send_angles([0, 0, 0, 0, 0, 0], 50) # 设置等待时间，确保机械臂已经到达指定位置 -time.sleep(2.5) +# time.sleep(2.5) # 让关节1移动到90这个位置 # mc.send_angle(Angle.J1.value, 90, 50) # 设置等待时间，确保机械臂已经到达指定位置 -mc.send_coords([73.2, -85.9, 495.7, -94.26, -30.17, -84.42],20,0) -time.sleep(2) +# mc.send_coords([73.2, -85.9, 495.7, -94.26, -30.17, -84.42],20,0) +# time.sleep(2) -mc.send_angles([2.19, 0.35, -3.42, -0.61, 1.23, -30.32],20) +# mc.send_angles([2.19, 0.35, -3.42, -0.61, 1.23, -30.32],20) -num=0 -while num<5: - print('-------------------->1') - print(mc.get_angles()) - time.sleep(1) - print('--------------------->2') - print(mc.get_coords()) - print('----------------->3') - time.sleep(1) - print(mc.get_radians()) +# num=0 +# while num<5: +# print('-------------------->1') +# print(mc.get_angles()) +# time.sleep(1) +# print('--------------------->2') +# print(mc.get_coords()) +# print('----------------->3') +# time.sleep(1) +# print(mc.get_radians()) - num+=1 +# num+=1 -time.sleep(2) +# time.sleep(2) # # 以下代码可以让机械臂左右摇摆 # # 设置循环次数 @@ -51,4 +51,37 @@ time.sleep(2) # # 设置等待时间，确保机械臂已经到达指定位置 # time.sleep(1.5) -# num \ No newline at end of file + +# num +mc.set_gripper_mode(0) +# print('11') +# time.sleep(1) +# mc.set_gripper_value(50, 50) +# time.sleep(1) +# print(mc.get_angles()) + +mc.set_gripper_state(0,100) +time.sleep(0.05) + +angles = [[-27.64, 15.2, -53.34, -52.47, 88.76, 99.05]] +coords = [189.7, -350.6, 334.0, -170.85, -0.86, -91.24] +mc.set_color(0, 255, 0) +for i in range(4): + coords[1] += 150 + # mc.send_coords(coords, 100, 1) + time.sleep(2) + mc.set_gripper_state(0,100) + time.sleep(0.05) + coords[2] -= 155 + # mc.send_coords(coords,100,1) + print(mc.get_angles()) + time.sleep(3) + mc.set_gripper_state(1,100) + time.sleep(1) + coords[2] += 155 + # mc.send_coords(coords,100,1) + time.sleep(2) + coords[2] -= 155 + # mc.send_coords(coords,100,1) + time.sleep(2) + mc.set_gripper_state(0,100) \ No newline at end of file